Add json input method

diff --git a/CHANGELOG.md b/CHANGELOG.md
index c4bedda50ad29b61fc5f21e67774a0805831dd83..1285e6e644a4b5dd48bd1115999a25d03de22942 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,6 +19,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - JSON output.
 - Should continue method.
 - Deinit method -- get ready for planner reset.
+- JSON input.
 
 ### Deprecated
 - `should_continue` method.

diff --git a/api/rrts.h b/api/rrts.h
index c803745df584a834ffcc6465468a073e2657a12f..313b905623c80acb4fc3c006e8b75d80fc890c2f 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -178,6 +178,9 @@ class RRTS {
                 /*! \brief Generate JSON output.
                 */
                 Json::Value json();
+                /*! \brief Load JSON input.
+                */
+                void json(Json::Value jvi);
 
                 // RRT procedures
                 virtual double cost_build(RRTNode &f, RRTNode &t);

diff --git a/src/rrts.cc b/src/rrts.cc
index fab81b2b1657ce95477c187d898ecb027a2fcfd1..ecb66490d8e2b3a704eb75df6e54af527406b1b2 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -435,6 +435,63 @@ Json::Value RRTS::json()
         return jvo;
 }
 
+void RRTS::json(Json::Value jvi)
+{
+        assert(jvi["init"] == Json::nullValue);
+        assert(jvi["goal"] == Json::nullValue);
+        assert(jvi["goals"] == Json::nullValue);
+        assert(jvi["obst"] == Json::nullValue);
+
+        this->nodes().front().x(jvi["init"][0].asDouble());
+        this->nodes().front().y(jvi["init"][1].asDouble());
+        this->nodes().front().h(jvi["init"][2].asDouble());
+        {
+                RRTNode tmp_node;
+                tmp_node.x(jvi["goal"][0].asDouble());
+                tmp_node.y(jvi["goal"][1].asDouble());
+                tmp_node.h(jvi["goal"][2].asDouble());
+                this->goals().push_back(tmp_node);
+                for (auto g: jvi["goals"]) {
+                        tmp_node.x(g[0].asDouble());
+                        tmp_node.y(g[1].asDouble());
+                        tmp_node.h(g[2].asDouble());
+                        this->goals().push_back(tmp_node);
+                }
+        }
+        {
+                Obstacle tmp_obstacle;
+                for (auto o: jvi["obst"]) {
+                        tmp_obstacle.poly().clear();
+                        for (auto c: o) {
+                                double tmp_x = c[0].asDouble();
+                                double tmp_y = c[1].asDouble();
+                                auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
+                                tmp_obstacle.poly().push_back(tmp_tuple);
+                        }
+                        this->obstacles().push_back(tmp_obstacle);
+                }
+        }
+        {
+                double edist_init_goal = sqrt(
+                        pow(
+                                this->nodes().front().x()
+                                - this->goals().front().x(),
+                                2
+                        )
+                        + pow(
+                                this->nodes().front().y()
+                                - this->goals().front().y(),
+                                2
+                        )
+                );
+                this->set_sample(
+                        this->nodes().front().x(), edist_init_goal,
+                        this->nodes().front().y(), edist_init_goal,
+                        0, 2 * M_PI
+                );
+        }
+}
+
 RRTS::RRTS()
         : gen_(std::random_device{}())
 {