From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Fri, 19 Aug 2022 09:02:00 +0000 (+0200)
Subject: Add simple occupancy grid collision check
X-Git-Tag: v0.13.0^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/e50446160c52e70d2f7df61c12b429a285549717?ds=sidebyside

Add simple occupancy grid collision check
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b05da5a..574c6e7 100644
--- 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 87d39c4..e285fa2 100644
--- 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 ed2da64..da509bf 100644
--- 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
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 */