Added TrackingError.msg

Erased nav_2d_msgs from cmake
Added publishing func to controller server

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

Author: silanus23

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -29,10 +29,12 @@
 #include "tf2_ros/transform_listener.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_msgs/msg/speed_limit.hpp"
+#include "nav2_msgs/msg/tracking_error.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/odometry_utils.hpp"
+#include "nav2_util/path_utils.hpp"
 #include "nav2_util/twist_publisher.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -167,6 +169,10 @@ class ControllerServer : public nav2::LifecycleNode
    * action server
    */
   void updateGlobalPath();
+  /**
+   * @brief Calculates and publishes the TrackingError's all components
+   */
+  void publishTrackingState();
   /**
    * @brief Calls velocity publisher to publish the velocity on "cmd_vel" topic
    * @param velocity Twist velocity to be published
@@ -227,6 +233,7 @@ class ControllerServer : public nav2::LifecycleNode
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::mutex dynamic_params_lock_;
+  std::mutex path_mutex_;
 
   // The controller needs a costmap node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
@@ -236,7 +243,8 @@ 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_;
+  
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
   ProgressCheckerMap progress_checkers_;
@@ -268,6 +276,8 @@ class ControllerServer : public nav2::LifecycleNode
   double min_x_velocity_threshold_;
   double min_y_velocity_threshold_;
   double min_theta_velocity_threshold_;
+  double search_window_ ;
+  int start_index_ ;
 
   double failure_tolerance_;
   bool use_realtime_priority_;

nav2_controller/src/controller_server.cpp

@@ -55,6 +55,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
   declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
   declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
+  declare_parameter("search_window", rclcpp::ParameterValue(5.0));
 
   declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
 
@@ -134,6 +135,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("failure_tolerance", failure_tolerance_);
   get_parameter("use_realtime_priority", use_realtime_priority_);
   get_parameter("publish_zero_velocity", publish_zero_velocity_);
+  get_parameter("search_window", search_window_);
 
   costmap_ros_->configure();
   // Launch a thread to run the costmap node
@@ -225,6 +227,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration, odom_topic);
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
+  tracking_error_pub_ = create_publisher<nav2_msgs::msg::TrackingError>("tracking_error", 10);
 
   double costmap_update_timeout_dbl;
   get_parameter("costmap_update_timeout", costmap_update_timeout_dbl);
@@ -500,6 +503,8 @@ void ControllerServer::computeControl()
 
       computeAndPublishVelocity();
 
+      publishTrackingState();
+
       if (isGoalReached()) {
         RCLCPP_INFO(get_logger(), "Reached the goal!");
         break;
@@ -604,16 +609,18 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
     throw nav2_core::InvalidPath("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);
-
+    
+  start_index_ = 0;
   current_path_ = path;
+
 }
 
 void ControllerServer::computeAndPublishVelocity()
@@ -750,6 +757,42 @@ void ControllerServer::updateGlobalPath()
   }
 }
 
+void ControllerServer::publishTrackingState()
+{
+  if (current_path_.poses.size() < 2) {
+    RCLCPP_DEBUG(get_logger(), "Path has fewer than 2 points, cannot compute tracking error.");
+    return;
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!getRobotPose(robot_pose)) {
+    RCLCPP_WARN(get_logger(), "Failed to obtain robot pose, skipping tracking error publication.");
+    return;
+  }
+
+  const double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(robot_pose, end_pose_);
+  const auto path_search_result = nav2_util::distance_from_path(current_path_, robot_pose.pose, start_index_, search_window_);
+  const size_t closest_idx = path_search_result.closest_segment_index;
+  start_index_ = closest_idx;
+
+  const auto& segment_start = current_path_.poses[closest_idx];
+  const auto& segment_end = current_path_.poses[closest_idx + 1];
+  
+  double cross_product = nav2_util::geometry_utils::cross_product_2d(
+  robot_pose.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);
+}
+
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
   auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>(velocity);

nav2_msgs/CMakeLists.txt

@@ -37,6 +37,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/msg/TrackingError.msg

@@ -0,0 +1,8 @@
+# Real-time tracking error for Nav2 controller
+
+std_msgs/Header header
+float32 tracking_error
+uint32 last_index
+float32 cross_product
+geometry_msgs/PoseStamped robot_pose
+float32 distance_to_goal

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -228,6 +228,35 @@ inline double distance_to_segment(
   return std::hypot(dx_proj, dy_proj);
 }
 
+/**
+ * @brief Computes the 2D cross product between the vector from start to end and the vector from start to point. 
+ * The sign of this calculation's result can be used to determine which side are you on of the track. 
+ * 
+ * See: https://en.wikipedia.org/wiki/Cross_product
+ * 
+ * @param point The point to check relative to the segment.
+ * @param start The starting pose of the segment.
+ * @param end The ending pose of the segment.
+ * @return The signed 2D cross product value.
+ */
+inline double cross_product_2d(
+  const geometry_msgs::msg::Point & point,
+  const geometry_msgs::msg::Pose & start,
+  const geometry_msgs::msg::Pose & end)
+{
+  const auto & p = point;
+  const auto & a = start.position;
+  const auto & b = end.position;
+
+  const double path_vec_x = b.x - a.x;
+  const double path_vec_y = b.y - a.y;
+
+  const double robot_vec_x = p.x - a.x;
+  const double robot_vec_y = p.y - a.y;
+
+  return  (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util
 

nav2_util/src/CMakeLists.txt

@@ -18,7 +18,6 @@ target_link_libraries(${library_name} PUBLIC
   ${lifecycle_msgs_TARGETS}
   ${nav2_msgs_TARGETS}
   ${nav_msgs_TARGETS}
-  ${nav_2d_msgs_TARGETS}
   ${rcl_interfaces_TARGETS}
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action