Navigate through poses terminates pre-maturely when incidentally passing over final poses about navigation2 HOT 21 OPEN

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

Sounds good to me!

from navigation2.

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.

from navigation2.

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)

from navigation2.

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 😄

from navigation2.

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.

from navigation2.

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

from navigation2.

is this issue being worked on still?

from navigation2.

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!

from navigation2.

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.

from navigation2.

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?

from navigation2.

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 😄

from navigation2.

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.

from navigation2.

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.

from navigation2.

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.

from navigation2.

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?

from navigation2.

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

from navigation2.

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.

from navigation2.

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

from navigation2.

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.

from navigation2.

Sounds like a plan to me!

from navigation2.

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?

from navigation2.