Navigate through poses terminates pre-maturely when incidentally passing over final poses

[thegindi]

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

nav2.mp4

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 https://github.com/open-navigation/navigation2/issues/2723 however I believe there was some miscommunication. https://github.com/open-navigation/navigation2/issues/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

[SteveMacenski]

Those tickets were mostly about intersecting paths, not intersecting goals, hence the additional element of the goal checker.

A goal checker checking the current robot's pose relative to the path and then using the Nav2 Utils method to find the path length makes sense to me (i.e. within radius of the goal and path is less than some reasonably set threshold length). The struggle you'll run into is how to pick the closest point on the path the robot that doesn't sometimes select the end point if you do a simple iteration looking for the smallest distance. You can't simply assume the start of the path is the robot's closest point because paths may not be recomputed often or at all.

We resolve that in the controller plugins by keeping track of the robot's last pose and limiting the distance for search (see RPP's path handler for example) which seems to work well as long as the path is being updated.

There may be better solutions though and I'd encourage some creative thinking :-) This is a nice self-contained contribution ripe for some creativity and out of the box thinking

[KSorte]

is this issue being worked on still?

[SteveMacenski]

The last comment is only 2 days old, so I think that's up to @thegindi about if they want to work on it! But I encourage anyone interested in contributing to the project to potentially work on it!

[thegindi]

I'm happy to work on it. However, I am a bit swamped at the moment so will have to pick it up in maybe 3 weeks time.

[KSorte]

I'm happy to work on it. However, I am a bit swamped at the moment so will have to pick it up in maybe 3 weeks time.

I will explore possible solutions for this next week and get back to you if that is okay?

[SteveMacenski]

Sounds great to me at least! Please follow up here with your ideas and we can go over them together to make sure we have a good solution to code up 😄

[thegindi]

I will explore possible solutions for this next week and get back to you if that is okay?

Sounds good to me!

[KSorte]

Whenever we solve this issue, we would be committing to main and not humble correct?

in the nav2_bt_navigator::NavigateThroughPosesNavigator::onLoop() I found a lambda function that finds the closest pose to the current pose on the global path:

    // Find the closest pose to current pose on global path
    auto find_closest_pose_idx =
      [&current_pose, &current_path]() {
        size_t closest_pose_idx = 0;
        double curr_min_dist = std::numeric_limits<double>::max();
        for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
          double curr_dist = nav2_util::geometry_utils::euclidean_distance(
            current_pose, current_path.poses[curr_idx]);
          if (curr_dist < curr_min_dist) {
            curr_min_dist = curr_dist;
            closest_pose_idx = curr_idx;
          }
        }
        return closest_pose_idx;
      };

We can create a goal checker that uses this method to find closest path point. then find remaining path length.
In the scenario @thegindi describes in the issue, if the robot reaches point 1, this path length should still be non zero right? Even point 1 and point 3 are the same? If we couple navigate_through_poses with compute_path_to_poses, this path length will be necessarily zero I assume.

[SteveMacenski]

we would be committing to main and not humble correct?

Correct, assuming that it doesn't break the method signatures (which I don't know why it would) then we can backport to Humble/iron so that they can use it as well.

The problem with that lambda is that the closest point could be the goal that skips ahead. That's why in RPP we do a similar thing but bound the amount we search ahead by some maximum distance rather than searching the entire path. Imagine you have a path and a goal that is slightly on the left of the path after a loop. If the robot's localized slightly left or path tracking on the left side, then the goal is the closest path point and it will select that one.

Keep in mind that the path may not be recomputed often or at all in some configurations. So I think while tracking, we have to track the last path point in the goal checker (since I think we can assume that the progress checker is called on a consistent basis to use a rolling window tracking the closest path points) to start as the "closest" and search only within a window size rather than the full path. This is how we got past the looping problems in the previous ticket linked (that just didn't address the goal checker)

[M-Schroeder]

Assuming, that the poses in the global plan are in the right order:
If the global plan is already a bit old, the positions of that plan should first get closer to the current position and then further away again.
To get the closest position, I start the search at the beginning and stop, if the distance between current position and the pose of the plan gets larger again (50% larger than the smallest distance, that was found so far).
This is the corresponding part of the code (poseInMap is the current pose expressed in the frame map):

  // find the closest pose of the global plan to the current pose. This is the referencePose for k=0
  int indexClosest = 0;
  double smallestDist = 1e9;
  bool stop = false;
  for(int i=0; ((i<planLength)&(!stop)); i++){
    double dist = euclidean_distance(global_plan_.poses[i].pose, poseInMap.pose);
    if(dist < smallestDist){
      indexClosest = i;
      smallestDist = dist;
    } else if(dist > 1.5*smallestDist)
      // I can stop the search, if the distance gets larger again.
      stop = true;
  }
  referencePath.poses.push_back(global_plan_.poses[indexClosest]);

I need that for getting reference positions for an MPC, but maybe it is helpful for this problem too.

[SteveMacenski]

I had tried something like that before but was concerned that localization error or paths that intersect might cause some edge cases there. For example, if you're doing a coverage swath pattern and start from the beginning, you'll get closer then further again when evaluating the distance on an previously passed row.

Isn't the goal checker called on a regular frequency? I think what I described is a marginally more robust and already in use elsewhere in the codebase as mentioned above. I think there's wisdom in keeping all these things doing roughly the same thing unless we can come up with a bulletproof solution.

[KSorte]

goal checker is called inside the computeControl() function which is the callback for the follow_path action server implemented inside controller server. So I assume that the goal checker is called every time a goal is sent to follow path node. Right?

[SteveMacenski]

It is called every iteration of the control loop to check if its now complete

https://github.com/ros-navigation/navigation2/blob/main/nav2_controller/src/controller_server.cpp#L750

[KSorte]

The goal checker and the progress checker both seem to not deal with path points but the robot positions as arguments. I am trying to figure out where are we tracking the last path point and not robot pose.

[SteveMacenski]

We could change the API to include the path (if given). Note that that would cause an API breakage so it couldn't be backported to Humble, but would be available in Jazzy

[KSorte]

We could change the API to include the path (if given). Note that that would cause an API breakage so it couldn't be backported to Humble, but would be available in Jazzy

Understood. That makes sense. That would mean we would be making changes to the controller server as well as changing the simple goal checker right? Or do we create a new goal checker plugin? I prefer the former if we are going to change the API itself.

[SteveMacenski]

Yeah, so we'd change the GoalChecker's header definition in nav2_core to take in the path, update the plugins that exist to have it in their signature (and just ignore it), then add it to the goal checker we need it in and do the process we discussed (or another thing we come up with). No matter what, I think we'll need the path in the goal checker, so this makes sense.

I think we can update the default nav2 goal checker. This makes sense to have supported out of the box for the reasons you filed this in the first place 😄

[KSorte]

Sounds like a plan to me!

[thegindi]

Just to clarify, the idea is we add the path to goal checker and limit the goal checker to search within it surrounding region of the path?

[KSorte]

Just to clarify, the idea is we add the path to goal checker and limit the goal checker to search within it surrounding region of the path?

yes. Similar to the path intersecting ticket that was discussed previously.

[KSorte]

Just to clarify, the idea is we add the path to goal checker and limit the goal checker to search within it surrounding region of the path?

yes. Similar to the path intersecting ticket that was discussed previously.

I will begin work on this as soon as possible.