xy[1].asFloat()
));
}
+ for (auto e: ps.slot().frame())
+ so.push_back(SegmentObstacle(e->init(), e->goal()));
}
#ifdef USE_SLOTPLANNER
TSTART();
#endif
if (ps.slot().bnodes().size() > 0) {
ps.setAll();
- p.samplingInfo_ = ps.getSamplingInfo();
- p.useSamplingInfo_ = true;
+ //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();
p.next();
p.tend();
if (p.opt_path()) {
- if (ps.cusp().size() > 0)
- p.tlog(p.findt(p.slot_cusp().back()));
- else
- p.tlog(p.findt());
+ p.tlog(p.findt());
}
}
+ if (ps.slotType() == PARALLEL)
+ p.tlog(p.findt(p.slot_cusp().back()));
#elif defined USE_PTHREAD
bool gf = false;
RRTNode *ron = nullptr;