- Forward perpendicular parking methods for `SlotPlanner`.
- Angle forward/backward parking.
- Multiple `firsts` points tested before random sampling.
+- Dubins path steering procedure.
### Changed
- Parking slot in scenario starts with the point closest to the init pose.
vehicle_platform/bcar.cc
vehicle_platform/cost.cc
+ vehicle_platform/dubins.cc
vehicle_platform/reeds_shepp.cpp
vehicle_platform/steer.cc
)
- `st2` - Steer with maximum turning radius and direction in mind.
- `st3` - Reeds and Shepp steer procedure.
- `st4` - Very basic closed-loop simulator.
+- `st5` - Dubins paths.
To disable *OpenMP*, add `-DCMAKE_DISABLE_FIND_PACKAGE_OpenMP=TRUE` to `cmake`
command or to `build.sh` script.
p.tlog(p.findt());
}
}
- if (ps.slotType() == PARALLEL)
+ if (p.goal_found() && ps.slotType() == PARALLEL)
p.tlog(p.findt(p.slot_cusp().back()));
#elif defined USE_PTHREAD
bool gf = false;
glVertex2f(GLVERTEX(this->samples_.back()));
glEnd();
}
- // Plot nodes
+ // Plot nodes (position, orientation
std::vector<RRTNode *> s; // DFS stack
std::vector<RRTNode *> r; // reset visited_
RRTNode *tmp;
glBegin(GL_LINES);
s.push_back(this->root_);
+ while (s.size() > 0) {
+ tmp = s.back();
+ s.pop_back();
+ if (!tmp->visit()) {
+ r.push_back(tmp);
+ for (auto ch: tmp->children()) {
+ s.push_back(ch);
+ glColor3f(0.5, 0.5, 0.5);
+ BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
+ glVertex2f(
+ bc.lfx() * GLPLWSCALE,
+ bc.lfy() * GLPLHSCALE
+ );
+ glVertex2f(
+ bc.lrx() * GLPLWSCALE,
+ bc.lry() * GLPLHSCALE
+ );
+ glVertex2f(
+ bc.lrx() * GLPLWSCALE,
+ bc.lry() * GLPLHSCALE
+ );
+ glVertex2f(
+ bc.rrx() * GLPLWSCALE,
+ bc.rry() * GLPLHSCALE
+ );
+ glVertex2f(
+ bc.rrx() * GLPLWSCALE,
+ bc.rry() * GLPLHSCALE
+ );
+ glVertex2f(
+ bc.rfx() * GLPLWSCALE,
+ bc.rfy() * GLPLHSCALE
+ );
+ }
+ }
+ }
+ glEnd();
+ // Plot nodes
+ glBegin(GL_LINES);
+ s.push_back(this->root_);
while (s.size() > 0) {
tmp = s.back();
s.pop_back();
ch_cost = dnodes[tmp.ni].c +
this->cost(cusps[tmp.ni], cusps[i]);
steered = this->steer(cusps[tmp.ni], cusps[i]);
+ if (steered.size() <= 0)
+ break;
for (unsigned int j = 0; j < steered.size() - 1; j++)
steered[j]->add_child(steered[j + 1], 1);
if (this->collide(
return false;
std::vector<RRTNode *> cusps;
for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
- if (tmp_cusps[i] != tmp_cusps[(i + 1) % tmp_cusps.size()])
+ if (!IS_NEAR(
+ tmp_cusps[i],
+ tmp_cusps[(i + 1) % tmp_cusps.size()]
+ ))
cusps.push_back(tmp_cusps[i]);
}
std::reverse(cusps.begin(), cusps.end());
return false;
}
+bool RRTNode::inFront(RRTNode *n)
+{
+ float nx = cos(this->h());
+ float ny = sin(this->h());
+ float dx = n->x() - this->x();
+ float dy = n->y() - this->y();
+ float dot = nx * dx + ny * dy;
+ if (dot > 0)
+ return true;
+ return false;
+}
+
RRTEdge::RRTEdge():
init_(new RRTNode()),
goal_(new RRTNode())
}
}
}
- en_add = true;
- cusps = 0;
- for (auto ns: this->steer(pn, rs, 0.01)) {
- ns->rs(rs);
- if (!en_add) {
- delete ns;
- } else if (IS_NEAR(pn, ns)) {
- delete ns;
- } else {
- if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
- cusps++;
- if (cusps > 4)
- en_add = false;
- nvs = this->nv(
- ns,
- MIN(
- GAMMA_RRTSTAR(
- this->nodes().size()),
- 0.2)); // TODO const
- this->nodes().push_back(ns);
- this->add_iy(ns);
- // connect
- if (!this->connect(pn, ns, nvs)) {
- this->iy_[IYI(ns->y())].pop_back();
- this->nodes().pop_back();
- delete ns;
- en_add = false;
- } else {
- this->add_ixy(ns);
- // rewire
- this->rewire(nvs, ns);
- pn = ns;
- newly_added.push_back(pn);
- if (this->goal_found(pn, CO)) {
- this->goal_cost();
- this->tlog(this->findt());
- en_add = false;
- }
- }
- }
- }
if (this->samples().size() <= 1)
return this->goal_found();
// steer to goal
}
}
}
- en_add = true;
- cusps = 0;
- for (auto ns: this->steer(pn, this->goal(), 0.01)) {
- ns->rs(rs);
- if (!en_add) {
- delete ns;
- } else if (IS_NEAR(pn, ns)) {
- delete ns;
- } else {
- if (sgn(pn->s()) != sgn(ns->s()))
- cusps++;
- if (cusps > 4)
- en_add = false;
- this->nodes().push_back(ns);
- this->add_iy(ns);
- pn->add_child(ns, this->cost(pn, ns));
- if (this->collide(pn, ns)) {
- pn->children().pop_back();
- ns->remove_parent();
- this->iy_[IYI(ns->y())].pop_back();
- this->nodes().pop_back();
- delete ns;
- en_add = false;
- } else {
- this->add_ixy(ns);
- this->ocost(ns);
- pn = ns;
- if (this->goal_found(pn, CO)) {
- this->goal_cost();
- this->tlog(this->findt());
- en_add = false;
- }
- }
- }
- }
}
// steer to goals
for (auto na: newly_added) {
print("{}: {}".format(a["f"], v[a["f"]]))
if __name__ == "__main__":
- plt.rcParams["font.size"] = 29
- res = []
- LOGF = "log-slotplanner"
- for d in listdir(LOGF):
- r = {}
- for sf in [i["f"] for i in LOG]:
- r[sf] = load_trajectories("{}/{}/{}".format(LOGF, d, sf))
- res.append({
- "f": d,
- "elap": get_val_if_exist(r["T2"], "elap"),
- "rrte": get_val_if_exist(r["T2"], "rrte"),
- "ppse": get_val_if_exist(r["T2"], "ppse"),
- "succ": (
- count_if_exist(r["T2"], "traj") /
- count_if_exist(r["T2"], "elap")
- ),
- })
- res2 = []
- LOGF = "log-rrt"
- for d in listdir(LOGF):
- r = {}
- for sf in [i["f"] for i in LOG]:
- r[sf] = load_trajectories("{}/{}/{}".format(LOGF, d, sf))
- res2.append({
- "f": d,
- "elap": get_val_if_exist(r["T2"], "elap"),
- "rrte": get_val_if_exist(r["T2"], "rrte"),
- "ppse": get_val_if_exist(r["T2"], "ppse"),
- "succ": (
- count_if_exist(r["T2"], "traj") /
- count_if_exist(r["T2"], "elap")
- ),
- })
-
- fig = plt.figure()
- # For color scheme
- # see https://github.com/vega/vega/wiki/Scales#scale-range-literals
- ax = fig.add_subplot(111)
- ax.set_title("""Elapsed time for different lengths
- of parallel parking slot""")
-
- ax.set_ylabel("Time [s]")
- ax.set_xlabel("Parking slot length [m]")
-
- # res Slot Planner
- coord = [float(r["f"].split("_")[1]) for r in res]
-
- #val = [sum(r["ppse"])/len(r["ppse"]) for r in res]
- #fin = [(x, y) for (x, y) in zip(coord, val)]
- #fin.sort()
- #plt.plot(
- # [x for (x, y) in fin],
- # [y for (x, y) in fin],
- # color = "g",
- # label = "Slot Planner",
- #)
-
- val = [max(r["elap"]) for r in res]
- fin = [(x, y) for (x, y) in zip(coord, val)]
- fin.sort()
- plt.plot(
- [x for (x, y) in fin],
- [y for (x, y) in fin],
- color = "#e6550d",
- linestyle = "--",
- label = "Elapsed worst",
- )
-
- #val = [sum(r["rrte"])/len(r["rrte"]) for r in res]
- #fin = [(x, y) for (x, y) in zip(coord, val)]
- #fin.sort()
- #plt.plot(
- # [x for (x, y) in fin],
- # [y for (x, y) in fin],
- # color = "#fd8d3c",
- # label = "RRT",
- #)
-
- val = [sum(r["elap"])/len(r["elap"]) for r in res]
- fin = [(x, y) for (x, y) in zip(coord, val)]
- fin.sort()
- plt.plot(
- [x for (x, y) in fin],
- [y for (x, y) in fin],
- color = "#e6550d",
- label = "Elapsed average",
- )
-
- val = [r["succ"] for r in res]
- fin = [(x, y) for (x, y) in zip(coord, val)]
- fin.sort()
- plt.plot(
- [x for (x, y) in fin],
- [y for (x, y) in fin],
- #color = "#fd8d3c",
- color = "#fdae6b",
- label = "Success rate",
- )
-
- # res2 RRT
- coord = [float(r["f"].split("_")[1]) for r in res2]
-
- val = [max(r["elap"]) for r in res2]
- fin = [(x, y) for (x, y) in zip(coord, val)]
- fin.sort()
- plt.plot(
- [x for (x, y) in fin],
- [y for (x, y) in fin],
- color = "#3182bd",
- linestyle = "--",
- label = "Elapsed worst",
- )
-
- val = [sum(r["elap"])/len(r["elap"]) for r in res2]
- fin = [(x, y) for (x, y) in zip(coord, val)]
- fin.sort()
- plt.plot(
- [x for (x, y) in fin],
- [y for (x, y) in fin],
- color = "#3182bd",
- label = "Elapsed average",
- )
-
- val = [r["succ"] for r in res2]
- fin = [(x, y) for (x, y) in zip(coord, val)]
- fin.sort()
- plt.plot(
- [x for (x, y) in fin],
- [y for (x, y) in fin],
- #color = "#6baed6",
- color = "#9ecae1",
- label = "Success rate",
- )
-
- plt.legend(bbox_to_anchor=(1, 1), loc=1, borderaxespad=0)
- plt.show()
+ r = {}
+ for sf in [i["f"] for i in LOG]:
+ r[sf] = load_trajectories("{}/{}".format(LOGF, sf))
+ print_successrate()
+ plot_maxtime()
+ plot_costdist()
float alfa();
/** Return true if `n` not inside of `ccl`, `ccr` */
bool drivable(RRTNode *n);
+ /** Return ``RRTNode`` where ``BicycleCar`` may steer to.
+
+ When the node ``n`` is not drivable, use guess.
+
+ @param n The node to drive to.
+
+ */
+ RRTNode *drivableNode(RRTNode *n);
/** Return node rotated by `dh` around `c` */
BicycleCar *move(RRTNode *c, float dh);
};
--- /dev/null
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef DUBINS_H
+#define DUBINS_H
+
+typedef enum
+{
+ LSL = 0,
+ LSR = 1,
+ RSL = 2,
+ RSR = 3,
+ RLR = 4,
+ LRL = 5
+} DubinsPathType;
+
+typedef struct
+{
+ /* the initial configuration */
+ double qi[3];
+ /* the lengths of the three segments */
+ double param[3];
+ /* model forward velocity / model angular velocity */
+ double rho;
+ /* the path type described */
+ DubinsPathType type;
+} DubinsPath;
+
+#define EDUBOK (0) /* No error */
+#define EDUBCOCONFIGS (1) /* Colocated configurations */
+#define EDUBPARAM (2) /* Path parameterisitation error */
+#define EDUBBADRHO (3) /* the rho value is invalid */
+#define EDUBNOPATH (4) /* no connection between configurations with this word */
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param path - the resultant path
+ * @param q0 - a configuration specified as an array of x, y, theta
+ * @param q1 - a configuration specified as an array of x, y, theta
+ * @param rho - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @return - non-zero on error
+ */
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
+
+/**
+ * Generate a path with a specified word from an initial configuration to
+ * a target configuration, with a specified turning radius
+ *
+ * @param path - the resultant path
+ * @param q0 - a configuration specified as an array of x, y, theta
+ * @param q1 - a configuration specified as an array of x, y, theta
+ * @param rho - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param pathType - the specific path type to use
+ * @return - non-zero on error
+ */
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+double dubins_path_length(DubinsPath* path);
+
+/**
+ * Return the length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length(DubinsPath* path, int i);
+
+/**
+ * Return the normalized length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length_normalized( DubinsPath* path, int i );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path - an initialised path
+ * @return - one of LSL, LSR, RSL, RSR, RLR or LRL
+ */
+DubinsPathType dubins_path_type(DubinsPath* path);
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q - the configuration result
+ * @returns - non-zero if 't' is not in the correct range
+ */
+int dubins_path_sample(DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
+ *
+ * @param path - the path to sample
+ * @param stepSize - the distance along the path for subsequent samples
+ * @param cb - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ *
+ * @returns - zero on successful completion, or the result of the callback
+ */
+int dubins_path_sample_many(DubinsPath* path,
+ double stepSize,
+ DubinsPathSamplingCallback cb,
+ void* user_data);
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q - the configuration result
+ */
+int dubins_path_endpoint(DubinsPath* path, double q[3]);
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path - an initialised path
+ * @param t - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
+
+
+#endif /* DUBINS_H */
+
static bool comp_ccost(RRTNode *n1, RRTNode *n2);
float update_ccost();
bool visit();
+ /** Return ``true`` if ``n`` is in front of ``this``.
+ *
+ * @param n The node that is beeing checked.
+ */
+ bool inFront(RRTNode *n);
};
class RRTEdge {
std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal);
std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step);
std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw);
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw);
#endif
bool CircleObstacle::collide(RRTEdge *e)
{
- std::vector<RRTEdge *> edges;
- edges.push_back(e);
- return this->collide(edges);
+ // see http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
+ // Find the closest point on seq.
+ float seg_v[] = {
+ e->goal()->x() - e->init()->x(),
+ e->goal()->y() - e->init()->y()
+ };
+ float pt_v[] = {
+ this->x() - e->init()->x(),
+ this->y() - e->init()->y()
+ };
+ float seg_vl = sqrt(pow(seg_v[0], 2) + pow(seg_v[1], 2));
+ // seg_vl must be > 0 otherwise it is invalid segment length.
+ if (seg_vl <= 0)
+ return false;
+ float seg_v_unit[] = {seg_v[0] / seg_vl, seg_v[1] / seg_vl};
+ float proj = pt_v[0]*seg_v_unit[0] + pt_v[1]*seg_v_unit[1];
+ float closest[] = {0, 0};
+ if (proj <= 0) {
+ closest[0] = e->init()->x();
+ closest[1] = e->init()->y();
+ } else if (proj >= seg_vl) {
+ closest[0] = e->goal()->x();
+ closest[1] = e->goal()->y();
+ } else {
+ float proj_v[] = {seg_v_unit[0] * proj, seg_v_unit[1] * proj};
+ closest[0] = proj_v[0] + e->init()->x();
+ closest[1] = proj_v[1] + e->init()->y();
+ }
+ // Find the segment circle.
+ float dist_v[] = {this->x() - closest[0], this->y() - closest[1]};
+ float dist = sqrt(pow(dist_v[0], 2) + pow(dist_v[1], 2));
+ if (dist <= this->r())
+ return true;
+ return false;
+ // Offset computation.
+ // float offset[] = {
+ // dist_v[0] / dist * (BCAR_TURNING_RADIUS - dist),
+ // dist_v[1] / dist * (BCAR_TURNING_RADIUS - dist)
+ // };
}
bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
{
- std::vector<RRTEdge *> bedges;
- float radi = this->r() / cos(M_PI / 4); // TODO 4 is square
- float angl = 2 * M_PI / 4;
- float x1;
- float y1;
- float x2;
- float y2;
- int i;
- for (i = 0; i < 4; i++) {
- x1 = radi * cos(i * angl);
- y1 = radi * sin(i * angl);
- x2 = radi * cos((i + 1) * angl);
- y2 = radi * sin((i + 1) * angl);
- x1 += this->x();
- y1 += this->y();
- x2 += this->x();
- y2 += this->y();
- bedges.push_back(new RRTEdge(
- new RRTNode(x1, y1, 0),
- new RRTNode(x2, y2, 0)));
- }
- for (auto &be: bedges) {
- for (auto &e: edges) {
- if (SegmentObstacle(
- be->init(),
- be->goal())
- .collide(e)) {
- for (auto e: bedges) {
- delete e->init();
- delete e->goal();
- delete e;
- }
- return true;
- }
- }
- }
- for (auto e: bedges) {
- delete e->init();
- delete e->goal();
- delete e;
- }
+ for (auto e: edges)
+ if (this->collide(e))
+ return true;
return false;
}
RRTNode *ccr = this->ccr();
if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
pow(this->turning_radius_, 2))
- drivable = false;
+ return false;
if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
pow(this->turning_radius_, 2))
- drivable = false;
- return drivable;
+ return false;
+ return true;
+}
+
+RRTNode *BicycleCar::drivableNode(RRTNode *n)
+{
+ bool drivable = true;
+ RRTNode *ccl = this->ccl();
+ if (
+ pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2)
+ <= pow(this->turning_radius_, 2)
+ ) {
+ if (this->inFront(n))
+ return this->move(ccl, M_PI);
+ else
+ return this->move(ccl, -M_PI);
+ }
+ RRTNode *ccr = this->ccr();
+ if (
+ pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2)
+ <= pow(this->turning_radius_, 2)
+ ) {
+ if (this->inFront(n))
+ return this->move(ccr, -M_PI);
+ else
+ return this->move(ccr, M_PI);
+ }
+ return n;
}
BicycleCar *BicycleCar::move(RRTNode *c, float dh)
--- /dev/null
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifdef WIN32
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include "dubins.h"
+
+#define EPSILON (10e-10)
+
+typedef enum
+{
+ L_SEG = 0,
+ S_SEG = 1,
+ R_SEG = 2
+} SegmentType;
+
+/* The segment types for each of the Path types */
+const SegmentType DIRDATA[][3] = {
+ { L_SEG, S_SEG, L_SEG },
+ { L_SEG, S_SEG, R_SEG },
+ { R_SEG, S_SEG, L_SEG },
+ { R_SEG, S_SEG, R_SEG },
+ { R_SEG, L_SEG, R_SEG },
+ { L_SEG, R_SEG, L_SEG }
+};
+
+typedef struct
+{
+ double alpha;
+ double beta;
+ double d;
+ double sa;
+ double sb;
+ double ca;
+ double cb;
+ double c_ab;
+ double d_sq;
+} DubinsIntermediateResults;
+
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
+
+/**
+ * Floating point modulus suitable for rings
+ *
+ * fmod doesn't behave correctly for angular quantities, this function does
+ */
+double fmodr( double x, double y)
+{
+ return x - y*floor(x/y);
+}
+
+double mod2pi( double theta )
+{
+ return fmodr( theta, 2 * M_PI );
+}
+
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
+{
+ int i, errcode;
+ DubinsIntermediateResults in;
+ double params[3];
+ double cost;
+ double best_cost = INFINITY;
+ int best_word = -1;
+ errcode = dubins_intermediate_results(&in, q0, q1, rho);
+ if(errcode != EDUBOK) {
+ return errcode;
+ }
+
+
+ path->qi[0] = q0[0];
+ path->qi[1] = q0[1];
+ path->qi[2] = q0[2];
+ path->rho = rho;
+
+ for( i = 0; i < 6; i++ ) {
+ DubinsPathType pathType = (DubinsPathType)i;
+ errcode = dubins_word(&in, pathType, params);
+ if(errcode == EDUBOK) {
+ cost = params[0] + params[1] + params[2];
+ if(cost < best_cost) {
+ best_word = i;
+ best_cost = cost;
+ path->param[0] = params[0];
+ path->param[1] = params[1];
+ path->param[2] = params[2];
+ path->type = pathType;
+ }
+ }
+ }
+ if(best_word == -1) {
+ return EDUBNOPATH;
+ }
+ return EDUBOK;
+}
+
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
+{
+ int errcode;
+ DubinsIntermediateResults in;
+ errcode = dubins_intermediate_results(&in, q0, q1, rho);
+ if(errcode == EDUBOK) {
+ double params[3];
+ errcode = dubins_word(&in, pathType, params);
+ if(errcode == EDUBOK) {
+ path->param[0] = params[0];
+ path->param[1] = params[1];
+ path->param[2] = params[2];
+ path->qi[0] = q0[0];
+ path->qi[1] = q0[1];
+ path->qi[2] = q0[2];
+ path->rho = rho;
+ path->type = pathType;
+ }
+ }
+ return errcode;
+}
+
+double dubins_path_length( DubinsPath* path )
+{
+ double length = 0.;
+ length += path->param[0];
+ length += path->param[1];
+ length += path->param[2];
+ length = length * path->rho;
+ return length;
+}
+
+double dubins_segment_length( DubinsPath* path, int i )
+{
+ if( (i < 0) || (i > 2) )
+ {
+ return INFINITY;
+ }
+ return path->param[i] * path->rho;
+}
+
+double dubins_segment_length_normalized( DubinsPath* path, int i )
+{
+ if( (i < 0) || (i > 2) )
+ {
+ return INFINITY;
+ }
+ return path->param[i];
+}
+
+DubinsPathType dubins_path_type( DubinsPath* path )
+{
+ return path->type;
+}
+
+void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
+{
+ double st = sin(qi[2]);
+ double ct = cos(qi[2]);
+ if( type == L_SEG ) {
+ qt[0] = +sin(qi[2]+t) - st;
+ qt[1] = -cos(qi[2]+t) + ct;
+ qt[2] = t;
+ }
+ else if( type == R_SEG ) {
+ qt[0] = -sin(qi[2]-t) + st;
+ qt[1] = +cos(qi[2]-t) - ct;
+ qt[2] = -t;
+ }
+ else if( type == S_SEG ) {
+ qt[0] = ct * t;
+ qt[1] = st * t;
+ qt[2] = 0.0;
+ }
+ qt[0] += qi[0];
+ qt[1] += qi[1];
+ qt[2] += qi[2];
+}
+
+int dubins_path_sample( DubinsPath* path, double t, double q[3] )
+{
+ /* tprime is the normalised variant of the parameter t */
+ double tprime = t / path->rho;
+ double qi[3]; /* The translated initial configuration */
+ double q1[3]; /* end-of segment 1 */
+ double q2[3]; /* end-of segment 2 */
+ const SegmentType* types = DIRDATA[path->type];
+ double p1, p2;
+
+ if( t < 0 || t > dubins_path_length(path) ) {
+ return EDUBPARAM;
+ }
+
+ /* initial configuration */
+ qi[0] = 0.0;
+ qi[1] = 0.0;
+ qi[2] = path->qi[2];
+
+ /* generate the target configuration */
+ p1 = path->param[0];
+ p2 = path->param[1];
+ dubins_segment( p1, qi, q1, types[0] );
+ dubins_segment( p2, q1, q2, types[1] );
+ if( tprime < p1 ) {
+ dubins_segment( tprime, qi, q, types[0] );
+ }
+ else if( tprime < (p1+p2) ) {
+ dubins_segment( tprime-p1, q1, q, types[1] );
+ }
+ else {
+ dubins_segment( tprime-p1-p2, q2, q, types[2] );
+ }
+
+ /* scale the target configuration, translate back to the original starting point */
+ q[0] = q[0] * path->rho + path->qi[0];
+ q[1] = q[1] * path->rho + path->qi[1];
+ q[2] = mod2pi(q[2]);
+
+ return EDUBOK;
+}
+
+int dubins_path_sample_many(DubinsPath* path, double stepSize,
+ DubinsPathSamplingCallback cb, void* user_data)
+{
+ int retcode;
+ double q[3];
+ double x = 0.0;
+ double length = dubins_path_length(path);
+ while( x < length ) {
+ dubins_path_sample( path, x, q );
+ retcode = cb(q, x, user_data);
+ if( retcode != 0 ) {
+ return retcode;
+ }
+ x += stepSize;
+ }
+ return 0;
+}
+
+int dubins_path_endpoint( DubinsPath* path, double q[3] )
+{
+ return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
+}
+
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
+{
+ /* calculate the true parameter */
+ double tprime = t / path->rho;
+
+ if((t < 0) || (t > dubins_path_length(path)))
+ {
+ return EDUBPARAM;
+ }
+
+ /* copy most of the data */
+ newpath->qi[0] = path->qi[0];
+ newpath->qi[1] = path->qi[1];
+ newpath->qi[2] = path->qi[2];
+ newpath->rho = path->rho;
+ newpath->type = path->type;
+
+ /* fix the parameters */
+ newpath->param[0] = fmin( path->param[0], tprime );
+ newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
+ newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
+ return 0;
+}
+
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
+{
+ double dx, dy, D, d, theta, alpha, beta;
+ if( rho <= 0.0 ) {
+ return EDUBBADRHO;
+ }
+
+ dx = q1[0] - q0[0];
+ dy = q1[1] - q0[1];
+ D = sqrt( dx * dx + dy * dy );
+ d = D / rho;
+ theta = 0;
+
+ /* test required to prevent domain errors if dx=0 and dy=0 */
+ if(d > 0) {
+ theta = mod2pi(atan2( dy, dx ));
+ }
+ alpha = mod2pi(q0[2] - theta);
+ beta = mod2pi(q1[2] - theta);
+
+ in->alpha = alpha;
+ in->beta = beta;
+ in->d = d;
+ in->sa = sin(alpha);
+ in->sb = sin(beta);
+ in->ca = cos(alpha);
+ in->cb = cos(beta);
+ in->c_ab = cos(alpha - beta);
+ in->d_sq = d * d;
+
+ return EDUBOK;
+}
+
+int dubins_LSL(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0, tmp1, p_sq;
+
+ tmp0 = in->d + in->sa - in->sb;
+ p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
+
+ if(p_sq >= 0) {
+ tmp1 = atan2( (in->cb - in->ca), tmp0 );
+ out[0] = mod2pi(tmp1 - in->alpha);
+ out[1] = sqrt(p_sq);
+ out[2] = mod2pi(in->beta - tmp1);
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+
+int dubins_RSR(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0 = in->d - in->sa + in->sb;
+ double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
+ if( p_sq >= 0 ) {
+ double tmp1 = atan2( (in->ca - in->cb), tmp0 );
+ out[0] = mod2pi(in->alpha - tmp1);
+ out[1] = sqrt(p_sq);
+ out[2] = mod2pi(tmp1 -in->beta);
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_LSR(DubinsIntermediateResults* in, double out[3])
+{
+ double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
+ if( p_sq >= 0 ) {
+ double p = sqrt(p_sq);
+ double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
+ out[0] = mod2pi(tmp0 - in->alpha);
+ out[1] = p;
+ out[2] = mod2pi(tmp0 - mod2pi(in->beta));
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_RSL(DubinsIntermediateResults* in, double out[3])
+{
+ double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
+ if( p_sq >= 0 ) {
+ double p = sqrt(p_sq);
+ double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
+ out[0] = mod2pi(in->alpha - tmp0);
+ out[1] = p;
+ out[2] = mod2pi(in->beta - tmp0);
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_RLR(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
+ double phi = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
+ if( fabs(tmp0) <= 1) {
+ double p = mod2pi((2*M_PI) - acos(tmp0) );
+ double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
+ out[0] = t;
+ out[1] = p;
+ out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_LRL(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
+ double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
+ if( fabs(tmp0) <= 1) {
+ double p = mod2pi( 2*M_PI - acos( tmp0) );
+ double t = mod2pi(-in->alpha - phi + p/2.);
+ out[0] = t;
+ out[1] = p;
+ out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
+{
+ int result;
+ switch(pathType)
+ {
+ case LSL:
+ result = dubins_LSL(in, out);
+ break;
+ case RSL:
+ result = dubins_RSL(in, out);
+ break;
+ case LSR:
+ result = dubins_LSR(in, out);
+ break;
+ case RSR:
+ result = dubins_RSR(in, out);
+ break;
+ case LRL:
+ result = dubins_LRL(in, out);
+ break;
+ case RLR:
+ result = dubins_RLR(in, out);
+ break;
+ default:
+ result = EDUBNOPATH;
+ }
+ return result;
+}
+
+
along with I am car. If not, see <http://www.gnu.org/licenses/>.
*/
+#include <algorithm>
#include <cmath>
#include "bcar.h"
+#include "dubins.h"
#include "reeds_shepp.h"
#include "steer.h"
#define ST4KI 0
#define ST4KD 0
+#define ST5TURNINGRADIUS 10.82
+#define ST5STEP 0.2
+
std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
{
float angl;
}
return nodes;
}
+
+int cb_st5(double q[3], double t, void *user_data)
+{
+ std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
+ nodes->push_back(new RRTNode(q[0], q[1], q[2]));
+ return 0;
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal)
+{
+ if (init->inFront(goal))
+ return st5(init, goal, ST5STEP, false);
+ return st5(init, goal, ST5STEP, true);
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw)
+{
+ return st5(init, goal, ST5STEP, bw);
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw)
+{
+ std::vector<RRTNode *> nodes;
+ BicycleCar *bc = new BicycleCar(init->x(), init->y(), init->h());
+ RRTNode *dg = bc->drivableNode(goal); // drivable goal
+ double q0[] = {init->x(), init->y(), init->h()};
+ double q1[] = {dg->x(), dg->y(), dg->h()};
+ DubinsPath path;
+ if (bw)
+ dubins_shortest_path(&path, q1, q0, ST5TURNINGRADIUS);
+ else
+ dubins_shortest_path(&path, q0, q1, ST5TURNINGRADIUS);
+ dubins_path_sample_many(&path, step, cb_st5, &nodes);
+ if (bw)
+ std::reverse(nodes.begin(), nodes.end());
+ return nodes;
+}
projects / hubacji1 / iamcar.git / commitdiff