Add simple occupancy grid collision check

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

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 87d39c4bc25d47f8915a96f906e60b60ec6164f2..e285fa2796c4d287067a96a6f26aa2da2c043520 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -38,6 +38,24 @@
 
 namespace rrts {
 
+/*! \brief Collision check based on enlarged occupancy grid.
+ *
+ */
+class RRTExt21 : public virtual RRTS {
+private:
+       unsigned int _grid_width = 0;
+       unsigned int _grid_height = 0;
+       float _grid_res = 0.0;
+       int8_t const *_grid_data = nullptr;
+       double _origin_x = 0.0;
+       double _origin_y = 0.0;
+       bool collide(RRTNode const &n);
+       bool collide_steered();
+public:
+       void set_grid_to_check(unsigned int w, unsigned int h, float r,
+                       int8_t const *d, double x, double y);
+};
+
 /*! \brief Collision check based on occupancy grid.
  *
  * \ingroup ext-col

diff --git a/incl/rrtsp.hh b/incl/rrtsp.hh
index ed2da64bae956260e76d312f6f4bdd988a14fc33..da509bfd3bc1fe74d449b0d42c43c452774dbc65 100644 (file)
--- a/incl/rrtsp.hh
+++ b/incl/rrtsp.hh
@@ -25,7 +25,7 @@ namespace rrts {
  *
  * \ingroup planners
  */
-class P40 : public RRTExt20, public RRTExt8, public RRTExt10, public RRTExt14,
+class P40 : public RRTExt21, public RRTExt8, public RRTExt10, public RRTExt14,
                public RRTExt15, public RRTExt19, public RRTExt17,
                public RRTExt13 {
 public:

diff --git a/src/rrtext21.cc b/src/rrtext21.cc
new file mode 100644 (file)
index 0000000..df7cd94
--- /dev/null
+++ b/src/rrtext21.cc
@@ -0,0 +1,55 @@
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <cassert>
+#include <cmath>
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt21::collide(RRTNode const &n)
+{
+       int col = floor((n.x() - this->_origin_x) / this->_grid_res);
+       int row = floor((n.y() - this->_origin_y) / this->_grid_res);
+       if (col < 0 || row < 0 || col >= this->_grid_width
+                       || row >= this->_grid_height) {
+               // grid is too small or sampling too far
+               return true;
+       }
+       if (this->_grid_data[row * this->_grid_width + col] > 65) {
+               return true;
+       }
+       return false;
+}
+
+bool
+RRTExt21::collide_steered()
+{
+       unsigned int i = 0;
+       for (auto &n: this->steered_) {
+               if (this->collide(n)) {
+                       break;
+               }
+               i++;
+       }
+       this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+       return this->steered_.size() == 0;
+}
+
+void
+RRTExt21::set_grid_to_check(unsigned int w, unsigned int h, float r,
+               int8_t const *d, double x, double y)
+{
+       this->_grid_width = w;
+       this->_grid_height = h;
+       this->_grid_res = r;
+       this->_grid_data = d;
+       this->_origin_x = x;
+       this->_origin_y = y;
+}
+
+} /* namespace rrts */