Replanning not working with dynamically changing environement

[dyouakim]

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.

[alainsanguinetti]

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

[dyouakim]

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.

[alainsanguinetti]

sorry, my first comment was wrong. However, trajectoryparts 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.

[dyouakim]

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

[Notou]

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...

[v4hn]

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

[v4hn]

Continued in https://github.com/ros-planning/moveit/pull/550