X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/blobdiff_plain/2c5e4a2065e97a67b38a22c91340c7fd79c19776..ee6b0f6e6374aca2a12964a08ef418f23b3eb40f:/base/main.cc diff --git a/base/main.cc b/base/main.cc index c327d78..aad1561 100644 --- a/base/main.cc +++ b/base/main.cc @@ -138,44 +138,69 @@ 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() + ) + ); + p.firsts().push(p.goal()); 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() + )); + } + } + + if (jvi["slot"] != Json::nullValue) { + jvo["slot"] = jvi["slot"]; + } + std::vector co; std::vector 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"]) { 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) @@ -189,65 +214,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 steered; for (auto jn: jvi["traj"][0]) { @@ -297,8 +277,8 @@ int main() p.tlog(p.findt()); } } - if (p.goal_found() && ps.slotType() == PARALLEL) - p.tlog(p.findt(p.slot_cusp().back())); + p.tend(); + jvo["time"] = p.elapsed(); #elif defined USE_PTHREAD bool gf = false; RRTNode *ron = nullptr; @@ -435,8 +415,9 @@ escapeloop: jvo["nodo"][0] = nodo; #endif // log cost - for (j = 0; j < p.clog().size(); j++) - jvo["cost"][j] = p.clog()[j]; + //for (j = 0; j < p.clog().size(); j++) + // jvo["cost"][j] = p.clog()[j]; + jvo["cost"] = p.tlog()[p.tlog().size() - 1].front()->ccost(); // log #nodes for (j = 0; j < p.nlog().size(); j++) jvo["node"][j] = p.nlog()[j]; @@ -456,8 +437,20 @@ 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++; } + jvo["goals"][0][0] = p.tlog()[p.tlog().size() - 1].front()->x(); + jvo["goals"][0][1] = p.tlog()[p.tlog().size() - 1].front()->y(); + jvo["goals"][0][2] = p.tlog()[p.tlog().size() - 1].front()->h(); #ifdef JSONLOGEDGES i = 0; j = 0;