From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 15 Aug 2022 15:41:01 +0000 (+0200)
Subject: Merge branch 'occupancy-grid-collision-check'
X-Git-Tag: v0.12.0~2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/142b2aed1f72664c33590343c2e28f37a7ec107b?hp=8ea481ad5931ed9e87fee78aec600ce2b1b26a99

Merge branch 'occupancy-grid-collision-check'
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1cc1f96..b05da5a 100644
--- 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 0c8cd06..87d39c4 100644
--- 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/incl/rrtsp.hh b/incl/rrtsp.hh
index 24f329f..ed2da64 100644
--- a/incl/rrtsp.hh
+++ b/incl/rrtsp.hh
@@ -22,12 +22,10 @@
 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,
+class P40 : public RRTExt20, public RRTExt8, public RRTExt10, public RRTExt14,
 		public RRTExt15, public RRTExt19, public RRTExt17,
 		public RRTExt13 {
 public:
@@ -38,16 +36,11 @@ public:
 		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();
-		RRTExt2::reset();
 	}
 };
 
diff --git a/src/rrtext20.cc b/src/rrtext20.cc
new file mode 100644
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 */