Save last traj as `path` to JSON output

[hubacji1/iamcar.git] / base / main.cc

diff --git a/base/main.cc b/base/main.cc
index bc3dc8b9ce4f72bdfde1d1abba02385681b9c1b1..11fa7d425f8e50f8afce2e21d93afde8dc56acab 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -171,26 +171,17 @@ int main()
         p.HMAX = p.root()->x();
         p.VMIN = p.root()->y();
         p.VMAX = p.root()->y();
+        j = 0;
         for (auto o: jvi["obst"]) {
                 float tmpx;
                 float tmpy;
                 float tmpr;
                 float tmps;
-                if (o["circle"] != Json::nullValue) {
-                        tmpx = o["circle"][0].asFloat();
-                        tmpy = o["circle"][1].asFloat();
-                        tmpr = o["circle"][2].asFloat();
-                        co.push_back(CircleObstacle(tmpx, tmpy, tmpr));
-                        if (tmpx < p.HMIN) p.HMIN = tmpx;
-                        if (tmpx > p.HMAX) p.HMAX = tmpx;
-                        if (tmpy < p.VMIN) p.VMIN = tmpy;
-                        if (tmpy > p.VMAX) p.VMAX = tmpy;
-                }
-                if (o["segment"] != Json::nullValue) {
-                        tmpx = o["segment"][0][0].asFloat();
-                        tmpy = o["segment"][0][1].asFloat();
-                        tmpr = o["segment"][1][0].asFloat();
-                        tmps = o["segment"][1][1].asFloat();
+                for (i = 0; i < o.size() - 1; i++) {
+                        tmpx = o[i][0].asFloat();
+                        tmpy = o[i][1].asFloat();
+                        tmpr = o[i + 1][0].asFloat();
+                        tmps = o[i + 1][1].asFloat();
                         so.push_back(SegmentObstacle(
                                 new RRTNode(tmpx, tmpy, 0),
                                 new RRTNode(tmpr, tmps, 0)
@@ -204,65 +195,20 @@ int main()
                         if (tmpr > p.HMAX) p.HMAX = tmpr;
                         if (tmps < p.VMIN) p.VMIN = tmps;
                         if (tmps > p.VMAX) p.VMAX = tmps;
+
+                        // output
+                        jvo["obst"][j][i][0] = tmpx;
+                        jvo["obst"][j][i][1] = tmpy;
                 }
+                jvo["obst"][j][i][0] = tmpr;
+                jvo["obst"][j][i][1] = tmps;
+                j++;
         }
         p.defaultSamplingInfo();
         p.link_obstacles(&co, &so);
         p.ocost(p.root());
         p.ocost(p.goal());
 
-        ParallelSlot ps = ParallelSlot();
-        if (
-                jvi["slot"] != Json::nullValue &&
-                jvi["slot"]["polygon"] != Json::nullValue
-        ) {
-                for (auto xy: jvi["slot"]["polygon"]) {
-                        ps.slot().add_bnode(new RRTNode(
-                                xy[0].asFloat(),
-                                xy[1].asFloat()
-                        ));
-                }
-                for (auto e: ps.slot().frame())
-                        so.push_back(SegmentObstacle(e->init(), e->goal()));
-        }
-#ifdef USE_SLOTPLANNER
-        TSTART();
-        if (ps.slot().bnodes().size() > 0)
-                ps.fip(co, so);
-        TEND();
-        jvo["ppse"] = ELAPSED;
-        TPRINT("ParallelSlot");
-#endif
-        if (ps.slot().bnodes().size() > 0) {
-                ps.setAll();
-                //if (ps.getMidd() != nullptr)
-                //        p.setSamplingInfo(ps.getSamplingInfo());
-        }
-        if (ps.cusp().size() > 0) {
-                p.goal(ps.getMidd());
-                p.slot_cusp(ps.cusp().front()); // use first found solution
-                p.goals(ps.goals());
-                jvo["midd"][0] = p.goal()->x();
-                jvo["midd"][1] = p.goal()->y();
-                jvo["midd"][2] = p.goal()->h();
-                jvo["goal"][0] = p.slot_cusp().back()->x();
-                jvo["goal"][1] = p.slot_cusp().back()->y();
-                jvo["goal"][2] = p.slot_cusp().back()->h();
-        } else {
-                jvo["goal"][0] = p.goal()->x();
-                jvo["goal"][1] = p.goal()->y();
-                jvo["goal"][2] = p.goal()->h();
-        }
-        TSTART();
-        std::cerr << "Slot Info:" << std::endl;
-        if (ps.slotSide() == LEFT)
-                std::cerr << "- LEFT" << std::endl;
-        else
-                std::cerr << "- RIGHT" << std::endl;
-        if (ps.slotType() == PARALLEL)
-                std::cerr << "- PARALLEL" << std::endl;
-        else
-                std::cerr << "- PERPENDICULAR" << std::endl;
 #ifdef USE_LOADF
         std::vector<RRTNode *> steered;
         for (auto jn: jvi["traj"][0]) {
@@ -312,8 +258,6 @@ int main()
                         p.tlog(p.findt());
                 }
         }
-        if (p.goal_found() && ps.slotType() == PARALLEL)
-                p.tlog(p.findt(p.slot_cusp().back()));
 #elif defined USE_PTHREAD
         bool gf = false;
         RRTNode *ron = nullptr;
@@ -471,6 +415,15 @@ escapeloop:
                         jvo["traj"][j][i][4] = n->s();
                         i++;
                 }
+                if (j == p.tlog().size() - 1) {
+                        i = 0;
+                        for (auto n: traj) {
+                                jvo["path"][i][0] = n->x();
+                                jvo["path"][i][1] = n->y();
+                                jvo["path"][i][2] = n->h();
+                                i++;
+                        }
+                }
                 j++;
         }
 #ifdef JSONLOGEDGES