Bug report

Required Info:

• Operating System:
  -Ubuntu 22.04.3
• ROS2 Version:
  • Humble Binaries
• Version or commit hash:
  • ros-humble-navigation2 1.1.12-1jammy.20240126.081240
• DDS implementation:
  • not sure

Steps to reproduce issue

Follow steps in https://navigation.ros.org/getting_started/index.html to run default TB3 simulation

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

1. set 2D pose estimate
2. Switch to waypoint / Nav through poses mode
3. Set 3 waypoints where waypoint 1 and 3 are essentially the same and waypoint 2 is distinct

Expected behavior

the turtlebot would travel to the waypoints in order.

Actual behavior

The turtlebot thinks it has achieved its goal as soon as it reaches waypoint 1 (which also happens to be very similar to the final waypoint)

Additional information

This issue has already been raised before #2723 however I believe there was some miscommunication. #2622 This issue was believed to have solved the problem yet they are different issues.

I believe what is happening here is that the simpleGoalChecker is only checking the robots current pose against the final pose in the path. Hence, if the robot happens to run over this final pose while it is still halfway through completing its mission, it will simply think it has reached the end.

I have currently implemented a solution which implements a new BT node "removePassedPoses" (this node essentially replicates the "removePassedGoals" plugin except it will remove poses from the path as the robot progresses.

Then adding the following snippet in in controller_server::isGoalReached()

if(path.poses.size()<3){
    return goal_checkers_[current_goal_checker_]->isGoalReached(
    pose.pose, transformed_end_pose.pose,
    velocity);
  }else{
    return false;
  }

Feature request

Feature description

Implementation considerations