Controller FollowPath feedback: Fix wrong frame in distance calculation, add more info to feedback (#4931)

* Add more info to feedback, fix wrong frame in distance calculation

Signed-off-by: Adi Vardi <[email protected]>

* Write FollowPath Feedback to blackboard

Signed-off-by: Adi Vardi <[email protected]>

* first publish velocity, then try to publish feedback

also remove debug prints

Signed-off-by: Adi Vardi <[email protected]>

* Revert "Write FollowPath Feedback to blackboard"

This reverts commit 1ee9ec1.

Signed-off-by: Adi Vardi <[email protected]>

* small fixes to na2_tree_nodes.xml

Signed-off-by: Adi Vardi <[email protected]>

* fix formatting issue

Signed-off-by: Adi Vardi <[email protected]>

* remove additional feedback fields from FollowPath action

Signed-off-by: Adi Vardi <[email protected]>

* throw if transform failed

Signed-off-by: Adi Vardi <[email protected]>

---------

Signed-off-by: Adi Vardi <[email protected]>

Author: adivardi

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>
       <output_port name="error_msg">"Back up error message"</output_port>
     </Action>
@@ -131,7 +131,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>
       <output_port name="error_msg">Follow Path error message</output_port>

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

@@ -668,18 +668,31 @@ void ControllerServer::computeAndPublishVelocity()
     }
   }
 
+  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::ControllerTFError("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 =
-    [&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;
@@ -688,12 +701,10 @@ void ControllerServer::computeAndPublishVelocity()
       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);
   action_server_->publish_feedback(feedback);
-
-  RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
-  publishVelocity(cmd_vel_2d);
 }
 
 void ControllerServer::updateGlobalPath()