Robot is oscillating back and forth after passing v-shaped turning.

[antoszy]

I set the global path in waypoin planner in v-shape:

When the robot is already 1.5 m past the corner point the new plan that shows up is pointing backwards and the robot starts to move back

After moving back a little ii gets a new plan, which correctly points towards the goal.

In this place robot swings back and forth until the plan gets aborted: Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors

Video showing the problem: https://user-images.githubusercontent.com/23712573/145777186-eea04ec3-b14b-4ff4-a0c3-8a32c696eeb5.mp4

Configuration

ROS version: melodic Global planner: WaypointGlobalPlanner The robot has a differential drive. Teb planner configuration: teb_local_planner_params.yaml.txt

[antoszy]

The problem is caused be the following lines in funciton TebLocalPlannerROS::transformGlobalPlan:

      if (new_sq_dist > sq_dist_threshold)
        break;  // force stop if we have reached the costmap border

The assumption here is that: if the plan goes out of local costmap, than there is no point in iterating the plan any further. This assumption is wrong because the path can leave and reenter local costmap (see image below).

We can workaround this problem by decreasing global_plan_prune_distance parameter, but the correct solution would be to remove the above mentioned lines, as they are based on a wrong assumption.

Video before fix: https://user-images.githubusercontent.com/23712573/145777186-eea04ec3-b14b-4ff4-a0c3-8a32c696eeb5.mp4

Video after fix: https://user-images.githubusercontent.com/23712573/147055974-0373f5e5-7485-435c-b7de-12acf3d3d382.mov

[corot]

Superbly reported issue! thanks for the thorough analyses. The fix you propose looks correct; indeed, seems to me that the lines you are removing are a relic from the base_local_planner version, not needed here because on TEB we try to find the path's closest point to the robot. @croesmann , can you confirm this?

But I can only reproduce the issue by disabling path pruning (I just commented the line, but I suppose setting global_plan_prune_distance to inf will do the same)

Unless you have set global_plan_prune_distance to something very big, I guess your path has only 3 or four poses (the purple dots), and the pruning "fails" upon reaching this condition. Would you mind to check if that's the case? If pruning where working, the path starts 1m behind your robot, and the issue shouldn't happen.