Include speed in Reeds and Shepp callback

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/vehicle_platform/reeds_shepp.cpp b/vehicle_platform/reeds_shepp.cpp
index 575d428e01574601f6dcaea32f4f523753c1f9f6..0f7d755b7565040386cabccd395c0663ffa7196a 100644 (file)
--- a/vehicle_platform/reeds_shepp.cpp
+++ b/vehicle_platform/reeds_shepp.cpp
@@ -563,23 +563,24 @@ void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size,
     double dist = rho_ * path.length(); // TODO shorten path here
 
     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;
     if (seg > path.length()) seg = path.length();
 
-    double phi, v;
+    double phi, v = 0;
 
     s[0] = s[1] = 0.0;
     s[2] = q0[2];
+    s[3] = 0;
 
     for (unsigned int i=0; i<5 && seg>0; ++i)
     {
@@ -617,4 +618,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/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 8986250a9f098be550b5d801bb82642470d80dd8..40bb848e38c2f6f6d64382db0bdfd2f9a0c534e0 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -87,10 +87,11 @@ std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
         return nodes;
 }
 
-int cb_st3(double q[3], void *user_data)
+int cb_st3(double q[4], void *user_data)
 {
         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
+        nodes->back()->s(q[3]);
         return 0;
 }