Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
thegindi opened this issue May 7, 2024 · 27 comments · May be fixed by #4789
Open

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

thegindi opened this issue May 7, 2024 · 27 comments · May be fixed by #4789

Comments

@thegindi
Copy link

thegindi commented May 7, 2024

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 #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

@SteveMacenski
Copy link
Member

SteveMacenski commented May 7, 2024

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
Copy link

KSorte commented May 9, 2024

is this issue being worked on still?

@SteveMacenski
Copy link
Member

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
Copy link
Author

thegindi commented May 9, 2024

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
Copy link

KSorte commented May 10, 2024

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
Copy link
Member

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
Copy link
Author

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

Sounds good to me!

@KSorte
Copy link

KSorte commented May 14, 2024

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
Copy link
Member

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
Copy link

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
Copy link
Member

SteveMacenski commented May 15, 2024

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
Copy link

KSorte commented May 16, 2024

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
Copy link
Member

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
Copy link

KSorte commented May 16, 2024

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
Copy link
Member

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
Copy link

KSorte commented May 16, 2024

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
Copy link
Member

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
Copy link

KSorte commented May 16, 2024

Sounds like a plan to me!

@thegindi
Copy link
Author

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
Copy link

KSorte commented May 17, 2024

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
Copy link

KSorte commented May 17, 2024

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.

@Kaiser-Wang
Copy link

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?

@josephduchesne josephduchesne linked a pull request Dec 10, 2024 that will close this issue
11 tasks
@SteveMacenski SteveMacenski linked a pull request Dec 10, 2024 that will close this issue
11 tasks
@anath93
Copy link

anath93 commented May 9, 2025

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

@SteveMacenski
Copy link
Member

SteveMacenski commented May 9, 2025

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 !

@anath93
Copy link

anath93 commented May 9, 2025

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

@anath93
Copy link

anath93 commented May 22, 2025

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:

Image

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

Image

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.

@padhupradheep
Copy link
Member

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging a pull request may close this issue.

7 participants