ompl solution path not always cleared

[torydebra]

Hi there,
Just encountered a problem: when a plan is requested a second/third... time, it may happen that the trajectory (_planner->getSolutionPath()) remains the same, as the previous planned one. For example:

void OmplPlanner::setStartAndGoalStates(const Eigen::VectorXd& start,
                                        const Eigen::VectorXd& goal,
                                        const double threshold)

...
    _pdef->setStartAndGoalStates(ompl_start, ompl_goal, threshold);

    Eigen::VectorXd int_start, int_goal;
    _sw->getState(ompl_start.get(), int_start);
    _sw->getState(ompl_goal.get(), int_goal);

    std::cout << "start: " << int_start.transpose() << std::endl;
    std::cout << "goal: " << int_goal.transpose() << std::endl;

produces, as expected:

start:     1.26619    0.509053    -1.89093 3.08153e-06     0.74163     1.26619
goal:     0.895672       0.6943     -1.54941 -6.60662e-06     0.897883     0.895682

but, after solving:

_planner->setStartAndGoalStates(starting_q, planning_qgoal, _goal_thrs);
_planner->solve(_planning_time, _planner_type)

std::cout << "trajectory: " << std::endl;
for (const auto & it : _planner->getSolutionPath()) {
    std::cout << it.transpose() << std::endl;
}
std::cout << std::endl;

this prints:

trajectory: 
    0.871956     0.715716     -1.50945 -5.14212e-06      0.91714     0.871965
    1.26619    0.509052    -1.89093 3.08155e-06    0.741629     1.26619

So the last point is equal the given start, which is the (correct) trajectory but of the previous planner request.
This does not happen always, but often.

I found out that this seems to be solved easily explicity calling the clear methods in the OmplPlanner::setStartAndGoalStates method before the _pdef->setStartAndGoalStates():

// set start and goal
_pdef->clearStartStates();
_pdef->clearGoal();
_pdef->clearSolutionPaths();

(not sure if all of three are necessary, but _pdef->clearSolutionPaths(); for sure to solve my problem)

@lucarossini-iit

[lucarossini-iit]

Hi @torydebra. We had similar issues in the past but it seems a desired behavior by ompl because you can call the same planner several times in case it is not able to find a solution in the given timeout. The easiest and most effective way we found to clear the planner is to reset the shared_ptr:

std::shared_ptr<OmplPlanner> _planner;
_planner.reset()

of course then you need to reinitialize the OmplPlanner from the beginning. You can find an example here

Alternatively, your way should work fine, but be aware of missing things to be cleared or re-initialized