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.

diff --git a/api/rrts.h b/api/rrts.h
index 2b10dd18c66dc16adfe2337ab92e7ba486541db2..cd44b09b210b5e22dc62ca0bd0bffff9fa6685d1 100644 (file)
--- 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 a7fd30eb086515402151dfb8b5af64c563e61315..bca083429b1ed5c5a96ec4ca7ed99c3c7ba32557 100644 (file)
--- 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());