Path distance feature

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -28,6 +28,7 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "tf2_ros/transform_listener.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_msgs/msg/tracking_feedback.hpp"
 #include "nav2_msgs/msg/speed_limit.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
@@ -236,6 +237,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_;
+  nav2::Publisher<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_pub_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
@@ -268,6 +270,8 @@ class ControllerServer : public nav2::LifecycleNode
   double min_x_velocity_threshold_;
   double min_y_velocity_threshold_;
   double min_theta_velocity_threshold_;
+  double search_window_;
+  size_t start_index_;
 
   double failure_tolerance_;
   bool use_realtime_priority_;
@@ -277,6 +281,8 @@ class ControllerServer : public nav2::LifecycleNode
   // Whether we've published the single controller warning yet
   geometry_msgs::msg::PoseStamped end_pose_;
 
+  geometry_msgs::msg::PoseStamped transformed_end_pose_;
+
   // Last time the controller generated a valid command
   rclcpp::Time last_valid_cmd_time_;
 

nav2_controller/src/controller_server.cpp

@@ -23,6 +23,7 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/path_utils.hpp"
 #include "nav2_controller/controller_server.hpp"
 
 using namespace std::chrono_literals;
@@ -43,6 +44,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   lp_loader_("nav2_core", "nav2_core::Controller"),
   default_ids_{"FollowPath"},
   default_types_{"dwb_core::DWBLocalPlanner"},
+  start_index_(0),
   costmap_update_timeout_(300ms)
 {
   RCLCPP_INFO(get_logger(), "Creating controller server");
@@ -65,6 +67,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 
   declare_parameter("odom_topic", rclcpp::ParameterValue("odom"));
   declare_parameter("odom_duration", rclcpp::ParameterValue(0.3));
+  declare_parameter("search_window", rclcpp::ParameterValue(2.0));
 
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
@@ -134,6 +137,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 +229,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_feedback_pub_ = create_publisher<nav2_msgs::msg::TrackingFeedback>("tracking_feedback");
 
   double costmap_update_timeout_dbl;
   get_parameter("costmap_update_timeout", costmap_update_timeout_dbl);
@@ -267,8 +272,8 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
     it->second->activate();
   }
   vel_publisher_->on_activate();
+  tracking_feedback_pub_->on_activate();
   action_server_->activate();
-
   auto node = shared_from_this();
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -302,6 +307,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
+  tracking_feedback_pub_->on_deactivate();
 
   remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
@@ -613,6 +619,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;
 }
 
@@ -667,38 +674,58 @@ 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;
-  if (!nav2_util::transformPoseInTargetFrame(
-          pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(),
-          current_path_.header.frame_id, costmap_ros_->getTransformTolerance()))
+  nav2_msgs::msg::TrackingFeedback current_tracking_feedback;
+
+  // Use the current robot pose's timestamp for the transformation
+  end_pose_.header.stamp = pose.header.stamp;
+
+  if (nav2_util::transformPoseInTargetFrame(
+      end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(),
+      costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()))
   {
-    throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame");
+    // Transform succeeded
+  } else {
+    RCLCPP_WARN(get_logger(), "Failed to transform end pose to global 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);
+  if (current_path_.poses.size() >= 2) {
+    double current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
+    pose, transformed_end_pose_);
 
-  nav_msgs::msg::Path & current_path = current_path_;
-  auto find_closest_pose_idx = [&robot_pose_in_path_frame, &current_path]()
+    // Transform robot pose to path frame for path tracking calculations
+    geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;
+    if (!nav2_util::transformPoseInTargetFrame(
+      pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(),
+            current_path_.header.frame_id, costmap_ros_->getTransformTolerance()))
     {
-      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(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;
-        }
-      }
-      return closest_pose_idx;
-    };
+      throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame");
+    }
+
+    const auto path_search_result = nav2_util::distance_from_path(
+      current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_);
+
+    // 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->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;
+    tracking_feedback_msg->speed = std::hypot(twist.linear.x, twist.linear.y);
+    tracking_feedback_msg->remaining_path_length =
+      nav2_util::geometry_utils::calculate_path_length(current_path_, start_index_);
+    start_index_ = path_search_result.closest_segment_index;
+
+    // Update current tracking error and publish
+    current_tracking_feedback = *tracking_feedback_msg;
+    if (tracking_feedback_pub_->get_subscription_count() > 0) {
+      tracking_feedback_pub_->publish(std::move(tracking_feedback_msg));
+    }
+  }
 
-  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);
+  // Publish action feedback
+  std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
+  feedback->tracking_feedback = current_tracking_feedback;
   action_server_->publish_feedback(feedback);
 }
 
@@ -798,14 +825,8 @@ bool ControllerServer::isGoalReached()
 
   geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist());
 
-  geometry_msgs::msg::PoseStamped transformed_end_pose;
-  end_pose_.header.stamp = pose.header.stamp;
-  nav2_util::transformPoseInTargetFrame(
-    end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(),
-    costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance());
-
   return goal_checkers_[current_goal_checker_]->isGoalReached(
-    pose.pose, transformed_end_pose.pose,
+    pose.pose, transformed_end_pose_.pose,
     velocity);
 }
 

nav2_msgs/CMakeLists.txt

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

nav2_msgs/action/FollowPath.action

@@ -1,10 +1,10 @@
-#goal definition
+# goal definition
 nav_msgs/Path path
 string controller_id
 string goal_checker_id
 string progress_checker_id
 ---
-#result definition
+# result definition
 
 # Error codes
 # Note: The expected priority order of the errors should match the message order
@@ -22,6 +22,7 @@ std_msgs/Empty result
 uint16 error_code
 string error_msg
 ---
-#feedback definition
-float32 distance_to_goal
-float32 speed
+# 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
+nav2_msgs/TrackingFeedback tracking_feedback

nav2_msgs/msg/TrackingFeedback.msg

@@ -0,0 +1,11 @@
+# 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 current_path_index
+geometry_msgs/PoseStamped robot_pose
+float32 distance_to_goal
+float32 speed
+float32 remaining_path_length

nav2_simple_commander/nav2_simple_commander/example_follow_path.py

@@ -69,9 +69,9 @@ def main() -> None:
         if feedback and i % 5 == 0:
             print(
                 'Estimated distance remaining to goal position: '
-                + f'{feedback.distance_to_goal:.3f}'
+                + f'{feedback.tracking_feedback.distance_to_goal:.3f}'
                 + '\nCurrent speed of the robot: '
-                + f'{feedback.speed:.3f}'
+                + f'{feedback.tracking_feedback.speed:.3f}'
             )
 
     # Do something depending on the return code

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -231,6 +231,89 @@ inline bool isPointInsidePolygon(
   return res;
 }
 
+/**
+ * @brief Find the distance to a point
+ * @param global_pose Robot's current or planned position
+ * @param target target point
+ * @return int
+ */
+inline double distanceToPoint(
+  const geometry_msgs::msg::PoseStamped & point1,
+  const geometry_msgs::msg::PoseStamped & point2)
+{
+  const double dx = point1.pose.position.x - point2.pose.position.x;
+  const double dy = point1.pose.position.y - point2.pose.position.y;
+  return std::hypot(dx, dy);
+}
+
+/**
+ * @brief Find the shortest distance to a vector
+ *
+ * See: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
+ *
+ * @param global_pose Robot's current or planned position
+ * @param start Starting segment of the path
+ * @param finish End segment of the path
+ * @return int
+ */
+inline double distance_to_path_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