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<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"]) {
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)
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]) {
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;
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];
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;
projects / hubacji1 / iamcar.git / blobdiff