Giters

moveit / moveit_ros

THIS REPO HAS MOVED TO https://github.com/ros-planning/moveit

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Replanning not working with dynamically changing environement

dyouakim opened this issue · comments

commented

Given the discussion on MoveIt! group https://groups.google.com/forum/#!topic/moveit-users/3OPdM_Db40E
Looks like changing the environment during the execution does not invoke a re-planning action.
The analysis and suggested solution are explained in the thread. I just need more confirmation to ensure that this modification makes sense and will not affect another working functionality.

commented

With the solution that you suggest:
if (time_index_.empty())
return std::make_pair((int)current_context_, trajectories_[ current_context_ ]->trajectory_parts_.size() );

does it successfully detect the path as invalid ? Because you are basically asking to check only between the last and last point if the remaining path is valid.

Maybe a safer way would be:
if (time_index_.empty())
return std::make_pair((int)current_context_, 0 );
This would explicitly check the whole path

commented

Actually it is working, if I am not mistaken current_context will give 0 and the length of the trajectory usually is 1.
And also I discovered that this condition is failing when the environment has changed after a plan is generated but before execution starts. If the update arrives after the execution starts, it goes to the "else" in getCurrentIndex . I am still not sure about the change and also not sure about the computation of current index, as the idea of isRemainingPath is to check only the remaining path and the change of the current index is tied to the speed of execution at the controller level, so what happens with an octomap bag file where the environment is continuously changing that it is failing to detect collision.
I will continue analyzing before making any changes and post more details later here.

commented

sorry, my first comment was wrong.
However, trajectory_parts_ is one because you only have one controller. This means you are comparing a number of different controller with a waypoint index. You should as well hard code 1 or 0.

commented

Yeah maybe it is good solution for my case, I just wanted to keep it more general.

commented

Is there any update on this?
I'm using your suggested modification without issues it seems.

if (time_index_.empty())
return std::make_pair((int)current_context_, 0 );

But maybe you have found some side effects or a better way to do it...

commented

I guess there is no update here.
However, it would be nice to see a pull-request for this on the current MoveIt repo.