Add occupancy grid-based collision check

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1cc1f965545b730a06431c3767062dbee408bdc0..b05da5a6335d80794f430bc63f0609132701c688 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,6 +28,7 @@ link_libraries(jsoncpp_lib)
 
 add_library(rrts STATIC
        src/rrts.cc
+       src/rrtext20.cc
        src/rrtext19.cc
        src/rrtext18.cc
        src/rrtext17.cc

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 0c8cd06660ae2d4ecc9f058e1f0f3dd49182f996..87d39c4bc25d47f8915a96f906e60b60ec6164f2 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -38,6 +38,18 @@
 
 namespace rrts {
 
+/*! \brief Collision check based on occupancy grid.
+ *
+ * \ingroup ext-col
+ */
+class RRTExt20 : public virtual RRTS {
+private:
+       std::vector<bcar::Point> const *_points_to_check = nullptr;
+       bool collide_steered();
+public:
+       void set_points_to_check(std::vector<bcar::Point> const *p);
+};
+
 /*! \brief Use Dubins paths-based steering procedure.
  *
  * \ingroup ext-steer

diff --git a/src/rrtext20.cc b/src/rrtext20.cc
new file mode 100644 (file)
index 0000000..d82ad5f
--- /dev/null
+++ b/src/rrtext20.cc
@@ -0,0 +1,55 @@
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <cassert>
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt20::collide_steered()
+{
+       assert(this->_points_to_check != nullptr);
+       std::vector<bcar::Point> points_to_check(*this->_points_to_check);
+       unsigned int i = 0;
+       for (auto &n: this->steered_) {
+               for (auto &p: points_to_check) {
+                       Point const n_p(n);
+                       CarSize const n_cs(this->bc());
+                       if (p.inside_of(n_p, n_cs.edist_to_rr())) {
+                               break;
+                       }
+                       if (p.edist(n_p) < n_cs.edist_to_lf()
+                                       && p.edist(n_p) > n_cs.edist_to_rr()) {
+                               BicycleCar n_bc(this->bc());
+                               n_bc.x(n.x());
+                               n_bc.y(n.y());
+                               n_bc.h(n.h());
+                               std::vector<Point> poly;
+                               poly.push_back(n_bc.lf());
+                               poly.push_back(n_bc.lr());
+                               poly.push_back(n_bc.rr());
+                               poly.push_back(n_bc.rf());
+                               poly.push_back(n_bc.lf());
+                               if (p.inside_of(poly)) {
+                                       break;
+                               }
+
+                       }
+               }
+               i++;
+       }
+       this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+       return this->steered_.size() == 0;
+}
+
+void
+RRTExt20::set_points_to_check(std::vector<bcar::Point> const *p)
+{
+       this->_points_to_check = p;
+}
+
+} /* namespace rrts */