Path distance feature

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
@@ -236,6 +242,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_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
@@ -268,6 +275,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;
@@ -613,6 +618,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
     get_logger(), "Path end point is (%.2f, %.2f)",
     end_pose_.pose.position.x, end_pose_.pose.position.y);
 
+  start_index_ = 0;
   current_path_ = path;
 }
 
@@ -750,6 +756,44 @@ 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

@@ -188,6 +188,75 @@ inline int find_next_matching_goal_in_waypoint_statuses(
   return itr - waypoint_statuses.begin();
 }
 
+/**
+ * @brief Find the shortest distance from a point to a line segment
+ *
+ * Uses the closed-form solution for the distance from a point to a segment in 2D.
+ * See: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
+ *
+ * @param point The point to measure from
+ * @param start The start pose of the segment
+ * @param end The end pose of the segment
+ * @return double The shortest distance
+ */
+inline double distance_to_segment(
+  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 dx_seg = b.x - a.x;
+  const double dy_seg = b.y - a.y;
+
+  const double seg_len_sq = (dx_seg * dx_seg) + (dy_seg * dy_seg);
+
+  if (seg_len_sq <= 1e-9) {
+    return euclidean_distance(point, a);
+  }
+
+  const double dot = ((p.x - a.x) * dx_seg) + ((p.y - a.y) * dy_seg);
+  const double t = std::clamp(dot / seg_len_sq, 0.0, 1.0);
+
+  const double proj_x = a.x + t * dx_seg;
+  const double proj_y = a.y + t * dy_seg;
+
+  const double dx_proj = p.x - proj_x;
+  const double dy_proj = p.y - proj_y;
+  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/include/nav2_util/path_utils.hpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__PATH_UTILS_HPP_
+#define NAV2_UTIL__PATH_UTILS_HPP_
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
+namespace nav2_util
+{
+
+/**
+ * @brief Result of searching for the closest segment on a path.
+ */
+struct PathSearchResult
+{
+  double distance;
+  size_t closest_segment_index;
+};
+/**
+ * @brief Finds the minimum distance from a robot pose to a path segment.
+ *
+ * This function searches for the closest segment on the given path to the robot's pose,
+ * starting from a specified index and optionally limiting the search to a window length.
+ * It returns the minimum distance found and the index of the closest segment.
+ *
+ * @param path The path to search (sequence of poses).
+ * @param robot_pose The robot's current pose in pose form.
+ * @param start_index The index in the path to start searching from.
+ * @param search_window_length The maximum length (in meters) to search along the path.
+ * @return PathSearchResult Struct containing the minimum distance and the index of the closest segment.
+ */
+PathSearchResult distance_from_path(
+  const nav_msgs::msg::Path & path,
+  const geometry_msgs::msg::Pose & robot_pose,
+  const size_t start_index = 0,
+  const double search_window_length = std::numeric_limits<double>::max());
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__PATH_UTILS_HPP_

nav2_util/src/CMakeLists.txt

@@ -6,6 +6,7 @@ add_library(${library_name} SHARED
   controller_utils.cpp
   odometry_utils.cpp
   array_parser.cpp
+  path_utils.cpp
 )
 target_include_directories(${library_name}
   PUBLIC

nav2_util/src/path_utils.cpp

@@ -0,0 +1,77 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_util/path_utils.hpp"
+
+#include <limits>
+#include <cmath>
+#include <stdexcept>
+
+#include "nav2_util/geometry_utils.hpp"
+namespace nav2_util
+{
+
+PathSearchResult distance_from_path(
+  const nav_msgs::msg::Path & path,
+  const geometry_msgs::msg::Pose & robot_pose,
+  const size_t start_index,
+  const double search_window_length)
+{
+  PathSearchResult result;
+  result.closest_segment_index = start_index;
+  result.distance = std::numeric_limits<double>::max();
+
+  if (path.poses.empty()) {
+    return result;
+  }
+
+  if (path.poses.size() == 1) {
+    result.distance = nav2_util::geometry_utils::euclidean_distance(
+    robot_pose, path.poses.front().pose);
+    result.closest_segment_index = 0;
+    return result;
+  }
+
+  if (start_index >= path.poses.size() - 1) {
+    throw std::invalid_argument(
+      "Invalid operation: 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;
+  for (size_t i = start_index; i < path.poses.size() - 1; ++i) {
+    if (distance_traversed > search_window_length) {
+      break;
+    }
+
+    const double current_distance = geometry_utils::distance_to_segment(
+      robot_pose.position,
+      path.poses[i].pose,
+      path.poses[i + 1].pose);
+
+    if (current_distance < result.distance) {
+      result.distance = current_distance;
+      result.closest_segment_index = i;
+    }
+
+    distance_traversed += geometry_utils::euclidean_distance(
+      path.poses[i],
+      path.poses[i + 1]);
+  }
+
+  return result;
+}
+
+}  // namespace nav2_util

nav2_util/test/CMakeLists.txt

@@ -32,3 +32,6 @@ target_link_libraries(test_twist_publisher ${library_name} rclcpp::rclcpp ${geom
 
 ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp)
 target_link_libraries(test_twist_subscriber ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS})
+
+ament_add_gtest(test_path_utils test_path_utils.cpp)
+target_link_libraries(test_path_utils ${library_name} ${nav_2d_msgs_TARGETS} ${geometry_msgs_TARGETS})