For multiquery I had to clear the solutions from a previous query, or otherwise it returns the solution for previous start config if previous solution is better (lower cost). I also had to add PRMstar ( || task->algorithm == sim_ompl_algorithm_PRMstar). See function below:
Code: Select all
void setStartState(SScriptCallBack *p, const char *cmd, setStartState_in *in, setStartState_out *out)
{
TaskDef *task = getTask(in->taskHandle);
validateStateSize(task, in->state);
task->startState.clear();
for(size_t i = 0; i < in->state.size(); i++)
task->startState.push_back(in->state[i]);
// for multi-query PRM, if the OMPL's ProblemDefinition has already been set,
// we want only to clear the query and add the new start state:
if(task->problemDefinitionPtr && task->planner && task->algorithm == sim_ompl_algorithm_PRM || task->algorithm == sim_ompl_algorithm_PRMstar))
{
task->planner->as<og::PRM>()->clearQuery();
ob::ScopedState<> startState(task->stateSpacePtr);
for(size_t i = 0; i < task->startState.size(); i++)
startState[i] = task->startState[i];
std::cout << "plugin.cpp (setStartState): clearing start states" << std::endl;
// Do I also need to clear other stuff, like "solutions"/best pathCost?
//std::cout << "SolutionCount " << task->problemDefinitionPtr->getSolutionCount() << std::endl;
task->problemDefinitionPtr->clearSolutionPaths(); //Uli, clear the solution(s) of previous query
//std::cout << "SolutionCount " << task->problemDefinitionPtr->getSolutionCount() << std::endl;
task->problemDefinitionPtr->clearStartStates();
task->problemDefinitionPtr->addStartState(startState);
}
}
Uli