Update test-template, use json() from planner

diff --git a/src/test-template.cc b/src/test-template.cc
index fb7109977ab94bc3fd9c4eb73b822fe30f600bc9..9f057b9e4eb82018c5cf369e86c811b2f2e858d1 100644 (file)
--- a/src/test-template.cc
+++ b/src/test-template.cc
@@ -97,82 +97,11 @@ int main()
                 );
         }
 
+        rrts.init();
         TSTART();
+        while (rrts.next()) {}
         TEND();
 
-        {
-                jvo["time"] = TDIFF();
-        }
-        {
-                jvo["iterations"] = rrts.icnt();
-        }
-        {
-                jvo["init"][0] = rrts.nodes().front().x();
-                jvo["init"][1] = rrts.nodes().front().y();
-                jvo["init"][2] = rrts.nodes().front().h();
-        }
-        {
-                if (rrts.path().size() > 0) {
-                        jvo["cost"] = cc(*rrts.path().back());
-                } else {
-                        jvo["cost"] = -1;
-                }
-        }
-        {
-                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 cu = 0;
-                unsigned int co = 0;
-                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();
-                        if (n->t(RRTNodeType::cusp))
-                                cu++;
-                        if (n->t(RRTNodeType::connected))
-                                co++;
-                        pcnt++;
-                }
-                jvo["cusps-in-path"] = cu;
-                jvo["connecteds-in-path"] = co;
-        }
-        {
-                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++;
-                }
-        }
-        {
-                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++;
-                }
-        }
-        {
-                jvo["nodes"] = (unsigned int) rrts.nodes().size();
-        }
-        //{
-        //        unsigned int ncnt = 0;
-        //        for (auto n: rrts.nodes()) {
-        //                jvo["nodes_x"][ncnt] = n.x();
-        //                jvo["nodes_y"][ncnt] = n.y();
-        //                //jvo["nodes_h"][ncnt] = n.h();
-        //                ncnt++;
-        //        }
-        //}
-        std::cout << jvo << std::endl;
+        std::cout << rrts.json() << std::endl;
         return 0;
 }