MotionPlannerInterface* MotionPlannerFactory::Create(const MotionPlanningProblem& problem)
{
if(problem.startSet) FatalError("MotionPlannerFactory: Cannot do start-set problems yet");
if(problem.qstart.empty() && (!problem.qgoal.empty() || problem.goalSet!=NULL)) FatalError("MotionPlannerFactory: Goal set specified but start not specified");
if(!problem.qstart.empty() && problem.goalSet) { //point-to-goal problem
//pick a multi-query planner for the underlying planner
string oldtype = type;
if(type == "any") type = "sblprt";
MotionPlannerInterface* mp = Create(problem.space);
type = oldtype;
PointToSetMotionPlanner* psmp = new PointToSetMotionPlanner(mp,problem.qstart,problem.goalSet);
return psmp;
}
else {
MotionPlannerInterface* mp = CreateRaw(problem.space);
if(!problem.qstart.empty()) {
int qstart = mp->AddMilestone(problem.qstart);
Assert(qstart == 0);
}
if(!problem.qgoal.empty()) {
int qgoal = mp->AddMilestone(problem.qgoal);
Assert(qgoal == 1);
}
return ApplyModifiers(mp,problem);
}
}
Private(base::const_byte_span key)
: _rsa(CreateRaw(key)) {
if (_rsa) {
computeFingerprint();
}
}
