From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 17 Mar 2023 14:24:04 +0000 (+0100)
Subject: Force R&S to always include last point of segment
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/40caf1bccefbc41c697f4dfc2249d17361659828

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 9bb15fc..7cf9b01 100644
--- 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);