handled last_fixes

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -241,7 +241,7 @@ class ControllerServer : public nav2::LifecycleNode
   std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
   nav2::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
-    rclcpp::Publisher<nav2_msgs::msg::TrackingError>::SharedPtr tracking_error_pub_;
+  nav2::Publisher<nav2_msgs::msg::TrackingError>::SharedPtr tracking_error_pub_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
@@ -291,6 +291,9 @@ class ControllerServer : public nav2::LifecycleNode
   // Current path container
   nav_msgs::msg::Path current_path_;
 
+  // Last tracking error
+  double signed_distance_ = 0;
+
 private:
   /**
     * @brief Callback for speed limiting messages

nav2_controller/src/controller_server.cpp

@@ -704,6 +704,7 @@ void ControllerServer::computeAndPublishVelocity()
   feedback->distance_to_goal = nav2_util::geometry_utils::calculate_path_length(current_path_,
       closest_pose_idx);
   action_server_->publish_feedback(feedback);
+  feedback->tracking_error = signed_distance_;
 }
 
 void ControllerServer::updateGlobalPath()
@@ -786,9 +787,6 @@ void ControllerServer::publishTrackingState()
     return;
   }
 
-  const double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
-    robot_pose, end_pose_in_robot_frame);
-
   const auto path_search_result = nav2_util::distance_from_path(
     current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_);
 
@@ -802,16 +800,14 @@ void ControllerServer::publishTrackingState()
   double cross_product = nav2_util::geometry_utils::cross_product_2d(
     robot_pose_in_path_frame.pose.position, segment_start.pose, segment_end.pose);
 
-  nav2_msgs::msg::TrackingError tracking_error_msg;
-  tracking_error_msg.header.stamp = now();
-  tracking_error_msg.header.frame_id = robot_pose.header.frame_id;
-  tracking_error_msg.tracking_error = path_search_result.distance;
-  tracking_error_msg.last_index = closest_idx;
-  tracking_error_msg.cross_product = cross_product;
-  tracking_error_msg.distance_to_goal = distance_to_goal;
-  tracking_error_msg.robot_pose = robot_pose;
-
-  tracking_error_pub_->publish(tracking_error_msg);
+  signed_distance_ = path_search_result.distance * (cross_product >= 0.0 ? 1.0 : -1.0);
+  auto tracking_error_msg = std::make_unique<nav2_msgs::msg::TrackingError>();
+  tracking_error_msg->header.stamp = now();
+  tracking_error_msg->header.frame_id = robot_pose.header.frame_id;
+  tracking_error_msg->tracking_error = signed_distance_;
+  tracking_error_msg->current_path_index = closest_idx;
+  tracking_error_msg->robot_pose = robot_pose;
+  tracking_error_pub_->publish(std::move(tracking_error_msg));
 }
 
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)

nav2_msgs/CMakeLists.txt

@@ -39,6 +39,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/RouteNode.msg"
   "msg/RouteEdge.msg"
   "msg/EdgeCost.msg"
+  "msg/TrackingError.msg"
   "msg/Trajectory.msg"
   "msg/TrajectoryPoint.msg"
   "srv/GetCosts.srv"

nav2_msgs/action/FollowPath.action

@@ -25,3 +25,4 @@ string error_msg
 #feedback definition
 float32 distance_to_goal
 float32 speed
+float32 tracking_error

nav2_msgs/msg/TrackingError.msg

@@ -1,8 +1,8 @@
 # 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
+# Positive sign indicates the robot is to the left of the path, negative to the right
 float32 tracking_error
-uint32 last_index
-float32 cross_product
+uint32 current_path_index
 geometry_msgs/PoseStamped robot_pose
-float32 distance_to_goal