Refactor JSON input init, goal (typo)

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

diff --git a/base/main.cc b/base/main.cc
index 422e3308ad572ecb0b803a4927875fe50c67346e..bc3dc8b9ce4f72bdfde1d1abba02385681b9c1b1 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,40 +138,75 @@ 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();
         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();
         for (auto o: jvi["obst"]) {
+                float tmpx;
+                float tmpy;
+                float tmpr;
+                float tmps;
                 if (o["circle"] != Json::nullValue) {
-                        co.push_back(CircleObstacle(
-                                                o["circle"][0].asFloat(),
-                                                o["circle"][1].asFloat(),
-                                                o["circle"][2].asFloat()));
+                        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();
                         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;
                 }
         }
+        p.defaultSamplingInfo();
         p.link_obstacles(&co, &so);
         p.ocost(p.root());
         p.ocost(p.goal());
@@ -183,6 +222,8 @@ int main()
                                 xy[1].asFloat()
                         ));
                 }
+                for (auto e: ps.slot().frame())
+                        so.push_back(SegmentObstacle(e->init(), e->goal()));
         }
 #ifdef USE_SLOTPLANNER
         TSTART();
@@ -194,13 +235,13 @@ int main()
 #endif
         if (ps.slot().bnodes().size() > 0) {
                 ps.setAll();
-                p.samplingInfo_ = ps.getSamplingInfo();
-                p.useSamplingInfo_ = true;
+                //if (ps.getMidd() != nullptr)
+                //        p.setSamplingInfo(ps.getSamplingInfo());
         }
-        p.goals(ps.goals());
         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();
@@ -271,6 +312,8 @@ 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;