From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 7 Oct 2019 11:55:31 +0000 (+0200)
Subject: Add speed from Reed-Sepp steer
X-Git-Tag: v0.3.0~10^2~2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/0e115b49d76bf4a3b0a2b4e6b73ffe1ec7650d98

Add speed from Reed-Sepp steer
---

diff --git a/incl/reeds_shepp.h b/incl/reeds_shepp.h
index a038a2a..add610a 100644
--- a/incl/reeds_shepp.h
+++ b/incl/reeds_shepp.h
@@ -84,7 +84,7 @@ public:
     ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
 
 protected:
-    void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[3]);
+    void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[4]);
 
     /** \brief Turning radius */
     double rho_;
diff --git a/src/reeds_shepp.cpp b/src/reeds_shepp.cpp
index 95628a1..6e80900 100644
--- a/src/reeds_shepp.cpp
+++ b/src/reeds_shepp.cpp
@@ -563,14 +563,14 @@ void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size,
     double dist = rho_ * path.length();
 
     for (double seg=0.0; seg<=dist; seg+=step_size){
-        double qnew[3] = {};
+        double qnew[4] = {};
         interpolate(q0, path, seg/rho_, qnew);
         cb( qnew, user_data);
     }
     return;
 }
 
-void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3])
+void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
 {
 
     if (seg < 0.0) seg = 0.0;
@@ -617,4 +617,5 @@ void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, doubl
 
     s[0] = s[0] * rho_ + q0[0];
     s[1] = s[1] * rho_ + q0[1];
+    s[3] = v;
 }
diff --git a/src/rrts.cc b/src/rrts.cc
index 9b4fcb9..c089b85 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -140,13 +140,14 @@ std::vector<RRTNode *> RRTS::nv(RRTNode &t)
         return nv;
 }
 
-int cb_rs_steer(double q[3], void *user_data)
+int cb_rs_steer(double q[4], void *user_data)
 {
         std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
         nodes->push_back(RRTNode());
         nodes->back().x(q[0]);
         nodes->back().y(q[1]);
         nodes->back().h(q[2]);
+        nodes->back().sp(q[3]);
         return 0;
 }