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();
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;