int PointToSetMotionPlanner::AddMilestone(const Config& q)
{
int n=mp->AddMilestone(q);
if(goalSpace->IsFeasible(q))
goalNodes.push_back(n);
return n;
}
int PointToSetMotionPlanner::PlanMore()
{
if(mp->CanAddMilestone()) {
sampleGoalCounter++;
if(sampleGoalCounter >= sampleGoalPeriod*((int)goalNodes.size()+1)) {
sampleGoalCounter = 0;
Config q;
if(SampleGoal(q)) {
return AddMilestone(q);
}
else
return -1;
}
}
int res = mp->PlanMore();
if(res >= 0) {
Config q;
mp->GetMilestone(res,q);
if(goalSpace->IsFeasible(q))
goalNodes.push_back(res);
}
return res;
}
bool PointToSetMotionPlanner::SampleGoal(Config& q)
{
goalSpace->Sample(q);
return goalSpace->IsFeasible(q);
}