From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 20 Jul 2021 11:25:40 +0000 (+0200)
Subject: Update rrt goal based on pose range refactoring
X-Git-Tag: v0.8.0~1^2~5
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/75ead9ecf0f0a75af096dbc8aaa9db3cfaf3c314

Update rrt goal based on pose range refactoring
---

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 9c0d4c2..f2c8962 100644
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -47,6 +47,8 @@ public:
 };
 
 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
+public:
+	using PoseRange::PoseRange;
 };
 
 /*! RRT* algorithm basic class. */
diff --git a/src/rrts.cc b/src/rrts.cc
index 09e65ed..100c8cc 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -207,7 +207,7 @@ RRTS::compute_path()
 	std::reverse(this->path_.begin(), this->path_.end());
 }
 
-RRTS::RRTS() : gen_(std::random_device{}())
+RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
 {
 	this->nodes_.reserve(4000000);
 	this->steered_.reserve(1000);
@@ -259,13 +259,16 @@ RRTS::json(Json::Value jvi)
 	this->nodes_.front().x(jvi["init"][0].asDouble());
 	this->nodes_.front().y(jvi["init"][1].asDouble());
 	this->nodes_.front().h(jvi["init"][2].asDouble());
-	this->goal_.x(jvi["goal"][0].asDouble());
-	this->goal_.y(jvi["goal"][1].asDouble());
-	this->goal_.b(jvi["goal"][2].asDouble());
 	if (jvi["goal"].size() == 4) {
-		this->goal_.e(jvi["goal"][3].asDouble());
+		this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+			jvi["goal"][1].asDouble(),
+			jvi["goal"][2].asDouble(),
+			jvi["goal"][3].asDouble());
 	} else {
-		this->goal_.e(jvi["goal"][2].asDouble());
+		this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+			jvi["goal"][1].asDouble(),
+			jvi["goal"][2].asDouble(),
+			jvi["goal"][2].asDouble());
 	}
 }
 
@@ -346,7 +349,8 @@ RRTS::next()
 void
 RRTS::reset()
 {
-	this->goal_ = RRTGoal();
+	this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
+		this->goal_.e());
 	this->path_.clear();
 	this->steered_.clear();
 	this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());