Add speed from Reed-Sepp steer

diff --git a/incl/reeds_shepp.h b/incl/reeds_shepp.h
index a038a2a9604a7254dafab892e414269094a17570..add610a49419d202bc58edd03ba1d5711df5e36b 100644 (file)
--- 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 95628a19c96555bfd193eca570b05af556e26289..6e80900923bddc9d0e293b57a6bef71a323f1a80 100644 (file)
--- 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 9b4fcb930a079e6340d2166bc5d6d27938ed10a4..c089b85fb52dbf95cc365f176901e95ca01ebca8 100644 (file)
--- 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;
 }