Output JSON goal (from JSON input)

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

diff --git a/base/main.cc b/base/main.cc
index 422e3308ad572ecb0b803a4927875fe50c67346e..a00c30505c75ad0841043006d2c2436f9b5425de 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -53,6 +53,10 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
         //#define USE_PTHREAD
 #endif
 
+// OpenGL window size
+#define SCREEN_WIDTH 1000
+#define SCREEN_HEIGHT 1000
+
 std::chrono::high_resolution_clock::time_point TSTART_;
 std::chrono::high_resolution_clock::time_point TEND_;
 float TELAPSED = 0;
@@ -134,94 +138,91 @@ int main()
         std::cin >> jvi;
         std::string encoding = jvi.get("encoding", "UTF-8" ).asString();
 
+        if (jvi["init"] == Json::nullValue) {
+                std::cerr << "I need `init` in JSON input scenario";
+                std::cerr << std::endl;
+                return 1;
+        }
+
+        if (jvi["goal"] == Json::nullValue) {
+                std::cerr << "I need `goal` in JSON input scenario";
+                std::cerr << std::endl;
+                return 1;
+        }
+
         PLANNER p(
-                        new RRTNode(
-                                jvi["init"][0].asFloat(),
-                                jvi["init"][1].asFloat(),
-                                jvi["init"][2].asFloat()),
-                        new RRTNode(
-                                jvi["goal"][0].asFloat(),
-                                jvi["goal"][1].asFloat(),
-                                jvi["goal"][2].asFloat()));
+                new RRTNode(
+                        jvi["init"][0].asFloat(),
+                        jvi["init"][1].asFloat(),
+                        jvi["init"][2].asFloat()
+                ),
+                new RRTNode(
+                        jvi["goal"][0].asFloat(),
+                        jvi["goal"][1].asFloat(),
+                        jvi["goal"][2].asFloat()
+                )
+        );
         jvo["init"][0] = p.root()->x();
         jvo["init"][1] = p.root()->y();
         jvo["init"][2] = p.root()->h();
+        jvo["goal"][0] = jvi["goal"][0].asFloat();
+        jvo["goal"][1] = jvi["goal"][1].asFloat();
+        jvo["goal"][2] = jvi["goal"][2].asFloat();
+
+        if (jvi["goals"] != Json::nullValue) {
+                for (auto g: jvi["goals"]) {
+                        p.goals().push_back(new RRTNode(
+                                g[0].asFloat(),
+                                g[1].asFloat(),
+                                g[2].asFloat()
+                        ));
+                }
+        }
+
         std::vector<CircleObstacle> co;
         std::vector<SegmentObstacle> so;
+        p.HMIN = p.root()->x();
+        p.HMAX = p.root()->x();
+        p.VMIN = p.root()->y();
+        p.VMAX = p.root()->y();
+        j = 0;
         for (auto o: jvi["obst"]) {
-                if (o["circle"] != Json::nullValue) {
-                        co.push_back(CircleObstacle(
-                                                o["circle"][0].asFloat(),
-                                                o["circle"][1].asFloat(),
-                                                o["circle"][2].asFloat()));
-                }
-                if (o["segment"] != Json::nullValue) {
+                float tmpx;
+                float tmpy;
+                float tmpr;
+                float tmps;
+                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(
-                                        o["segment"][0][0].asFloat(),
-                                        o["segment"][0][1].asFloat(),
-                                        0),
-                                new RRTNode(
-                                        o["segment"][1][0].asFloat(),
-                                        o["segment"][1][1].asFloat(),
-                                        0)));
+                                new RRTNode(tmpx, tmpy, 0),
+                                new RRTNode(tmpr, tmps, 0)
+                        ));
                         p.frame().add_bnode(so.back().init());
+                        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 (tmpr < p.HMIN) p.HMIN = tmpr;
+                        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()
-                        ));
-                }
-        }
-#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();
-                p.samplingInfo_ = ps.getSamplingInfo();
-                p.useSamplingInfo_ = true;
-        }
-        p.goals(ps.goals());
-        if (ps.cusp().size() > 0) {
-                p.goal(ps.getMidd());
-                p.slot_cusp(ps.cusp().front()); // use first found solution
-                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]) {
@@ -428,6 +429,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