Add drivable node guess, use in st5

diff --git a/incl/bcar.h b/incl/bcar.h
index fabbfafcf3d4926d6908a723a16c25dfae8a3959..5f6ac31a5ef24408d03e6b5eca3a749b4afcd730 100644 (file)
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -111,6 +111,14 @@ class BicycleCar: public RRTNode {
                 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);
 };

diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index 9d87f2861904f306a362518e4033a5aec0610ce4..b9ae800e50e225db1bfbb8d7595e503f829c9a44 100644 (file)
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -287,6 +287,32 @@ bool BicycleCar::drivable(RRTNode *n)
         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)
 {
         float zx = c->x();

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 0954e136ed837e04c3aa7974197e8c3033a8295e..d89a06e68d2517ef25262b3c57e157e2d8e42137 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -178,8 +178,10 @@ std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool 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[] = {goal->x(), goal->y(), goal->h()};
+        double q1[] = {dg->x(), dg->y(), dg->h()};
         DubinsPath path;
         if (bw)
                 dubins_shortest_path(&path, q1, q0, ST5TURNINGRADIUS);