Update steer procedure with step variable

diff --git a/CHANGELOG.md b/CHANGELOG.md
index c36f2466b79bb2a828d5cbb4219a904605f5b875..7bfbf3481ad30c1e524734a73d110cc0b9aea0ad 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Bidirectional T3 planner based on two T2 planners.
 - RRT\*-Connect algorithm based on [Klemm2015] paper.
 - Optimization procedure based on *Remove Redundant Points* from [Lan2015].
+- Variable steer step size to `st3` procedure.
 
 ### Changed
 - Make `sample` procedure part of RRTBase.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index e10610b9247547d863ec45354c4570555ab354eb..793722929f526399879760bfc742c59dfdb03ae0 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -831,3 +831,8 @@ std::vector<RRTNode *> RRTBase::steer(RRTNode *init, RRTNode *goal)
 {
         return st3(init, goal);
 }
+
+std::vector<RRTNode *> RRTBase::steer(RRTNode *init, RRTNode *goal, float step)
+{
+        return st3(init, goal, step);
+}

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index aa366cb3b87d8ff0f4fa81dfe74bbb101cfa3186..d30a016c34fcbcee65c2609167f27a12dc8a2cd8 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -115,6 +115,10 @@ class RRTBase {
                 RRTNode *nn(RRTNode *rs);
                 std::vector<RRTNode *> nv(RRTNode *node, float dist);
                 std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
+                std::vector<RRTNode *> steer(
+                                RRTNode *init,
+                                RRTNode *goal,
+                                float step);
 
                 // virtuals - implemented by child classes
                 virtual bool next() = 0;

diff --git a/incl/steer.h b/incl/steer.h
index 3ee9adb66541d377c30ff55f70ac383c5ca0770c..291ceb52e421874f772b529b683abd5a6c41cf56 100644 (file)
--- a/incl/steer.h
+++ b/incl/steer.h
@@ -24,6 +24,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal);
 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal);
 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step);
 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal);
 
 #endif

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 40bb848e38c2f6f6d64382db0bdfd2f9a0c534e0..5f4114ce29730e595e54dd4780d0dbaddfe94fbf 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -96,12 +96,17 @@ int cb_st3(double q[4], void *user_data)
 }
 
 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
+{
+        return st3(init, goal, ST3STEP);
+}
+
+std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step)
 {
         std::vector<RRTNode *> nodes;
         double q0[] = {init->x(), init->y(), init->h()};
         double q1[] = {goal->x(), goal->y(), goal->h()};
         ReedsSheppStateSpace rsss(ST3TURNINGRADIUS);
-        rsss.sample(q0, q1, ST3STEP, cb_st3, &nodes);
+        rsss.sample(q0, q1, step, cb_st3, &nodes);
         return nodes;
 }