Navigation: Global Planner and Replanning [closed]

edit

Hello,

In the navigation stack, global planner continuously generates a plan/path to the goal based on a fixed frequency (planner_frequency).

I want the robot not to deviate more than a fixed distance from the original path generated from its initial location to the goal. This is usually not a problem when no new obstacles appear on the path, since the new paths generated by the global planner mostly align with the original path. However, if during navigation an obstacle appears on the path, global planner, in order to avoid the obstacle, replans and generates a path which does not align with the original path.

What I would like to do is to get the robot to stop if the new path is farther than a certain distance of the original path. In this case, either the obstacle moves and the robot again continues to move towards the goal along the original path (or along a replanned path still close to the original path) or the obstacle doesn't move and the robot keeps staying in its position.

So the question is how can I compute the distance between the new paths being generated by the global planner and the original path? Should I compare the distance between the waypoints on the two paths one by one?

Thanks