Add heading error in controller server

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -209,7 +209,8 @@ NavigateThroughPosesNavigator::onLoop()
   res = blackboard->get(
     tracking_feedback_blackboard_id_,
     tracking_feedback);
-  feedback_msg->tracking_error = tracking_feedback.tracking_error;
+  feedback_msg->position_tracking_error = tracking_feedback.position_tracking_error;
+  feedback_msg->heading_tracking_error = tracking_feedback.heading_tracking_error;
 
   bt_action_server_->publishFeedback(feedback_msg);
 }
@@ -290,10 +291,13 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
   blackboard->set("number_recoveries", 0);  // NOLINT
   previous_path_ = nav_msgs::msg::Path();
 
-  // Update the goal pose on the blackboard
+  // Update the goal pose and path on the blackboard
   blackboard->set<nav_msgs::msg::Goals>(
     goals_blackboard_id_,
     std::move(goals_array));
+  blackboard->set<nav_msgs::msg::Path>(
+    path_blackboard_id_,
+    nav_msgs::msg::Path());
 
   // Reset the waypoint states vector in the blackboard
   std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(goals_array.goals.size());

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -189,7 +189,8 @@ NavigateToPoseNavigator::onLoop()
   res = blackboard->get(
     tracking_feedback_blackboard_id_,
     tracking_feedback);
-  feedback_msg->tracking_error = tracking_feedback.tracking_error;
+  feedback_msg->position_tracking_error = tracking_feedback.position_tracking_error;
+  feedback_msg->heading_tracking_error = tracking_feedback.heading_tracking_error;
 
   bt_action_server_->publishFeedback(feedback_msg);
 }
@@ -266,8 +267,9 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
   blackboard->set("number_recoveries", 0);  // NOLINT
   previous_path_ = nav_msgs::msg::Path();
 
-  // Update the goal pose on the blackboard
+  // Update the goal pose and path on the blackboard
   blackboard->set(goal_blackboard_id_, goal_pose);
+  blackboard->set(path_blackboard_id_, nav_msgs::msg::Path());
 
   return true;
 }

nav2_controller/src/controller_server.cpp

@@ -731,15 +731,32 @@ void ControllerServer::computeAndPublishVelocity()
       throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame");
     }
 
-
-    //
+    // Calculate closest point and position error from path
     const auto path_search_result = nav2_util::distance_from_path(
       current_path_, robot_pose_in_path_frame.pose, start_index_, params_->search_window);
 
+    // Calculate heading error
+    double heading_tracking_error = 0.0;
+    if (path_search_result.closest_segment_index <
+      current_path_.poses.size() - 1)
+    {
+      const auto & path_segment_start =
+        current_path_.poses[path_search_result.closest_segment_index].pose;
+      const auto & path_segment_end =
+        current_path_.poses[path_search_result.closest_segment_index + 1].pose;
+      double path_yaw = std::atan2(
+        path_segment_end.position.y - path_segment_start.position.y,
+        path_segment_end.position.x - path_segment_start.position.x);
+      double robot_yaw = tf2::getYaw(robot_pose_in_path_frame.pose.orientation);
+      heading_tracking_error = angles::shortest_angular_distance(
+        robot_yaw, path_yaw);
+    }
+
     // Create tracking error message
     auto tracking_feedback_msg = std::make_unique<nav2_msgs::msg::TrackingFeedback>();
     tracking_feedback_msg->header = pose.header;
-    tracking_feedback_msg->tracking_error = path_search_result.distance;
+    tracking_feedback_msg->position_tracking_error = path_search_result.distance;
+    tracking_feedback_msg->heading_tracking_error = heading_tracking_error;
     tracking_feedback_msg->current_path_index = path_search_result.closest_segment_index;
     tracking_feedback_msg->robot_pose = pose;
     tracking_feedback_msg->distance_to_goal = current_distance_to_goal;

nav2_msgs/action/FollowPath.action

@@ -25,5 +25,6 @@ string error_msg
 ---
 # feedback definition
 # Real-time tracking error indicating which side of the path the robot is on
-# If the tracking_error is positive, the robot is to the left of the path
+# If the position_tracking_error is positive, the robot is to the left of the path
+# If the heading_tracking_error is positive, the robot heading is rotated to the right of the path direction
 nav2_msgs/TrackingFeedback tracking_feedback

nav2_msgs/action/NavigateThroughPoses.action

@@ -23,6 +23,7 @@ builtin_interfaces/Duration navigation_time
 builtin_interfaces/Duration estimated_time_remaining
 int16 number_of_recoveries
 float32 distance_remaining
-float32 tracking_error
+float32 position_tracking_error
+float32 heading_tracking_error
 int16 number_of_poses_remaining
 WaypointStatus[] waypoint_statuses

nav2_msgs/action/NavigateToPose.action

@@ -21,4 +21,5 @@ builtin_interfaces/Duration navigation_time
 builtin_interfaces/Duration estimated_time_remaining
 int16 number_of_recoveries
 float32 distance_remaining
-float32 tracking_error
+float32 position_tracking_error
+float32 heading_tracking_error

nav2_msgs/msg/TrackingFeedback.msg

@@ -1,9 +1,14 @@
 # Real-time tracking error for Nav2 controller
 
 std_msgs/Header header
-# The sign of the tracking error indicates which side of the path the robot is on
+# The sign of the position tracking error indicates which side of the path the robot is on
 # Positive sign indicates the robot is to the left of the path, negative to the right
-float32 tracking_error
+float32 position_tracking_error
+# The sign of the heading tracking error indicates the angular difference between
+# the robot's heading and the path.
+# Positive sign means the robot heading is rotated to the right of the path direction,
+# negative sign means it is rotated to the left.
+float32 heading_tracking_error
 uint32 current_path_index
 geometry_msgs/PoseStamped robot_pose
 float32 distance_to_goal

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -1083,9 +1083,6 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
       syncTabsWithAccumulatedPoses();  // Sync tabs with new pose
     } else {
       acummulated_poses_ = nav_msgs::msg::Goals();
-      // Reset goal index for single goal navigation
-      goal_index_ = 0;
-      store_poses_.goals.push_back(pose);
       updateWpNavigationMarkers();
       std::cout << "Start navigation" << std::endl;
       startNavigation(pose);
@@ -1629,8 +1626,10 @@ inline std::string Nav2Panel::toLabel(T & msg)
     toString(rclcpp::Duration(msg.estimated_time_remaining).seconds(), 0) + " s"
     "</td></tr><tr><td width=150>Distance remaining:</td><td>" +
     toString(msg.distance_remaining, 2) + " m"
-    "</td></tr><tr><td width=150>Tracking error:</td><td>" +
-    toString(msg.tracking_error, 2) + " m"
+    "</td></tr><tr><td width=150>Position error:</td><td>" +
+    toString(msg.position_tracking_error, 2) + " m"
+    "</td></tr><tr><td width=150>Heading error:</td><td>" +
+    toString(msg.heading_tracking_error, 2) + " rad"
     "</td></tr><tr><td width=150>Time taken:</td><td>" +
     toString(rclcpp::Duration(msg.navigation_time).seconds(), 0) + " s"
     "</td></tr><tr><td width=150>Recoveries:</td><td>" +

nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py

@@ -116,11 +116,17 @@ def main() -> None:
             )
 
             print(
-                'Tracking error: '
-                + '{:.2f}'.format(feedback.tracking_error)
+                'Position error: '
+                + '{:.2f}'.format(feedback.position_tracking_error)
                 + ' meters.'
             )
 
+            print(
+                'Heading error: '
+                + '{:.2f}'.format(feedback.heading_tracking_error)
+                + ' radians.'
+            )
+
             # Some navigation timeout to demo cancellation
             if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
                 navigator.cancelTask()

nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py

@@ -96,11 +96,17 @@ def main() -> None:
             )
 
             print(
-                'Tracking error: '
-                + '{:.2f}'.format(feedback.tracking_error)
+                'Position error: '
+                + '{:.2f}'.format(feedback.position_tracking_error)
                 + ' meters.'
             )
 
+            print(
+                'Heading error: '
+                + '{:.2f}'.format(feedback.heading_tracking_error)
+                + ' radians.'
+            )
+
             # Some navigation timeout to demo cancellation
             if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
                 navigator.cancelTask()

nav2_util/src/path_utils.cpp

@@ -45,9 +45,9 @@ PathSearchResult distance_from_path(
 
   if (start_index >= path.poses.size()) {
     throw std::runtime_error(
-            "Invalid operation: requested start index (" + std::to_string(start_index) +
+            "Requested start index (" + std::to_string(start_index) +
             ") is greater than or equal to path size (" + std::to_string(path.poses.size()) +
-            "). Application is not properly managing state.");
+            ").");
   }
 
   double distance_traversed = 0.0;