From f2c63feb939bbe7cd4669c94e16f7d1d165ef45a Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Sun, 7 Feb 2021 12:46:45 +0100 Subject: [PATCH] Add entry as json input Entry here means the range of poses in the format {x, y, b, e}, where (x, y) are the cartesian coordinates, b is begin of the heading range and e is the end of the heading range. --- api/rrts.h | 4 ++++ src/rrts.cc | 27 +++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/api/rrts.h b/api/rrts.h index 2b10dd1..cd44b09 100644 --- a/api/rrts.h +++ b/api/rrts.h @@ -206,6 +206,10 @@ class RRTS { bool connect(); void rewire(); public: + /// --- + struct { double x; double y; double b; double e; } entry; + bool entry_set = false; + /// --- /*! \brief Initialize RRT algorithm if needed. */ virtual void init(); diff --git a/src/rrts.cc b/src/rrts.cc index a7fd30e..bca0834 100644 --- a/src/rrts.cc +++ b/src/rrts.cc @@ -572,9 +572,36 @@ void 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()); + { + if (jvi["entry"] != Json::nullValue) { + this->entry_set = true; + this->entry.x = jvi["entry"][0].asDouble(); + this->entry.y = jvi["entry"][1].asDouble(); + this->entry.b = jvi["entry"][2].asDouble(); + this->entry.e = jvi["entry"][3].asDouble(); + } else { + this->entry.x = 0.0; + this->entry.y = 0.0; + this->entry.b = 0.0; + this->entry.e = 0.0; + } + } { RRTNode tmp_node; RRTNode* gp = nullptr; + if (jvi["entry"] != Json::nullValue) { + this->entry_set = true; + this->entry.x = jvi["entry"][0].asDouble(); + this->entry.y = jvi["entry"][1].asDouble(); + this->entry.b = jvi["entry"][2].asDouble(); + this->entry.e = jvi["entry"][3].asDouble(); + tmp_node.x(this->entry.x); + tmp_node.y(this->entry.y); + tmp_node.h((this->entry.b + this->entry.e) / 2.0); + this->goals().push_back(tmp_node); + this->goals().back().p(gp); + gp = &this->goals().back(); + } for (auto g: jvi["goals"]) { tmp_node.x(g[0].asDouble()); tmp_node.y(g[1].asDouble()); -- 2.39.2