Add necessary input, output

diff --git a/src/test2.cc b/src/test2.cc
index bcab6bcb7de1d446a997c1486c3c3ac7db1740b0..b92ced8316c57a6f6c99c40cc3acec2c7440abe5 100644 (file)
--- a/src/test2.cc
+++ b/src/test2.cc
@@ -51,6 +51,81 @@ int main()
                 return 1;
         }
 
+        RRTS rrts;
+        rrts.nodes().front().x(jvi["init"][0].asDouble());
+        rrts.nodes().front().y(jvi["init"][1].asDouble());
+        rrts.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());
+                rrts.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());
+                        rrts.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);
+                        }
+                        rrts.obstacles().push_back(tmp_obstacle);
+                }
+        }
+
+        {
+                jvo["time"] = TDIFF();
+        }
+        {
+                jvo["init"][0] = rrts.nodes().front().x();
+                jvo["init"][1] = rrts.nodes().front().y();
+                jvo["init"][2] = rrts.nodes().front().h();
+        }
+        {
+                unsigned int ocnt = 0;
+                for (auto o: rrts.obstacles()) {
+                        unsigned int ccnt = 0;
+                        for (auto c: o.poly()) {
+                                jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
+                                jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
+                                ccnt++;
+                        }
+                        ocnt++;
+                }
+        }
+        {
+                unsigned int gcnt = 0;
+                for (auto g: rrts.goals()) {
+                        jvo["goals"][gcnt][0] = g.x();
+                        jvo["goals"][gcnt][1] = g.y();
+                        jvo["goals"][gcnt][2] = g.h();
+                        gcnt++;
+                }
+        }
+        {
+                if (rrts.path().size() > 0) {
+                        jvo["goal"][0] = rrts.path().back()->x();
+                        jvo["goal"][1] = rrts.path().back()->y();
+                        jvo["goal"][2] = rrts.path().back()->h();
+                }
+                unsigned int pcnt = 0;
+                for (auto n: rrts.path()) {
+                        jvo["path"][pcnt][0] = n->x();
+                        jvo["path"][pcnt][1] = n->y();
+                        jvo["path"][pcnt][2] = n->h();
+                        pcnt++;
+                }
+        }
+
         std::cout << jvo << std::endl;
         return 0;
 }