Add 5th steering procedure to RRTBase

`st5` uses Dubin's paths.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9a102c96dae7938d01dac5ae544bb67aeebe22da..9c2263ceadec0b204d3d1ebf62527e1cdba85e0d 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,6 +41,7 @@ add_executable(go_car_go
 
         vehicle_platform/bcar.cc
         vehicle_platform/cost.cc
+        vehicle_platform/dubins.cc
         vehicle_platform/reeds_shepp.cpp
         vehicle_platform/steer.cc
 )

diff --git a/README.md b/README.md
index 67255b3a399c90c56afe5fe54b2c4d88e04c64e8..69fa0b053327330def4263d4ed95e63a6369a0a9 100644 (file)
--- a/README.md
+++ b/README.md
@@ -67,6 +67,7 @@ Implemented Steering procedures:
 - `st2` - Steer with maximum turning radius and direction in mind.
 - `st3` - Reeds and Shepp steer procedure.
 - `st4` - Very basic closed-loop simulator.
+- `st5` - Dubins paths.
 
 To disable *OpenMP*, add `-DCMAKE_DISABLE_FIND_PACKAGE_OpenMP=TRUE` to `cmake`
 command or to `build.sh` script.

diff --git a/incl/steer.h b/incl/steer.h
index 291ceb52e421874f772b529b683abd5a6c41cf56..2abe4c36ea891f988f13ff6d2ee494fc1d8dac1e 100644 (file)
--- a/incl/steer.h
+++ b/incl/steer.h
@@ -26,5 +26,8 @@ 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);
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw);
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw);
 
 #endif

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 5f4114ce29730e595e54dd4780d0dbaddfe94fbf..420797113b4985eb0c6c2a22d729e77c8a8a4006 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -15,8 +15,10 @@ You should have received a copy of the GNU General Public License
 along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include <algorithm>
 #include <cmath>
 #include "bcar.h"
+#include "dubins.h"
 #include "reeds_shepp.h"
 #include "steer.h"
 
@@ -35,6 +37,9 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define ST4KI 0
 #define ST4KD 0
 
+#define ST5TURNINGRADIUS 10.82
+#define ST5STEP 0.2
+
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
 {
         float angl;
@@ -150,3 +155,36 @@ std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
         }
         return nodes;
 }
+
+int cb_st5(double q[3], double t, void *user_data)
+{
+        std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
+        nodes->push_back(new RRTNode(q[0], q[1], q[2]));
+        return 0;
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal)
+{
+        return st5(init, goal, ST5STEP, false);
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw)
+{
+        return st5(init, goal, ST5STEP, bw);
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw)
+{
+        std::vector<RRTNode *> nodes;
+        double q0[] = {init->x(), init->y(), init->h()};
+        double q1[] = {goal->x(), goal->y(), goal->h()};
+        DubinsPath path;
+        if (bw)
+                dubins_shortest_path(&path, q1, q0, ST5TURNINGRADIUS);
+        else
+                dubins_shortest_path(&path, q0, q1, ST5TURNINGRADIUS);
+        dubins_path_sample_many(&path, step, cb_st5, &nodes);
+        if (bw)
+                std::reverse(nodes.begin(), nodes.end());
+        return nodes;
+}