Force R&S to always include last point of segment

diff --git a/rrts/src/reeds_shepp.cpp b/rrts/src/reeds_shepp.cpp
index 9bb15fcd4cadf7dd76091c4323fccf2043c5ec39..7cf9b01440359c46d4ff7ba52cc16a7e2c97230a 100644 (file)
--- a/rrts/src/reeds_shepp.cpp
+++ b/rrts/src/reeds_shepp.cpp
@@ -528,7 +528,27 @@ void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size,
     ReedsSheppPath path = reedsShepp(q0, q1);
     double dist = rho_ * path.length();
 
+    unsigned int i = 0;
+    while (i < 5
+            && (path.type_[i] == RS_NOP
+                || std::abs(path.length_[i]) < 0.1*step_size/rho_)) {
+        i++;
+    }
     for (double seg=0.0; seg<=dist; seg+=step_size){
+        double done = 0.0;
+        for (unsigned int j = 0; j < i; j++) {
+            done += std::abs(path.length_[j]);
+        }
+        // 1e-3 is used when asserting parent != this in RRTNode::p method.
+        if (i < 5 && seg/rho_ >= (done + std::abs(path.length_[i])) - 1e-3*rho_) {
+            seg = (done + std::abs(path.length_[i])) * rho_;
+            i++;
+            while (i < 5
+                    && (path.type_[i] == RS_NOP
+                        || std::abs(path.length_[i]) < 0.1*step_size/rho_)) {
+                i++;
+            }
+        }
         double qnew[5] = {};
         interpolate(q0, path, seg/rho_, qnew);
         cb( qnew, user_data);