Add extension with dubins paths

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d1d68b4d7fb88aaca9e9c1d2fdbcc714c0903c83..1cc1f965545b730a06431c3767062dbee408bdc0 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,6 +28,7 @@ link_libraries(jsoncpp_lib)
 
 add_library(rrts STATIC
        src/rrts.cc
+       src/rrtext19.cc
        src/rrtext18.cc
        src/rrtext17.cc
        src/rrtext16.cc
@@ -39,5 +40,6 @@ add_library(rrts STATIC
        src/rrtext6.cc
        src/rrtext2.cc
        src/reeds_shepp.cpp
+       src/dubins.c
 )
 target_include_directories(rrts PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/incl)

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 4f20a7937897435e698b6b7a69e2d0753e6dcd2a..60eace15e6e4c33e30422a7fc080e5a189eab7b9 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -44,6 +44,8 @@ namespace rrts {
  * \see https://github.com/AndrewWalker/Dubins-Curves
  */
 class RRTExt19 : public virtual RRTS {
+private:
+       void steer(RRTNode const &f, RRTNode const &t);
 };
 
 /*! \brief Finish when more than 1000 iterations.

diff --git a/incl/rrtsp.hh b/incl/rrtsp.hh
index 255da02f336c163651589681ae804a8669b1ecb7..728ebf9770fb2b3c487737e6e85f828699505452 100644 (file)
--- a/incl/rrtsp.hh
+++ b/incl/rrtsp.hh
@@ -21,6 +21,35 @@
 
 namespace rrts {
 
+/*! \brief Planner for F1/10.
+ *
+ * TODO: change RRTExt2 to cost grid
+ *
+ * \ingroup planners
+ */
+class P40 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
+               public RRTExt15, public RRTExt19, public RRTExt17,
+               public RRTExt13 {
+public:
+       Json::Value json() const
+       {
+               auto jvo = RRTExt13::json();
+               auto json15 = RRTExt15::json();
+               jvo["log_path_cost"] = json15["log_path_cost"];
+               return jvo;
+       }
+       void json(Json::Value jvi)
+       {
+               RRTExt2::json(jvi);
+       }
+       void reset()
+       {
+               RRTExt8::reset();
+               RRTExt14::reset();
+               RRTExt13::reset();
+       }
+};
+
 /*! \brief Planner with optimization and reset.
  *
  * \ingroup planners

diff --git a/src/rrtext19.cc b/src/rrtext19.cc
new file mode 100644 (file)
index 0000000..60caa57
--- /dev/null
+++ b/src/rrtext19.cc
@@ -0,0 +1,41 @@
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+#include "dubins.h"
+
+namespace rrts {
+
+static int
+cb_steer(double q[3], double t, void *w)
+{
+       std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
+       st->push_back(RRTNode());
+       st->back().x(q[0]);
+       st->back().y(q[1]);
+       st->back().h(q[2]);
+       st->back().sp(1.0);
+       return 0;
+}
+
+void
+RRTExt19::steer(RRTNode const &f, RRTNode const &t)
+{
+       this->steered_.clear();
+       double q0[] = {f.x(), f.y(), f.h()};
+       double q1[] = {t.x(), t.y(), t.h()};
+       DubinsPath path;
+       int r = dubins_shortest_path(&path, q0, q1, this->bc_.mtr());
+       if (r != 0) {
+               return;
+       }
+       dubins_path_sample_many(&path, this->eta_, cb_steer, &this->steered_);
+       if (this->steered_.size() > 0) {
+               this->steered_.front().sp(0.0);
+       }
+}
+
+} /* namespace rrts */