Controller FollowPath feedback: Fix wrong frame in distance calculation, add more info to feedback

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -13,7 +13,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
-      <input_port name="disable_collision_checks">Disable collision checking</input_port>
+      <input_port name="disable_collision_checks" type="bool" default="false">Disable collision checking</input_port>
       <output_port name="error_code_id">"Back up error code"</output_port>
     </Action>
 
@@ -126,7 +126,7 @@
       <input_port name="path">Path to follow</input_port>
       <input_port name="goal_checker_id">Goal checker</input_port>
       <input_port name="progress_checker_id">Progress checker</input_port>
-      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_name">Action server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">Follow Path error code</output_port>
     </Action>

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -186,7 +186,7 @@ class ControllerServer : public nav2_util::LifecycleNode
    */
   bool isGoalReached();
   /**
-   * @brief Obtain current pose of the robot
+   * @brief Obtain current pose of the robot in costmap's frame
    * @param pose To store current pose of the robot
    * @return true if able to obtain current pose of the robot, else false
    */

nav2_controller/src/controller_server.cpp

@@ -659,18 +659,32 @@
     }
   }
 
+  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))
+  {
+    RCLCPP_ERROR(get_logger(), "Robot pose is not available.");
+    return;
+  }
+
   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 =
-    [&pose, &current_path]() {
+  auto find_closest_pose_idx = [&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(
-          pose, current_path.poses[curr_idx]);
+        double curr_dist =
+          nav2_util::geometry_utils::euclidean_distance(robot_pose_in_path_frame,
+          current_path.poses[curr_idx]);
         if (curr_dist < curr_min_dist) {
           curr_min_dist = curr_dist;
           closest_pose_idx = curr_idx;
@@ -679,12 +693,14 @@
       return closest_pose_idx;
     };
 
-  feedback->distance_to_goal =
-    nav2_util::geometry_utils::calculate_path_length(current_path_, find_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);
+  feedback->closest_path_pose_idx = closest_pose_idx;
+  feedback->distance_to_path =
+    nav2_util::geometry_utils::euclidean_distance(robot_pose_in_path_frame,
+      current_path.poses[closest_pose_idx]);
   action_server_->publish_feedback(feedback);
-
-  RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
-  publishVelocity(cmd_vel_2d);
 }
 
 void ControllerServer::updateGlobalPath()

nav2_msgs/action/FollowPath.action

@@ -24,4 +24,6 @@ string error_msg
 ---
 #feedback definition
 float32 distance_to_goal
+uint64 closest_path_pose_idx   # index of the pose on global path closest to current robot pose
+float32 distance_to_path
 float32 speed