Decide to steer forward/backward in st5

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 24fb4b40d6295359aa80a793fb6ee8fc91d3163e..a515386044d94aa3f320b145c8b0c5cca56c0893 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -109,6 +109,10 @@ class RRTNode {
                 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);
 };
 

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 420797113b4985eb0c6c2a22d729e77c8a8a4006..0954e136ed837e04c3aa7974197e8c3033a8295e 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -165,7 +165,9 @@ int cb_st5(double q[3], double t, void *user_data)
 
 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal)
 {
-        return st5(init, goal, ST5STEP, false);
+        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)