Add 5th steering procedure to RRTBase

[hubacji1/iamcar.git] / vehicle_platform / steer.cc

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 5f4114ce29730e595e54dd4780d0dbaddfe94fbf..420797113b4985eb0c6c2a22d729e77c8a8a4006 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -15,8 +15,10 @@ You should have received a copy of the GNU General Public License
 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"
 
@@ -35,6 +37,9 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define ST4KI 0
 #define ST4KD 0
 
+#define ST5TURNINGRADIUS 10.82
+#define ST5STEP 0.2
+
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
 {
         float angl;
@@ -150,3 +155,36 @@ std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
         }
         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)
+{
+        return st5(init, goal, ST5STEP, false);
+}
+
+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;
+        double q0[] = {init->x(), init->y(), init->h()};
+        double q1[] = {goal->x(), goal->y(), goal->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;
+}