xy[1].asFloat()
));
}
- }
- if (ps.slot().bnodes().size() > 0) {
- ps.setAll();
- p.samplingInfo_ = ps.getSamplingInfo();
- p.useSamplingInfo_ = true;
+ 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.fipr(ps.getFP());
+ ps.fip(co, so);
TEND();
jvo["ppse"] = ELAPSED;
TPRINT("ParallelSlot");
#endif
+ if (ps.slot().bnodes().size() > 0) {
+ ps.setAll();
+ //p.setSamplingInfo(ps.getSamplingInfo());
+ }
if (ps.cusp().size() > 0) {
- p.goal(ps.cusp().front().front());
+ 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"][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.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());
}
}
#elif defined USE_PTHREAD