From 9c9847f67669c2aa2b092096a983433be8183b81 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 8 Jan 2019 14:01:19 +0100
Subject: [PATCH] Update steer procedure with step variable

---
 CHANGELOG.md              | 1 +
 base/rrtbase.cc           | 5 +++++
 incl/rrtbase.h            | 4 ++++
 incl/steer.h              | 1 +
 vehicle_platform/steer.cc | 7 ++++++-
 5 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index c36f246..7bfbf34 100644
--- 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 e10610b..7937229 100644
--- 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 aa366cb..d30a016 100644
--- 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 3ee9adb..291ceb5 100644
--- 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 40bb848..5f4114c 100644
--- 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;
 }
 
-- 
2.39.2