diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index b68c8e5bb94..a4e33a4249b 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/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 odom_sub_; std::unique_ptr vel_publisher_; nav2::Subscription::SharedPtr speed_limit_sub_; + nav2::Publisher::SharedPtr tracking_feedback_pub_; // Progress Checker Plugin pluginlib::ClassLoader 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_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 98a6eda8629..1fb7ca23273 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/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( @@ -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(node, odom_duration, odom_topic); vel_publisher_ = std::make_unique(node, "cmd_vel"); + tracking_feedback_pub_ = create_publisher("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,56 @@ 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; + 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( - pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), - current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) + 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"); + throw nav2_core::ControllerTFError("Failed to transform end pose to global frame"); } - std::shared_ptr feedback = std::make_shared(); - 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, ¤t_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::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(); + 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 feedback = std::make_shared(); + feedback->tracking_feedback = current_tracking_feedback; action_server_->publish_feedback(feedback); } @@ -798,14 +823,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); } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index b44e08ccb3a..76523e1a3a5 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/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" diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 80b0a1d4efc..48c4b59d079 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/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 diff --git a/nav2_msgs/msg/TrackingFeedback.msg b/nav2_msgs/msg/TrackingFeedback.msg new file mode 100644 index 00000000000..a4b6a554d33 --- /dev/null +++ b/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 diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index c423f3a23dd..a2aae33262f 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/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 diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index d67f17e3c7c..f4cf4078042 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -231,6 +231,78 @@ inline bool isPointInsidePolygon( return res; } +/** + * @brief Computes the shortest (perpendicular) distance from a point to a path segment. + * + * Given a point and a line segment defined by two poses, calculates the minimum distance + * from the point to the segment in the XY plane. If the segment is too short, + * returns the distance from the point to the segment's start. + * + * See: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line + * + * @param point The point to measure from (geometry_msgs::msg::Point). + * @param start The starting pose of the segment (geometry_msgs::msg::Pose). + * @param end The ending pose of the segment (geometry_msgs::msg::Pose). + * @return The shortest distance from the point to the segment in meters. + */ +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 diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp new file mode 100644 index 00000000000..e6c1f2ee642 --- /dev/null +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -0,0 +1,61 @@ +// 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 +#include +#include + +#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 the robot's pose to the closest segment of a path. + * + * This function searches for the closest segment on the given path to the robot's pose. + * By default, it finds the globally nearest path point. Optionally, you can specify + * the index to start searching from (useful for path tracking tasks) and a maximum + * search window length (in meters) to limit the search range. + * + * @param path The path to search (sequence of poses). + * @param robot_pose The robot's current pose. + * @param start_index The index in the path to start searching from (default: 0). + * @param search_window_length The maximum length (in meters) to search along the path (default: unlimited). + * @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::max()); + +} // namespace nav2_util + +#endif // NAV2_UTIL__PATH_UTILS_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 99d24ee6348..d1ee2e86a6e 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/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 diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp new file mode 100644 index 00000000000..9d5975b65eb --- /dev/null +++ b/nav2_util/src/path_utils.cpp @@ -0,0 +1,85 @@ +// 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 +#include +#include + +#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::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()) { + throw std::runtime_error( + "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_path_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]); + } + + const auto & segment_start = path.poses[result.closest_segment_index]; + const auto & segment_end = path.poses[result.closest_segment_index + 1]; + + // Obtain the signed direction of the cross track error + const double cross_product = geometry_utils::cross_product_2d( + robot_pose.position, segment_start.pose, segment_end.pose); + result.distance *= (cross_product >= 0.0 ? 1.0 : -1.0); + + return result; +} + +} // namespace nav2_util diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index b7f290f8e78..68483963be9 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -37,3 +37,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} rclcpp::rclcpp ${geometry_msgs_TARGETS}) diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index e7f954675e2..e5b17678a9c 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -156,3 +156,38 @@ TEST(GeometryUtils, find_next_matching_goal_in_waypoint_statuses) ASSERT_EQ(find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, waypoint_statuses[matching_index].waypoint_pose), -1); } + +TEST(GeometryUtils, cross_product_2d_basic) +{ + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + + geometry_msgs::msg::Pose start; + start.position.x = 0.0; + start.position.y = 0.0; + + geometry_msgs::msg::Pose end; + end.position.x = 2.0; + end.position.y = 0.0; + + // The vector from start to end is (2,0) + // The vector from start to point is (1,2) + // Cross product: (2*2) - (0*1) = 4 + double result = nav2_util::geometry_utils::cross_product_2d(point, start, end); + EXPECT_NEAR(result, 4.0, 1e-6); + + // Now test a point below the segment + point.x = 1.0; + point.y = -2.0; + // Cross product: (2*-2) - (0*1) = -4 + result = nav2_util::geometry_utils::cross_product_2d(point, start, end); + EXPECT_NEAR(result, -4.0, 1e-6); + + // Point on the segment + point.x = 1.0; + point.y = 0.0; + // Cross product: (2*0) - (0*1) = 0 + result = nav2_util::geometry_utils::cross_product_2d(point, start, end); + EXPECT_NEAR(result, 0.0, 1e-6); +} diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp new file mode 100644 index 00000000000..30a16bfb30c --- /dev/null +++ b/nav2_util/test/test_path_utils.cpp @@ -0,0 +1,371 @@ +// 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 +#include +#include + +#include "gtest/gtest.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/path_utils.hpp" + +geometry_msgs::msg::PoseStamped createPoseStamped(double x, double y) +{ + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = 0.0; + pose.pose.orientation.w = 1.0; + return pose; +} + +geometry_msgs::msg::Pose createPose(double x, double y) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = 0.0; + pose.orientation.w = 1.0; + return pose; +} + +void generateCirclePath( + nav_msgs::msg::Path & path, + double center_x, double center_y, double radius, + int num_points, double start_angle = 0.0, double end_angle = 2.0 * M_PI) +{ + const double angle_step = (end_angle - start_angle) / (num_points - 1); + for (int i = 0; i < num_points; ++i) { + const double angle = start_angle + i * angle_step; + path.poses.push_back(createPoseStamped( + center_x + radius * std::cos(angle), + center_y + radius * std::sin(angle))); + } +} + +void generateCircleTrajectory( + std::vector & path, + double center_x, double center_y, double radius, + int num_points, double start_angle = 0.0, double end_angle = 2.0 * M_PI) +{ + const double angle_step = (end_angle - start_angle) / (num_points - 1); + for (int i = 0; i < num_points; ++i) { + const double angle = start_angle + i * angle_step; + path.push_back(createPose( + center_x + radius * std::cos(angle), + center_y + radius * std::sin(angle))); + } +} +class CloverleafPathTest : public ::testing::Test +{ +protected: + void SetUp() override + { + // Target path has three leaves with a radius of 5.0 + generateCirclePath(target_path, 5.0, 0.0, 5.0, 50); + generateCirclePath(target_path, -5.0, 0.0, 5.0, 50); + generateCirclePath(target_path, 0.0, 5.0, 5.0, 50); + + // Robot trajectory now also travels all three leaves, but with a radius of 4.8 + std::vector robot_path; + generateCircleTrajectory(robot_path, 5.0, 0.0, 4.8, 50); + generateCircleTrajectory(robot_path, -5.0, 0.0, 4.8, 50); + generateCircleTrajectory(robot_path, 0.0, 5.0, 4.8, 50); + robot_trajectory = robot_path; + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; +}; + +class RetracingCircleTest : public ::testing::Test +{ +protected: + void SetUp() override + { + generateCirclePath(target_path, 0.0, 0.0, 5.0, 50, 0.0, 2.0 * M_PI); + generateCirclePath(target_path, 0.0, 0.0, 5.0, 50, 2.0 * M_PI, 0.0); + std::vector robot_path; + generateCircleTrajectory(robot_path, 0.0, 0.0, 5.2, 50, 0.0, 2.0 * M_PI); + generateCircleTrajectory(robot_path, 0.0, 0.0, 5.2, 50, 2.0 * M_PI, 0.0); + robot_trajectory = robot_path; + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; +}; + +class ZigZagPathTest : public ::testing::Test +{ +protected: + void SetUp() override + { + target_path.poses.push_back(createPoseStamped(0.0, 0.0)); + target_path.poses.push_back(createPoseStamped(10.0, 0.0)); + target_path.poses.push_back(createPoseStamped(10.0, 5.0)); + target_path.poses.push_back(createPoseStamped(0.0, 5.0)); + target_path.poses.push_back(createPoseStamped(0.0, 10.0)); + + robot_trajectory = { + createPose(1.0, 0.2), createPose(5.0, 0.2), createPose(9.0, 0.2), + createPose(9.8, 1.0), createPose(10.2, 2.5), createPose(9.8, 4.0), + createPose(8.0, 5.2), createPose(5.0, 5.2), createPose(2.0, 5.2), + createPose(0.2, 6.0), createPose(0.2, 8.0), createPose(0.2, 9.8) + }; + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; +}; + +class HairpinTurnTest : public ::testing::Test +{ +protected: + void SetUp() override + { + target_path.poses.push_back(createPoseStamped(0.0, 1.0)); + target_path.poses.push_back(createPoseStamped(10.0, 1.0)); + target_path.poses.push_back(createPoseStamped(10.0, -1.0)); + target_path.poses.push_back(createPoseStamped(0.0, -1.0)); + + robot_trajectory = { + createPose(1.0, 1.2), createPose(3.0, 1.2), createPose(5.0, 1.2), + createPose(7.0, 1.2), createPose(8.5, 1.0), createPose(9.2, 0.5), + createPose(9.8, 0.0), createPose(9.8, -0.5), createPose(9.2, -1.0), + createPose(8.0, -1.2), createPose(6.0, -1.2), createPose(4.0, -1.2), + createPose(2.0, -1.2) + }; + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; +}; + +class CuttingCornerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + target_path.poses.push_back(createPoseStamped(0.0, 0.0)); + target_path.poses.push_back(createPoseStamped(10.0, 0.0)); + target_path.poses.push_back(createPoseStamped(10.0, 10.0)); + + robot_trajectory = { + createPose(0.0, 0.2), createPose(2.0, 0.2), createPose(4.0, 0.2), + createPose(6.0, 0.2), createPose(8.0, 0.2), createPose(9.0, 1.0), + createPose(9.8, 2.0), createPose(9.8, 4.0), createPose(9.8, 6.0), + createPose(9.8, 8.0), createPose(9.8, 10.0) + }; + expected_distances = {0.2, 0.2, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2, 0.2}; + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; + std::vector expected_distances; +}; + +class RetracingPathTest : public ::testing::Test +{ +protected: + void SetUp() override + { + for (int i = 0; i <= 10; ++i) {target_path.poses.push_back(createPoseStamped(i, 0.0));} + for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPoseStamped(i, 0.0));} + for (int i = 0; i <= 10; ++i) { + robot_trajectory.push_back(createPose(i, 0.5)); + } + for (int i = 9; i >= 0; --i) { + robot_trajectory.push_back(createPose(i, 0.5)); + } + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; +}; + +TEST(PathUtilsTest, EmptyPath) +{ + auto robot_pose = createPose(5.0, 5.0); + nav_msgs::msg::Path empty_path; + + auto result = nav2_util::distance_from_path(empty_path, robot_pose); + // Check all fields of the result for empty path + EXPECT_EQ(result.distance, std::numeric_limits::max()); + EXPECT_EQ(result.closest_segment_index, 0); +} + +TEST_F(CuttingCornerTest, TrajectoryCutsCorner) +{ + for (size_t i = 0; i < robot_trajectory.size(); ++i) { + const auto & robot_pose = robot_trajectory[i]; + auto result = nav2_util::distance_from_path(target_path, robot_pose); + EXPECT_NEAR(std::abs(result.distance), expected_distances[i], 0.1); + } +} + +TEST_F(RetracingPathTest, TrajectoryFollowsRetracingPath) +{ + const double expected_distance = 0.5; + + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distance_from_path(target_path, robot_pose); + EXPECT_NEAR(std::abs(result.distance), expected_distance, 1e-6); + } +} + +TEST_F(CloverleafPathTest, TrajectoryFollowsCloverleafLoop) +{ + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distance_from_path(target_path, robot_pose); + EXPECT_LT(std::abs(result.distance), 0.25); + } +} + +TEST_F(RetracingCircleTest, TrajectoryFollowsRetracingCircle) +{ + const double expected_distance = 0.2; + + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distance_from_path(target_path, robot_pose); + EXPECT_NEAR(std::abs(result.distance), expected_distance, 0.01); + } +} + +TEST_F(ZigZagPathTest, TrajectoryFollowsZigZagPath) +{ + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distance_from_path(target_path, robot_pose); + EXPECT_LT(std::abs(result.distance), 1.0); + } +} + +TEST_F(HairpinTurnTest, TrajectoryFollowsHairpinTurn) +{ + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distance_from_path(target_path, robot_pose); + EXPECT_LT(std::abs(result.distance), 1.5); + } +} + +class CuttingCornerWindowedTest : public CuttingCornerTest {}; + +TEST_F(CuttingCornerWindowedTest, WindowedSearch) +{ + size_t start_index = 0; + const double search_window = 11.0; + + for (size_t i = 0; i < robot_trajectory.size(); ++i) { + const auto & robot_pose = robot_trajectory[i]; + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); + start_index = result.closest_segment_index; + EXPECT_NEAR(std::abs(result.distance), expected_distances[i], 0.15); + } +} + +class RetracingPathWindowedTest : public RetracingPathTest {}; + +TEST_F(RetracingPathWindowedTest, WindowedSearch) +{ + const double expected_distance = 0.5; + const double search_window = 21.0; + size_t start_index = 0; + + for (size_t i = 0; i < robot_trajectory.size(); ++i) { + const auto & robot_pose = robot_trajectory[i]; + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); + start_index = result.closest_segment_index; + EXPECT_NEAR(std::abs(result.distance), expected_distance, 1e-6); + } +} + +class ZigZagPathWindowedTest : public ZigZagPathTest {}; + +TEST_F(ZigZagPathWindowedTest, WindowedSearch) +{ + const double search_window = 12.0; + size_t start_index = 0; + + for (size_t i = 0; i < robot_trajectory.size(); ++i) { + const auto & robot_pose = robot_trajectory[i]; + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); + start_index = result.closest_segment_index; + EXPECT_LT(std::abs(result.distance), 1.0); + } +} + +class HairpinTurnWindowedTest : public HairpinTurnTest {}; + +TEST_F(HairpinTurnWindowedTest, WindowedSearch) +{ + const double search_window = 13.0; + size_t start_index = 0; + + for (size_t i = 0; i < robot_trajectory.size(); ++i) { + const auto & robot_pose = robot_trajectory[i]; + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); + start_index = result.closest_segment_index; + EXPECT_LT(std::abs(result.distance), 1.5); + } +} + +TEST(PathUtilsWindowedTest, EdgeCases) +{ + auto robot_pose = createPose(5.0, 5.0); + nav_msgs::msg::Path test_path; + test_path.poses.push_back(createPoseStamped(0.0, 0.0)); + test_path.poses.push_back(createPoseStamped(10.0, 0.0)); + + auto result = nav2_util::distance_from_path(test_path, robot_pose, 0, 5.0); + EXPECT_NEAR(std::abs(result.distance), 5.0, 0.01); +} + +TEST(PathUtilsTest, FourArgEmptyPath) +{ + nav_msgs::msg::Path empty_path; + geometry_msgs::msg::Pose robot_pose = createPose(1.0, 1.0); + auto result = nav2_util::distance_from_path(empty_path, robot_pose, 0, 5.0); + + // Check all fields of the result for empty path + EXPECT_EQ(result.distance, std::numeric_limits::max()); +} + +TEST(PathUtilsTest, FourArgSinglePointPath) +{ + nav_msgs::msg::Path path; + path.poses.push_back(createPoseStamped(2.0, 3.0)); + geometry_msgs::msg::Pose robot_pose = createPose(5.0, 7.0); + auto result = nav2_util::distance_from_path(path, robot_pose, 0, 5.0); + // Distance between (5,7) and (2,3) is 5.0 + EXPECT_NEAR(result.distance, 5.0, 1e-6); + EXPECT_EQ(result.closest_segment_index, 0); +} + +TEST(PathUtilsTest, FourArgThrowsOnStartIndexOutOfBounds) +{ + nav_msgs::msg::Path path; + path.poses.push_back(createPoseStamped(0.0, 0.0)); + path.poses.push_back(createPoseStamped(1.0, 0.0)); + path.poses.push_back(createPoseStamped(2.0, 0.0)); // 3 elements (indices 0, 1, 2) + geometry_msgs::msg::Pose robot_pose = createPose(0.5, 0.0); + + // start_index is out of bounds (equal to path.poses.size()) + EXPECT_THROW( + nav2_util::distance_from_path(path, robot_pose, 3, 5.0), // 3 >= 3 + std::runtime_error); + + // start_index is way out of bounds + EXPECT_THROW( + nav2_util::distance_from_path(path, robot_pose, 100, 5.0), // 100 >= 3 + std::runtime_error); +}