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

is this issue being worked on still?

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!

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'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?

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 😄

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

Sounds good to me!

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.

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)

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.

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.

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?

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

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.

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

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.

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 😄

Sounds like a plan to me!

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?

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.

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.

I also ran into this issue when I tried to get the robot back to its starting pose in the end of navigate through poses. The robot didn't move because the robot's starting pose was the same as the final goal pose, and the goal checker thought the robot had already reached the final goal pose (which was true but all the intermediary poses were skipped).

Are there any solutions for this?

We had the same issue, fixed it using calculating yaw from current pose and fixed it in controller server and other custom plugins and in nav_through_poses and nav_to_pose, I wont be able to build against main branch due to resources and time but this fix is working on all our robots for almost 6 + months. Either I can paste the solution here or @padhupradheep said I can forward him the files after creating PR and he can build against the main branch OR currently I spoke to my team member, he is willing to help as well (Alex). Please let me know how you want this to be done @SteveMacenski

Whatever solution you think is best - @padhupradheep is a good resource! You can also give us a branch and what distribution and I'm sure we can find the time to forward port to main / Jazzy

Thanks for the follow up @anath93 !

@SteveMacenski Sounds good, I will send this over to Pradheep later today or latest by coming Monday !

I have given the files to Pradheep, looks like he is busy too. But in the meantime @thegindi if you wanna take initiative building the files against main or atleast in your code if you are building from source, please see below.
Header definitions:

Params for blackboard, I would start with 1.04 and stay with it unless your routes are intersecting at different angles

Navigatetopose.cpp

inline double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation)
{
  tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
  tf2::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  return yaw;
}

NavigateToPoseNavigator::onLoop()
{
  // action server feedback (pose, duration of task,
  // number of recoveries, and distance remaining to goal)
  auto feedback_msg = std::make_shared<ActionT::Feedback>();

  geometry_msgs::msg::PoseStamped current_pose;
  nav2_util::getCurrentPose(
    current_pose, *feedback_utils_.tf,
    feedback_utils_.global_frame, feedback_utils_.robot_frame,
    feedback_utils_.transform_tolerance);

  auto blackboard = bt_action_server_->getBlackboard();

  try {
    // Get current path points
    nav_msgs::msg::Path current_path;
    blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);

    // Find the closest pose to current pose on global path
    auto find_closest_pose_idx =
      [this,&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]);
          double orient_diff = createYawFromQuat(current_pose.pose.orientation) - createYawFromQuat(current_path.poses[curr_idx].pose.orientation);
          if (curr_dist < curr_min_dist && abs(orient_diff) < orient_angle_rad_) {
            curr_min_dist = curr_dist;
            closest_pose_idx = curr_idx;
          }
        }
        return closest_pose_idx;
      };

Add this line to bool NavigateToPoseNavigator::configure

  orient_angle_rad_ = node->get_parameter("orient_angle_rad", orient_angle_rad_);

goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string();
  if (!node->has_parameter("orient_angle_rad")) {
    node->declare_parameter("orient_angle_rad", rclcpp::ParameterValue(1.57));
  }
  orient_angle_rad_ = node->get_parameter("orient_angle_rad", orient_angle_rad_);

Navigatethroughposes.cpp (same as above)

controller_Server.cpp

declare_parameter("orient_angle_rad", rclcpp::ParameterValue(1.57));
get_parameter("orient_angle_rad", orient_angle_rad_);

double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation)
{
  tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
  tf2::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  return yaw;
}
void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
{
  RCLCPP_DEBUG(
    get_logger(),
    "Providing path to the controller %s", current_controller_.c_str());
  if (path.poses.empty()) {
    throw nav2_core::PlannerException("Invalid path, Path is empty.");
  }
  controllers_[current_controller_]->setPlan(path);

  end_pose_ = path.poses.back();
  end_pose_.header.frame_id = path.header.frame_id;
  goal_checkers_[current_goal_checker_]->reset();

  RCLCPP_DEBUG(
    get_logger(), "Path end point is (%.2f, %.2f)",
    end_pose_.pose.position.x, end_pose_.pose.position.y);

  current_path_ = path;
}

void ControllerServer::computeAndPublishVelocity()
{
  geometry_msgs::msg::PoseStamped pose;

  if (!getRobotPose(pose)) {
    throw nav2_core::PlannerException("Failed to obtain robot pose");
  }

  if (!progress_checker_->check(pose)) {
    throw nav2_core::PlannerException("Failed to make progress");
  }

  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());

  geometry_msgs::msg::TwistStamped cmd_vel_2d;

  try {
    cmd_vel_2d =
      controllers_[current_controller_]->computeVelocityCommands(
      pose,
      nav_2d_utils::twist2Dto3D(twist),
      goal_checkers_[current_goal_checker_].get());
    last_valid_cmd_time_ = now();
  } catch (nav2_core::PlannerException & e) {
    if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
      RCLCPP_WARN(this->get_logger(), "%s", e.what());
      cmd_vel_2d.twist.angular.x = 0;
      cmd_vel_2d.twist.angular.y = 0;
      cmd_vel_2d.twist.angular.z = 0;
      cmd_vel_2d.twist.linear.x = 0;
      cmd_vel_2d.twist.linear.y = 0;
      cmd_vel_2d.twist.linear.z = 0;
      cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
      cmd_vel_2d.header.stamp = now();
      if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
        failure_tolerance_ != -1.0)
      {
        throw nav2_core::PlannerException("Controller patience exceeded");
      }
    } else {
      throw nav2_core::PlannerException(e.what());
    }
  }
  RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
  publishVelocity(cmd_vel_2d);

  // Find the closest pose to current pose on global path
  geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;
  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
  if (!nav_2d_utils::transformPose(
          costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose,
          robot_pose_in_path_frame, tolerance))
  {
    throw nav2_core::PlannerException("Failed to transform robot pose to path frame");
  }

  std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
  feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y);

  // Find the closest pose to current pose on global path
  nav_msgs::msg::Path & current_path = current_path_;
  auto find_closest_pose_idx =
    [this,&robot_pose_in_path_frame, &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(
          robot_pose_in_path_frame, current_path.poses[curr_idx]);
        double orient_diff = createYawFromQuat(robot_pose_in_path_frame.pose.orientation) - createYawFromQuat(current_path.poses[curr_idx].pose.orientation);
        if (curr_dist < curr_min_dist && abs(orient_diff) < orient_angle_rad_) {
          curr_min_dist = curr_dist;
          closest_pose_idx = curr_idx;
        }
      }
      return closest_pose_idx;
    };
  const std::size_t closest_pose_idx = find_closest_pose_idx();
  feedback->distance_to_goal =
    nav2_util::geometry_utils::calculate_path_length(current_path_, closest_pose_idx);
  action_server_->publish_feedback(feedback);

}

if (type == ParameterType::PARAMETER_DOUBLE) {
      if (name == "controller_frequency") {
        controller_frequency_ = parameter.as_double();
      } else if (name == "min_x_velocity_threshold") {
        min_x_velocity_threshold_ = parameter.as_double();
      } else if (name == "min_y_velocity_threshold") {
        min_y_velocity_threshold_ = parameter.as_double();
      } else if (name == "min_theta_velocity_threshold") {
        min_theta_velocity_threshold_ = parameter.as_double();
      } else if (name == "orient_angle_rad") {
        orient_angle_rad_ = parameter.as_double();
      } else if (name == "failure_tolerance") {
        failure_tolerance_ = parameter.as_double();
      }
    }

Hope it helps you, sorry I recently found out from Pradheep about this bug otherwise would have posted this long time ago. @thegindi Hope it helps in short time until PR is created. @SteveMacenski sorry to flood your post by dumping the code here.

I will be on this in few days.. but if @thegindi wants to do it, I'd be really glad to leave it to him