Include speed in Reeds and Shepp callback

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

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;
 }