Update rrt goal based on pose range refactoring

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 9c0d4c28307f2390bcd0979d1a27383f38b89237..f2c8962d4b9484b55b247ecbe3eff565ec2d0190 100644 (file)
--- 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 09e65ed9a4d92647a04872bc52554101a0f3abf3..100c8cc243d770b4e979a1b5aefb3b0cc61155be 100644 (file)
--- 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());