From 0b47add0f9a45e90c9f797945a859cbce6ef685f Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 21 Jul 2025 07:16:10 +0300 Subject: [PATCH 01/70] geometry utils Signed-off-by: silanus23 --- .../include/nav2_util/geometry_utils.hpp | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index d67f17e3c7c..758c7813084 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -231,6 +231,51 @@ inline bool isPointInsidePolygon( return res; } +/** + * @brief Find the distance to a point + * @param global_pose Robot's current or planned position + * @param target + * @return int + */ +inline double distanceToPoint(const geometry_msgs::msg::PoseStamped &global_pose, + const geometry_msgs::msg::PoseStamped &target) +{ + const double xDist = global_pose.pose.position.x - target.pose.position.x; + const double yDist = global_pose.pose.position.y - target.pose.position.y; + return std::hypot(xDist,yDist); +} + +/** + * @brief Find the shortest distance to a vector + * @param global_pose Robot's current or planned position + * @param start Starting point of target vector + * @param finish End point of target vector + * @return int + */ +inline double distanceToSegment( +const geometry_msgs::msg::PoseStamped & point, +const geometry_msgs::msg::PoseStamped & start, +const geometry_msgs::msg::PoseStamped & end) +{ +const auto & p = point.pose.position; +const auto & a = start.pose.position; +const auto & b = end.pose.position; + +const double seg_len_sq = distanceToPoint(start, end); +if (seg_len_sq <= 1e-8) { + return std::sqrt(distanceToPoint(start, end)); +} + +const double dot = ((p.x - a.x) * (b.x - a.x)) + ((p.y - a.y) * (b.y - a.y)); +const double t = std::clamp(dot / seg_len_sq, 0.0, 1.0); + +const double proj_x = a.x + t * (b.x - a.x); +const double proj_y = a.y + t * (b.y - a.y); + +const double dx_proj = p.x - proj_x; +const double dy_proj = p.y - proj_y; +return std::hypot(dx_proj,dy_proj); +} } // namespace geometry_utils } // namespace nav2_util From ff7a2fa76f0778485fe5c07481ef352f00900953 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 22 Jul 2025 17:58:05 +0300 Subject: [PATCH 02/70] fixed geometry utils adding 2d Signed-off-by: silanus23 --- .../include/nav2_util/geometry_utils.hpp | 52 +++++++++++-------- 1 file changed, 29 insertions(+), 23 deletions(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 758c7813084..802336b4cd3 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -234,15 +234,16 @@ inline bool isPointInsidePolygon( /** * @brief Find the distance to a point * @param global_pose Robot's current or planned position - * @param target + * @param target target point * @return int */ -inline double distanceToPoint(const geometry_msgs::msg::PoseStamped &global_pose, - const geometry_msgs::msg::PoseStamped &target) +inline double distanceToPoint( + const geometry_msgs::msg::PoseStamped & point1, + const geometry_msgs::msg::PoseStamped & point2) { - const double xDist = global_pose.pose.position.x - target.pose.position.x; - const double yDist = global_pose.pose.position.y - target.pose.position.y; - return std::hypot(xDist,yDist); + 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); } /** @@ -253,29 +254,34 @@ inline double distanceToPoint(const geometry_msgs::msg::PoseStamped &global_pose * @return int */ inline double distanceToSegment( -const geometry_msgs::msg::PoseStamped & point, -const geometry_msgs::msg::PoseStamped & start, -const geometry_msgs::msg::PoseStamped & end) + const geometry_msgs::msg::PoseStamped & point, + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end) { -const auto & p = point.pose.position; -const auto & a = start.pose.position; -const auto & b = end.pose.position; + const auto & p = point.pose.position; + const auto & a = start.pose.position; + const auto & b = end.pose.position; -const double seg_len_sq = distanceToPoint(start, end); -if (seg_len_sq <= 1e-8) { - return std::sqrt(distanceToPoint(start, end)); -} + 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); -const double dot = ((p.x - a.x) * (b.x - a.x)) + ((p.y - a.y) * (b.y - a.y)); -const double t = std::clamp(dot / seg_len_sq, 0.0, 1.0); + if (seg_len_sq <= 1e-9) { + return distanceToPoint(point, start); + } + + 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 * (b.x - a.x); -const double proj_y = a.y + t * (b.y - a.y); + 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); + const double dx_proj = p.x - proj_x; + const double dy_proj = p.y - proj_y; + return std::hypot(dx_proj, dy_proj); } + } // namespace geometry_utils } // namespace nav2_util From e9b14faaa315992a86922ed4b4a5d1559d18bf52 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 24 Jul 2025 17:59:13 +0300 Subject: [PATCH 03/70] created path_utils Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 52 +++++++++ nav2_util/src/CMakeLists.txt | 2 + nav2_util/src/path_utils.cpp | 129 +++++++++++++++++++++ 3 files changed, 183 insertions(+) create mode 100644 nav2_util/include/nav2_util/path_utils.hpp create mode 100644 nav2_util/src/path_utils.cpp 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..0a309b092a2 --- /dev/null +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -0,0 +1,52 @@ +// 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_msgs/msg/waypoint_status.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace nav2_util +{ + +struct PathSearchResult +{ + double distance; + size_t closest_segment_index; +}; + +PathSearchResult distanceFromPath( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::PoseStamped & robot_pose); + +PathSearchResult distanceFromPath( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::PoseStamped & robot_pose, + const size_t start_index, + const double search_window_length); + +} // 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..42d498def0a 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 @@ -17,6 +18,7 @@ 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 diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp new file mode 100644 index 00000000000..f837c46460e --- /dev/null +++ b/nav2_util/src/path_utils.cpp @@ -0,0 +1,129 @@ +// 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 "nav2_util/geometry_utils.hpp" + +namespace nav2_util +{ +PathSearchResult distanceFromPath( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::PoseStamped & robot_pose) +{ + PathSearchResult result; + result.closest_segment_index = 0; + result.distance = std::numeric_limits::infinity(); + + if (path.poses.empty()) { + result.distance = std::numeric_limits::infinity(); + return result; + } + + if (path.poses.size() == 1) { + result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.front()); + result.closest_segment_index = 0; + return result; + } + + double min_distance = std::numeric_limits::max(); + size_t closest_segment = 0; + + for (size_t i = 0; i < path.poses.size() - 1; ++i) { + const double current_distance = nav2_util::geometry_utils::distanceToSegment( + robot_pose, + path.poses[i], + path.poses[i + 1]); + + if (current_distance < min_distance) { + min_distance = current_distance; + closest_segment = i; // 🔥 Track closest segment in global search too + } + } + + result.distance = min_distance; + result.closest_segment_index = closest_segment; + return result; +} + +PathSearchResult distanceFromPath( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::PoseStamped & 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::infinity(); + + if (path.poses.empty()) { + result.distance = std::numeric_limits::infinity(); + return result; + } + + if (path.poses.size() == 1) { + result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.front()); + result.closest_segment_index = 0; + return result; + } + + if (start_index >= path.poses.size()) { + result.distance = std::numeric_limits::infinity(); + return result; + } + + if (start_index == path.poses.size() - 1) { + result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.back()); + result.closest_segment_index = start_index; + return result; + } + + if (search_window_length <= 0.0) { + result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, + path.poses[start_index]); + result.closest_segment_index = start_index; + return result; + } + + double min_distance = std::numeric_limits::max(); + size_t closest_segment = start_index; + size_t segments_checked = 0; + const size_t max_segments = static_cast(search_window_length); + + for (size_t i = start_index; i < path.poses.size() - 1; ++i) { + const double current_segment_dist = nav2_util::geometry_utils::distanceToSegment( + robot_pose, + path.poses[i], + path.poses[i + 1]); + + if (current_segment_dist < min_distance) { + min_distance = current_segment_dist; + closest_segment = i; + } + + segments_checked++; + if (segments_checked >= max_segments) { + break; + } + } + + result.closest_segment_index = closest_segment; + result.distance = min_distance; + return result; +} + +} // namespace nav2_util From 805c3ebc9932a99e996493c6b451258edeeabb4a Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 24 Jul 2025 17:59:44 +0300 Subject: [PATCH 04/70] added tests Signed-off-by: silanus23 --- nav2_util/test/CMakeLists.txt | 3 + nav2_util/test/test_path_utils.cpp | 330 +++++++++++++++++++++++++++++ 2 files changed, 333 insertions(+) create mode 100644 nav2_util/test/test_path_utils.cpp diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index b7f290f8e78..b9ae1ea4daf 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} ${nav_2d_msgs_TARGETS} ${geometry_msgs_TARGETS}) \ No newline at end of file diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp new file mode 100644 index 00000000000..2541f65ff6b --- /dev/null +++ b/nav2_util/test/test_path_utils.cpp @@ -0,0 +1,330 @@ +// 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 "nav_msgs/msg/path.hpp" +#include "nav2_util/path_utils.hpp" + +geometry_msgs::msg::PoseStamped createPose(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; + pose.header.frame_id = "map"; + 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(createPose( + center_x + radius * std::cos(angle), + center_y + radius * std::sin(angle))); + } +} + +class CloverleafPathTest : public ::testing::Test +{ +protected: + void SetUp() override + { + 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); + + nav_msgs::msg::Path robot_path; + generateCirclePath(robot_path, 5.0, 0.0, 4.8, 50); + robot_trajectory = robot_path.poses; + } + 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); + nav_msgs::msg::Path robot_path; + generateCirclePath(robot_path, 0.0, 0.0, 5.2, 50, 0.0, 2.0 * M_PI); + generateCirclePath(robot_path, 0.0, 0.0, 5.2, 50, 2.0 * M_PI, 0.0); + robot_trajectory = robot_path.poses; + } + nav_msgs::msg::Path target_path; + std::vector robot_trajectory; +}; + +class ZigZagPathTest : public ::testing::Test +{ +protected: + void SetUp() override + { + target_path.poses.push_back(createPose(0.0, 0.0)); + target_path.poses.push_back(createPose(10.0, 0.0)); + target_path.poses.push_back(createPose(10.0, 5.0)); + target_path.poses.push_back(createPose(0.0, 5.0)); + target_path.poses.push_back(createPose(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(createPose(0.0, 1.0)); + target_path.poses.push_back(createPose(10.0, 1.0)); + target_path.poses.push_back(createPose(10.0, -1.0)); + target_path.poses.push_back(createPose(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(createPose(0.0, 0.0)); + target_path.poses.push_back(createPose(10.0, 0.0)); + target_path.poses.push_back(createPose(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(createPose(i, 0.0));} + for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPose(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, EmptyAndSinglePointPaths) +{ + auto robot_pose = createPose(5.0, 5.0); + nav_msgs::msg::Path empty_path; + + auto result = nav2_util::distanceFromPath(empty_path, robot_pose); + EXPECT_EQ(result.distance, std::numeric_limits::infinity()); + + nav_msgs::msg::Path single_point_path; + single_point_path.poses.push_back(createPose(0.0, 0.0)); + result = nav2_util::distanceFromPath(single_point_path, robot_pose); + EXPECT_NEAR(result.distance, 7.071, 0.01); +} + +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::distanceFromPath(target_path, robot_pose); + EXPECT_NEAR(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::distanceFromPath(target_path, robot_pose); + EXPECT_NEAR(result.distance, expected_distance, 1e-6); + } +} + +TEST_F(CloverleafPathTest, TrajectoryFollowsCloverleafLoop) +{ + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distanceFromPath(target_path, robot_pose); + EXPECT_LT(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::distanceFromPath(target_path, robot_pose); + EXPECT_NEAR(result.distance, expected_distance, 0.01); + } +} + +TEST_F(ZigZagPathTest, TrajectoryFollowsZigZagPath) +{ + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distanceFromPath(target_path, robot_pose); + EXPECT_LT(result.distance, 1.0); + } +} + +TEST_F(HairpinTurnTest, TrajectoryFollowsHairpinTurn) +{ + for (const auto & robot_pose : robot_trajectory) { + auto result = nav2_util::distanceFromPath(target_path, robot_pose); + EXPECT_LT(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::distanceFromPath(target_path, robot_pose, start_index, search_window); + EXPECT_NEAR(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::distanceFromPath(target_path, robot_pose, start_index, search_window); + EXPECT_NEAR(result.distance, expected_distance, 1e-6); + } +} + +class CloverleafPathWindowedTest : public CloverleafPathTest {}; + +TEST_F(CloverleafPathWindowedTest, WindowedSearch) +{ + const double search_window = 50.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::distanceFromPath(target_path, robot_pose, start_index, search_window); + EXPECT_LT(result.distance, 0.25); + } +} + +class RetracingCircleWindowedTest : public RetracingCircleTest {}; + +TEST_F(RetracingCircleWindowedTest, WindowedSearch) +{ + const double expected_distance = 0.2; + const double search_window = 100.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::distanceFromPath(target_path, robot_pose, start_index, search_window); + EXPECT_NEAR(result.distance, expected_distance, 0.05); + } +} + +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::distanceFromPath(target_path, robot_pose, start_index, search_window); + EXPECT_LT(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::distanceFromPath(target_path, robot_pose, start_index, search_window); + EXPECT_LT(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(createPose(0.0, 0.0)); + test_path.poses.push_back(createPose(10.0, 0.0)); + + auto result = nav2_util::distanceFromPath(test_path, robot_pose, 1, 5.0); + EXPECT_NEAR(result.distance, 7.071, 0.01); + + result = nav2_util::distanceFromPath(test_path, robot_pose, 0, 0.0); + EXPECT_NEAR(result.distance, 7.071, 0.01); + + result = nav2_util::distanceFromPath(test_path, robot_pose, 10, 5.0); + EXPECT_EQ(result.distance, std::numeric_limits::infinity()); +} From 79f69d207c96ff9de47e66e3b64ee801d07c1def Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 24 Jul 2025 18:51:46 +0300 Subject: [PATCH 05/70] minor changes in tests Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 37 +++++++----------------------- 1 file changed, 8 insertions(+), 29 deletions(-) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 2541f65ff6b..30044400101 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -51,12 +51,16 @@ 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 nav_msgs::msg::Path robot_path; generateCirclePath(robot_path, 5.0, 0.0, 4.8, 50); + generateCirclePath(robot_path, -5.0, 0.0, 4.8, 50); + generateCirclePath(robot_path, 0.0, 5.0, 4.8, 50); robot_trajectory = robot_path.poses; } nav_msgs::msg::Path target_path; @@ -236,6 +240,7 @@ TEST_F(CuttingCornerWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + start_index = result.closest_segment_index; EXPECT_NEAR(result.distance, expected_distances[i], 0.15); } } @@ -251,39 +256,11 @@ TEST_F(RetracingPathWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + start_index = result.closest_segment_index; EXPECT_NEAR(result.distance, expected_distance, 1e-6); } } -class CloverleafPathWindowedTest : public CloverleafPathTest {}; - -TEST_F(CloverleafPathWindowedTest, WindowedSearch) -{ - const double search_window = 50.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::distanceFromPath(target_path, robot_pose, start_index, search_window); - EXPECT_LT(result.distance, 0.25); - } -} - -class RetracingCircleWindowedTest : public RetracingCircleTest {}; - -TEST_F(RetracingCircleWindowedTest, WindowedSearch) -{ - const double expected_distance = 0.2; - const double search_window = 100.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::distanceFromPath(target_path, robot_pose, start_index, search_window); - EXPECT_NEAR(result.distance, expected_distance, 0.05); - } -} - class ZigZagPathWindowedTest : public ZigZagPathTest {}; TEST_F(ZigZagPathWindowedTest, WindowedSearch) @@ -294,6 +271,7 @@ TEST_F(ZigZagPathWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + start_index = result.closest_segment_index; EXPECT_LT(result.distance, 1.0); } } @@ -308,6 +286,7 @@ TEST_F(HairpinTurnWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + start_index = result.closest_segment_index; EXPECT_LT(result.distance, 1.5); } } From e0573d93193b657b8cd2515a4ab000c385d96a4f Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 24 Jul 2025 19:16:21 +0300 Subject: [PATCH 06/70] lint issue Signed-off-by: silanus23 --- nav2_util/test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index b9ae1ea4daf..629378c61fe 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -39,4 +39,4 @@ 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}) \ No newline at end of file +target_link_libraries(test_path_utils ${library_name} ${nav_2d_msgs_TARGETS} ${geometry_msgs_TARGETS}) From 2318a7b79e5e87e20c8b3b59657d8bbfc02cd0a6 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 28 Jul 2025 23:46:31 +0300 Subject: [PATCH 07/70] fixed reviews Signed-off-by: silanus23 --- .../include/nav2_util/geometry_utils.hpp | 16 +-- nav2_util/include/nav2_util/path_utils.hpp | 20 ++-- nav2_util/src/path_utils.cpp | 110 +++++------------- nav2_util/test/test_path_utils.cpp | 39 +++---- 4 files changed, 68 insertions(+), 117 deletions(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 802336b4cd3..f85af971dfd 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -253,14 +253,14 @@ inline double distanceToPoint( * @param finish End point of target vector * @return int */ -inline double distanceToSegment( - const geometry_msgs::msg::PoseStamped & point, - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & end) +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.pose.position; - const auto & a = start.pose.position; - const auto & b = end.pose.position; + 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; @@ -268,7 +268,7 @@ inline double distanceToSegment( const double seg_len_sq = (dx_seg * dx_seg) + (dy_seg * dy_seg); if (seg_len_sq <= 1e-9) { - return distanceToPoint(point, start); + return euclidean_distance(point, a); } const double dot = ((p.x - a.x) * dx_seg) + ((p.y - a.y) * dy_seg); diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 0a309b092a2..be8877d8880 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -31,21 +31,27 @@ namespace nav2_util { +/** + * @brief Result of searching for the closest segment on a path. + */ struct PathSearchResult { + /** + * @brief The minimum distance from the robot to the path. + */ double distance; + + /** + * @brief The index of the closest path segment. + */ size_t closest_segment_index; }; -PathSearchResult distanceFromPath( - const nav_msgs::msg::Path & path, - const geometry_msgs::msg::PoseStamped & robot_pose); - -PathSearchResult distanceFromPath( +PathSearchResult distance_from_path( const nav_msgs::msg::Path & path, const geometry_msgs::msg::PoseStamped & robot_pose, - const size_t start_index, - const double search_window_length); + const size_t start_index = 0, + const double search_window_length = std::numeric_limits::max()); } // namespace nav2_util diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index f837c46460e..46fb70256d5 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -13,54 +13,13 @@ // limitations under the License. #include "nav2_util/path_utils.hpp" - #include #include - #include "nav2_util/geometry_utils.hpp" - namespace nav2_util { -PathSearchResult distanceFromPath( - const nav_msgs::msg::Path & path, - const geometry_msgs::msg::PoseStamped & robot_pose) -{ - PathSearchResult result; - result.closest_segment_index = 0; - result.distance = std::numeric_limits::infinity(); - - if (path.poses.empty()) { - result.distance = std::numeric_limits::infinity(); - return result; - } - - if (path.poses.size() == 1) { - result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.front()); - result.closest_segment_index = 0; - return result; - } - - double min_distance = std::numeric_limits::max(); - size_t closest_segment = 0; - - for (size_t i = 0; i < path.poses.size() - 1; ++i) { - const double current_distance = nav2_util::geometry_utils::distanceToSegment( - robot_pose, - path.poses[i], - path.poses[i + 1]); - - if (current_distance < min_distance) { - min_distance = current_distance; - closest_segment = i; // 🔥 Track closest segment in global search too - } - } - result.distance = min_distance; - result.closest_segment_index = closest_segment; - return result; -} - -PathSearchResult distanceFromPath( +PathSearchResult distance_from_path( const nav_msgs::msg::Path & path, const geometry_msgs::msg::PoseStamped & robot_pose, const size_t start_index, @@ -68,61 +27,48 @@ PathSearchResult distanceFromPath( { PathSearchResult result; result.closest_segment_index = start_index; - result.distance = std::numeric_limits::infinity(); + result.distance = std::numeric_limits::max(); - if (path.poses.empty()) { - result.distance = std::numeric_limits::infinity(); - return result; - } - - if (path.poses.size() == 1) { - result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.front()); + if (path.poses.size() < 2) { + if (path.poses.empty()) { + return result; + } + result.distance = nav2_util::geometry_utils::euclidean_distance( + robot_pose.pose, path.poses.front().pose); result.closest_segment_index = 0; return result; } - if (start_index >= path.poses.size()) { - result.distance = std::numeric_limits::infinity(); - return result; - } - - if (start_index == path.poses.size() - 1) { - result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.back()); - result.closest_segment_index = start_index; + if (start_index >= path.poses.size() - 1) { + result.distance = nav2_util::geometry_utils::euclidean_distance( + robot_pose.pose, + path.poses.back().pose); + result.closest_segment_index = path.poses.size() - 1; return result; } - if (search_window_length <= 0.0) { - result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, - path.poses[start_index]); - result.closest_segment_index = start_index; - return result; - } - - double min_distance = std::numeric_limits::max(); - size_t closest_segment = start_index; - size_t segments_checked = 0; - const size_t max_segments = static_cast(search_window_length); + double distance_traversed = 0.0; for (size_t i = start_index; i < path.poses.size() - 1; ++i) { - const double current_segment_dist = nav2_util::geometry_utils::distanceToSegment( - robot_pose, - path.poses[i], - path.poses[i + 1]); - - if (current_segment_dist < min_distance) { - min_distance = current_segment_dist; - closest_segment = i; + if (distance_traversed > search_window_length) { + break; } - segments_checked++; - if (segments_checked >= max_segments) { - break; + const double current_distance = geometry_utils::distance_to_segment( + robot_pose.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].pose, + path.poses[i + 1].pose); } - result.closest_segment_index = closest_segment; - result.distance = min_distance; return result; } diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 30044400101..3ec05f64208 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -18,6 +18,7 @@ #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" @@ -168,12 +169,12 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) auto robot_pose = createPose(5.0, 5.0); nav_msgs::msg::Path empty_path; - auto result = nav2_util::distanceFromPath(empty_path, robot_pose); - EXPECT_EQ(result.distance, std::numeric_limits::infinity()); + auto result = nav2_util::distance_from_path(empty_path, robot_pose); + EXPECT_EQ(result.distance, std::numeric_limits::max()); nav_msgs::msg::Path single_point_path; single_point_path.poses.push_back(createPose(0.0, 0.0)); - result = nav2_util::distanceFromPath(single_point_path, robot_pose); + result = nav2_util::distance_from_path(single_point_path, robot_pose); EXPECT_NEAR(result.distance, 7.071, 0.01); } @@ -181,7 +182,7 @@ 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::distanceFromPath(target_path, robot_pose); + auto result = nav2_util::distance_from_path(target_path, robot_pose); EXPECT_NEAR(result.distance, expected_distances[i], 0.1); } } @@ -191,7 +192,7 @@ TEST_F(RetracingPathTest, TrajectoryFollowsRetracingPath) const double expected_distance = 0.5; for (const auto & robot_pose : robot_trajectory) { - auto result = nav2_util::distanceFromPath(target_path, robot_pose); + auto result = nav2_util::distance_from_path(target_path, robot_pose); EXPECT_NEAR(result.distance, expected_distance, 1e-6); } } @@ -199,7 +200,7 @@ TEST_F(RetracingPathTest, TrajectoryFollowsRetracingPath) TEST_F(CloverleafPathTest, TrajectoryFollowsCloverleafLoop) { for (const auto & robot_pose : robot_trajectory) { - auto result = nav2_util::distanceFromPath(target_path, robot_pose); + auto result = nav2_util::distance_from_path(target_path, robot_pose); EXPECT_LT(result.distance, 0.25); } } @@ -209,7 +210,7 @@ TEST_F(RetracingCircleTest, TrajectoryFollowsRetracingCircle) const double expected_distance = 0.2; for (const auto & robot_pose : robot_trajectory) { - auto result = nav2_util::distanceFromPath(target_path, robot_pose); + auto result = nav2_util::distance_from_path(target_path, robot_pose); EXPECT_NEAR(result.distance, expected_distance, 0.01); } } @@ -217,7 +218,7 @@ TEST_F(RetracingCircleTest, TrajectoryFollowsRetracingCircle) TEST_F(ZigZagPathTest, TrajectoryFollowsZigZagPath) { for (const auto & robot_pose : robot_trajectory) { - auto result = nav2_util::distanceFromPath(target_path, robot_pose); + auto result = nav2_util::distance_from_path(target_path, robot_pose); EXPECT_LT(result.distance, 1.0); } } @@ -225,7 +226,7 @@ TEST_F(ZigZagPathTest, TrajectoryFollowsZigZagPath) TEST_F(HairpinTurnTest, TrajectoryFollowsHairpinTurn) { for (const auto & robot_pose : robot_trajectory) { - auto result = nav2_util::distanceFromPath(target_path, robot_pose); + auto result = nav2_util::distance_from_path(target_path, robot_pose); EXPECT_LT(result.distance, 1.5); } } @@ -239,7 +240,8 @@ TEST_F(CuttingCornerWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; - auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); start_index = result.closest_segment_index; EXPECT_NEAR(result.distance, expected_distances[i], 0.15); } @@ -255,7 +257,8 @@ TEST_F(RetracingPathWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; - auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); start_index = result.closest_segment_index; EXPECT_NEAR(result.distance, expected_distance, 1e-6); } @@ -270,7 +273,8 @@ TEST_F(ZigZagPathWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; - auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); start_index = result.closest_segment_index; EXPECT_LT(result.distance, 1.0); } @@ -285,7 +289,8 @@ TEST_F(HairpinTurnWindowedTest, WindowedSearch) for (size_t i = 0; i < robot_trajectory.size(); ++i) { const auto & robot_pose = robot_trajectory[i]; - auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window); + auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, + search_window); start_index = result.closest_segment_index; EXPECT_LT(result.distance, 1.5); } @@ -298,12 +303,6 @@ TEST(PathUtilsWindowedTest, EdgeCases) test_path.poses.push_back(createPose(0.0, 0.0)); test_path.poses.push_back(createPose(10.0, 0.0)); - auto result = nav2_util::distanceFromPath(test_path, robot_pose, 1, 5.0); + auto result = nav2_util::distance_from_path(test_path, robot_pose, 1, 5.0); EXPECT_NEAR(result.distance, 7.071, 0.01); - - result = nav2_util::distanceFromPath(test_path, robot_pose, 0, 0.0); - EXPECT_NEAR(result.distance, 7.071, 0.01); - - result = nav2_util::distanceFromPath(test_path, robot_pose, 10, 5.0); - EXPECT_EQ(result.distance, std::numeric_limits::infinity()); } From fbe823eff659e84a0fc44b7d589ac15be3f794ec Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 29 Jul 2025 00:02:39 +0300 Subject: [PATCH 08/70] doxygen fix Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index be8877d8880..d1a1b8c6813 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -36,14 +36,7 @@ namespace nav2_util */ struct PathSearchResult { - /** - * @brief The minimum distance from the robot to the path. - */ double distance; - - /** - * @brief The index of the closest path segment. - */ size_t closest_segment_index; }; From efea555e12b14ce0cf68c4988f339475445b145c Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 12 Aug 2025 02:38:19 +0300 Subject: [PATCH 09/70] Last fixes. Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index d1a1b8c6813..e51b30ca5f8 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -39,7 +39,19 @@ 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. + * @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::PoseStamped & robot_pose, From 8ee1ebb3168e341c86e20d153f034f72bf623292 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 12 Aug 2025 02:52:53 +0300 Subject: [PATCH 10/70] Last fixes cpp. Signed-off-by: silanus23 --- nav2_util/src/path_utils.cpp | 33 +++++++++++++++++++----------- nav2_util/test/test_path_utils.cpp | 5 ++--- 2 files changed, 23 insertions(+), 15 deletions(-) diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index 46fb70256d5..e8ff0ee7d7a 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -13,8 +13,11 @@ // limitations under the License. #include "nav2_util/path_utils.hpp" + #include #include +#include + #include "nav2_util/geometry_utils.hpp" namespace nav2_util { @@ -29,26 +32,32 @@ PathSearchResult distance_from_path( result.closest_segment_index = start_index; result.distance = std::numeric_limits::max(); - if (path.poses.size() < 2) { - if (path.poses.empty()) { - return result; - } - result.distance = nav2_util::geometry_utils::euclidean_distance( - robot_pose.pose, path.poses.front().pose); - result.closest_segment_index = 0; + if (path.poses.empty()) { return result; } - if (start_index >= path.poses.size() - 1) { + if (path.poses.size() == 1) { result.distance = nav2_util::geometry_utils::euclidean_distance( - robot_pose.pose, - path.poses.back().pose); - result.closest_segment_index = path.poses.size() - 1; + robot_pose.pose, path.poses.front().pose); + result.closest_segment_index = 0; return result; } - double distance_traversed = 0.0; + if (start_index >= path.poses.size()) { + 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."); + } + if (path.header.frame_id != robot_pose.header.frame_id) { + throw std::invalid_argument( + "Invalid input, path frame (" + (path.header.frame_id) + + ") is not the same frame as robot frame (" + (robot_pose.header.frame_id) + + "). Use the same frame for both pose and path."); + } + + double distance_traversed = 0.0; for (size_t i = start_index; i < path.poses.size() - 1; ++i) { if (distance_traversed > search_window_length) { break; diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 3ec05f64208..e2ff3bc1b14 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -29,7 +29,6 @@ geometry_msgs::msg::PoseStamped createPose(double x, double y) pose.pose.position.y = y; pose.pose.position.z = 0.0; pose.pose.orientation.w = 1.0; - pose.header.frame_id = "map"; return pose; } @@ -303,6 +302,6 @@ TEST(PathUtilsWindowedTest, EdgeCases) test_path.poses.push_back(createPose(0.0, 0.0)); test_path.poses.push_back(createPose(10.0, 0.0)); - auto result = nav2_util::distance_from_path(test_path, robot_pose, 1, 5.0); - EXPECT_NEAR(result.distance, 7.071, 0.01); + auto result = nav2_util::distance_from_path(test_path, robot_pose, 0, 5.0); + EXPECT_NEAR(result.distance, 5.0, 0.01); } From 4947423eeb9a3e1666c910ab3d03cd0b7b75caee Mon Sep 17 00:00:00 2001 From: silanus Date: Tue, 12 Aug 2025 03:00:34 +0300 Subject: [PATCH 11/70] Update nav2_util/include/nav2_util/path_utils.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: silanus Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index e51b30ca5f8..172326c937c 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -24,7 +24,6 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" -#include "nav2_msgs/msg/waypoint_status.hpp" #include "nav2_util/geometry_utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" From c8db04face4d0f8e89eb7abfa2ab6028214097f1 Mon Sep 17 00:00:00 2001 From: silanus Date: Tue, 12 Aug 2025 03:00:42 +0300 Subject: [PATCH 12/70] Update nav2_util/include/nav2_util/path_utils.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: silanus Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 172326c937c..906ed01b7a8 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -25,8 +25,6 @@ #include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/geometry_utils.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - namespace nav2_util { From cfb33519d804261ce875502e91104ffb0abdf278 Mon Sep 17 00:00:00 2001 From: silanus Date: Tue, 12 Aug 2025 03:00:51 +0300 Subject: [PATCH 13/70] Update nav2_util/test/test_path_utils.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: silanus Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index e2ff3bc1b14..eaf2d39631b 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -156,8 +156,12 @@ class RetracingPathTest : public ::testing::Test { for (int i = 0; i <= 10; ++i) {target_path.poses.push_back(createPose(i, 0.0));} for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPose(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));} + 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; From 3bf72b221d6da698477297a52a8dbf9d499c3822 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 12 Aug 2025 10:23:01 +0300 Subject: [PATCH 14/70] Frame check fix Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 4 +- nav2_util/src/path_utils.cpp | 17 ++-- nav2_util/test/test_path_utils.cpp | 91 ++++++++++++++-------- 3 files changed, 64 insertions(+), 48 deletions(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 906ed01b7a8..797015f29ef 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -44,14 +44,14 @@ struct PathSearchResult * 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. + * @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::PoseStamped & robot_pose, + const geometry_msgs::msg::Pose & robot_pose, const size_t start_index = 0, const double search_window_length = std::numeric_limits::max()); diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index e8ff0ee7d7a..b694fa880b9 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -24,7 +24,7 @@ namespace nav2_util PathSearchResult distance_from_path( const nav_msgs::msg::Path & path, - const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Pose & robot_pose, const size_t start_index, const double search_window_length) { @@ -38,7 +38,7 @@ PathSearchResult distance_from_path( if (path.poses.size() == 1) { result.distance = nav2_util::geometry_utils::euclidean_distance( - robot_pose.pose, path.poses.front().pose); + robot_pose, path.poses.front().pose); result.closest_segment_index = 0; return result; } @@ -50,13 +50,6 @@ PathSearchResult distance_from_path( "). Application is not properly managing state."); } - if (path.header.frame_id != robot_pose.header.frame_id) { - throw std::invalid_argument( - "Invalid input, path frame (" + (path.header.frame_id) + - ") is not the same frame as robot frame (" + (robot_pose.header.frame_id) + - "). Use the same frame for both pose and path."); - } - double distance_traversed = 0.0; for (size_t i = start_index; i < path.poses.size() - 1; ++i) { if (distance_traversed > search_window_length) { @@ -64,7 +57,7 @@ PathSearchResult distance_from_path( } const double current_distance = geometry_utils::distance_to_segment( - robot_pose.pose.position, + robot_pose.position, path.poses[i].pose, path.poses[i + 1].pose); @@ -74,8 +67,8 @@ PathSearchResult distance_from_path( } distance_traversed += geometry_utils::euclidean_distance( - path.poses[i].pose, - path.poses[i + 1].pose); + path.poses[i], + path.poses[i + 1]); } return result; diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index eaf2d39631b..20014603ffe 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -22,7 +22,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_util/path_utils.hpp" -geometry_msgs::msg::PoseStamped createPose(double x, double y) +geometry_msgs::msg::PoseStamped createPoseStamped(double x, double y) { geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = x; @@ -32,6 +32,16 @@ geometry_msgs::msg::PoseStamped createPose(double x, double y) 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, @@ -40,12 +50,25 @@ void generateCirclePath( 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(createPose( + 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: @@ -57,14 +80,14 @@ class CloverleafPathTest : public ::testing::Test 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 - nav_msgs::msg::Path robot_path; - generateCirclePath(robot_path, 5.0, 0.0, 4.8, 50); - generateCirclePath(robot_path, -5.0, 0.0, 4.8, 50); - generateCirclePath(robot_path, 0.0, 5.0, 4.8, 50); - robot_trajectory = robot_path.poses; + 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; + std::vector robot_trajectory; }; class RetracingCircleTest : public ::testing::Test @@ -74,13 +97,13 @@ class RetracingCircleTest : public ::testing::Test { 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); - nav_msgs::msg::Path robot_path; - generateCirclePath(robot_path, 0.0, 0.0, 5.2, 50, 0.0, 2.0 * M_PI); - generateCirclePath(robot_path, 0.0, 0.0, 5.2, 50, 2.0 * M_PI, 0.0); - robot_trajectory = robot_path.poses; + 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; + std::vector robot_trajectory; }; class ZigZagPathTest : public ::testing::Test @@ -88,11 +111,11 @@ class ZigZagPathTest : public ::testing::Test protected: void SetUp() override { - target_path.poses.push_back(createPose(0.0, 0.0)); - target_path.poses.push_back(createPose(10.0, 0.0)); - target_path.poses.push_back(createPose(10.0, 5.0)); - target_path.poses.push_back(createPose(0.0, 5.0)); - target_path.poses.push_back(createPose(0.0, 10.0)); + 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), @@ -102,7 +125,7 @@ class ZigZagPathTest : public ::testing::Test }; } nav_msgs::msg::Path target_path; - std::vector robot_trajectory; + std::vector robot_trajectory; }; class HairpinTurnTest : public ::testing::Test @@ -110,10 +133,10 @@ class HairpinTurnTest : public ::testing::Test protected: void SetUp() override { - target_path.poses.push_back(createPose(0.0, 1.0)); - target_path.poses.push_back(createPose(10.0, 1.0)); - target_path.poses.push_back(createPose(10.0, -1.0)); - target_path.poses.push_back(createPose(0.0, -1.0)); + 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), @@ -124,7 +147,7 @@ class HairpinTurnTest : public ::testing::Test }; } nav_msgs::msg::Path target_path; - std::vector robot_trajectory; + std::vector robot_trajectory; }; class CuttingCornerTest : public ::testing::Test @@ -132,9 +155,9 @@ class CuttingCornerTest : public ::testing::Test protected: void SetUp() override { - target_path.poses.push_back(createPose(0.0, 0.0)); - target_path.poses.push_back(createPose(10.0, 0.0)); - target_path.poses.push_back(createPose(10.0, 10.0)); + 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), @@ -145,7 +168,7 @@ class CuttingCornerTest : public ::testing::Test 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 robot_trajectory; std::vector expected_distances; }; @@ -154,8 +177,8 @@ class RetracingPathTest : public ::testing::Test protected: void SetUp() override { - for (int i = 0; i <= 10; ++i) {target_path.poses.push_back(createPose(i, 0.0));} - for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPose(i, 0.0));} + 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)); } @@ -164,7 +187,7 @@ class RetracingPathTest : public ::testing::Test } } nav_msgs::msg::Path target_path; - std::vector robot_trajectory; + std::vector robot_trajectory; }; TEST(PathUtilsTest, EmptyAndSinglePointPaths) @@ -176,7 +199,7 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) EXPECT_EQ(result.distance, std::numeric_limits::max()); nav_msgs::msg::Path single_point_path; - single_point_path.poses.push_back(createPose(0.0, 0.0)); + single_point_path.poses.push_back(createPoseStamped(0.0, 0.0)); result = nav2_util::distance_from_path(single_point_path, robot_pose); EXPECT_NEAR(result.distance, 7.071, 0.01); } @@ -303,8 +326,8 @@ TEST(PathUtilsWindowedTest, EdgeCases) { auto robot_pose = createPose(5.0, 5.0); nav_msgs::msg::Path test_path; - test_path.poses.push_back(createPose(0.0, 0.0)); - test_path.poses.push_back(createPose(10.0, 0.0)); + 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(result.distance, 5.0, 0.01); From 52b01dc0d0777bac740fafbab6496454a51a4ac8 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 19 Sep 2025 01:14:08 +0300 Subject: [PATCH 15/70] msg Signed-off-by: silanus23 --- nav2_msgs/msg/TrackingError.msg | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 nav2_msgs/msg/TrackingError.msg diff --git a/nav2_msgs/msg/TrackingError.msg b/nav2_msgs/msg/TrackingError.msg new file mode 100644 index 00000000000..352e0dbf0cb --- /dev/null +++ b/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 From 8968b112f52025adfec6e61e72c589ac54e66bbe Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 19 Sep 2025 01:27:06 +0300 Subject: [PATCH 16/70] controller server publisher Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 1 + nav2_controller/src/controller_server.cpp | 64 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index b68c8e5bb94..65589078946 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_error.hpp" #include "nav2_msgs/msg/speed_limit.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/simple_action_server.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 98a6eda8629..ba3677bebbf 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; @@ -228,6 +229,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); + tracking_error_pub_ = create_publisher("tracking_error", 10); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); // Create the action server that we implement with our followPath method @@ -500,6 +502,8 @@ void ControllerServer::computeControl() computeAndPublishVelocity(); + publishTrackingState(); + if (isGoalReached()) { RCLCPP_INFO(get_logger(), "Reached the goal!"); break; @@ -750,6 +754,66 @@ 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; + } + +// Frame checks + geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; + if (!nav2_util::transformPoseInTargetFrame( + robot_pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), + current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) + { + RCLCPP_WARN(get_logger(), "Failed to transform robot pose to path frame."); + return; + } + + geometry_msgs::msg::PoseStamped end_pose_in_robot_frame; + if (!nav2_util::transformPoseInTargetFrame( + end_pose_, end_pose_in_robot_frame, *costmap_ros_->getTfBuffer(), + robot_pose.header.frame_id, costmap_ros_->getTransformTolerance())) + { + RCLCPP_WARN(get_logger(), "Failed to transform end pose to robot frame."); + 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_); + + 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]; + + // Cross product is for getting which side of the track + 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); +} + void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { auto cmd_vel = std::make_unique(velocity); From efd26bc9887e10c18dbffd436080206a3ff7f305 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 19 Sep 2025 01:34:40 +0300 Subject: [PATCH 17/70] controller server publisher fix Signed-off-by: silanus23 --- .../include/nav2_controller/controller_server.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 65589078946..b1e5e71dcc0 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -173,6 +173,10 @@ class ControllerServer : public nav2::LifecycleNode * @param velocity Twist velocity to be published */ void publishVelocity(const geometry_msgs::msg::TwistStamped & velocity); + /** + * @brief Calculates and publishes the TrackingError's all components + */ + void publishTrackingState(); /** * @brief Calls velocity publisher to publish zero velocity */ @@ -237,6 +241,7 @@ class ControllerServer : public nav2::LifecycleNode std::unique_ptr odom_sub_; std::unique_ptr vel_publisher_; nav2::Subscription::SharedPtr speed_limit_sub_; + rclcpp::Publisher::SharedPtr tracking_error_pub_; // Progress Checker Plugin pluginlib::ClassLoader progress_checker_loader_; @@ -269,6 +274,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_; From b66c5faee32640b93b115d3eb58d3d837d3df1cb Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 19 Sep 2025 01:39:10 +0300 Subject: [PATCH 18/70] cross_product Signed-off-by: silanus23 --- .../include/nav2_util/geometry_utils.hpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index f85af971dfd..5f215e62769 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -282,6 +282,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 From 1c035e6ede9c1fed89b97316d00f064f61cbb0e9 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 19 Sep 2025 19:50:22 +0300 Subject: [PATCH 19/70] handled last_fixes Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 5 ++++- nav2_controller/src/controller_server.cpp | 22 ++++++++----------- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/action/FollowPath.action | 1 + nav2_msgs/msg/TrackingError.msg | 6 ++--- 5 files changed, 18 insertions(+), 17 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index b1e5e71dcc0..86dfcb57117 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -241,7 +241,7 @@ class ControllerServer : public nav2::LifecycleNode std::unique_ptr odom_sub_; std::unique_ptr vel_publisher_; nav2::Subscription::SharedPtr speed_limit_sub_; - rclcpp::Publisher::SharedPtr tracking_error_pub_; + nav2::Publisher::SharedPtr tracking_error_pub_; // Progress Checker Plugin pluginlib::ClassLoader 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 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ba3677bebbf..fddf34912ed 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/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(); + 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) diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index b44e08ccb3a..20c72960c47 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/TrackingError.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..ee09dcea0fa 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -25,3 +25,4 @@ string error_msg #feedback definition float32 distance_to_goal float32 speed +float32 tracking_error diff --git a/nav2_msgs/msg/TrackingError.msg b/nav2_msgs/msg/TrackingError.msg index 352e0dbf0cb..75f35b21fb4 100644 --- a/nav2_msgs/msg/TrackingError.msg +++ b/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 From b8c4a3f94476c278f7c1f1e235c28d12f13916b2 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Sat, 20 Sep 2025 01:08:26 +0300 Subject: [PATCH 20/70] tracking error added to follow path side from side added to tracking error Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 4 +- nav2_controller/src/controller_server.cpp | 63 +++++-------------- nav2_util/src/path_utils.cpp | 10 ++- nav2_util/test/test_path_utils.cpp | 24 +++---- 4 files changed, 40 insertions(+), 61 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 86dfcb57117..8b94970dc9c 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -291,8 +291,8 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; - // Last tracking error - double signed_distance_ = 0; + // Last tracking error in the form of TrackingError.msg + nav2_msgs::msg::TrackingError current_tracking_error_; private: /** diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fddf34912ed..5d44dc3471a 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -500,10 +500,10 @@ void ControllerServer::computeControl() updateGlobalPath(); - computeAndPublishVelocity(); - publishTrackingState(); + computeAndPublishVelocity(); + if (isGoalReached()) { RCLCPP_INFO(get_logger(), "Reached the goal!"); break; @@ -681,30 +681,9 @@ void ControllerServer::computeAndPublishVelocity() } std::shared_ptr feedback = std::make_shared(); - feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); - - nav_msgs::msg::Path & current_path = current_path_; - auto find_closest_pose_idx = [&robot_pose_in_path_frame, ¤t_path]() - { - 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; - }; - 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); + feedback->tracking_error = current_tracking_error_; action_server_->publish_feedback(feedback); - feedback->tracking_error = signed_distance_; } void ControllerServer::updateGlobalPath() @@ -778,36 +757,28 @@ void ControllerServer::publishTrackingState() return; } - geometry_msgs::msg::PoseStamped end_pose_in_robot_frame; - if (!nav2_util::transformPoseInTargetFrame( - end_pose_, end_pose_in_robot_frame, *costmap_ros_->getTfBuffer(), - robot_pose.header.frame_id, costmap_ros_->getTransformTolerance())) - { - RCLCPP_WARN(get_logger(), "Failed to transform end pose to robot frame."); - return; - } - const auto path_search_result = nav2_util::distance_from_path( current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); - const size_t closest_idx = path_search_result.closest_segment_index; - start_index_ = closest_idx; + // Set the current closest idx to the next cycle's start + start_index_ = path_search_result.closest_segment_index; - const auto & segment_start = current_path_.poses[closest_idx]; - const auto & segment_end = current_path_.poses[closest_idx + 1]; + double current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(robot_pose, + end_pose_); - // Cross product is for getting which side of the track - double cross_product = nav2_util::geometry_utils::cross_product_2d( - robot_pose_in_path_frame.pose.position, segment_start.pose, segment_end.pose); - - signed_distance_ = path_search_result.distance * (cross_product >= 0.0 ? 1.0 : -1.0); auto tracking_error_msg = std::make_unique(); - 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->header = robot_pose.header; + tracking_error_msg->tracking_error = path_search_result.distance; + tracking_error_msg->current_path_index = start_index_; tracking_error_msg->robot_pose = robot_pose; + tracking_error_msg->distance_to_goal = current_distance_to_goal; + tracking_error_msg->speed = std::hypot( + getThresholdedTwist(odom_sub_->getRawTwist()).linear.x, + getThresholdedTwist(odom_sub_->getRawTwist()).linear.y); + current_tracking_error_ = *tracking_error_msg; tracking_error_pub_->publish(std::move(tracking_error_msg)); + } void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index b694fa880b9..4a45d40090b 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -38,7 +38,7 @@ PathSearchResult distance_from_path( if (path.poses.size() == 1) { result.distance = nav2_util::geometry_utils::euclidean_distance( - robot_pose, path.poses.front().pose); + robot_pose, path.poses.front().pose); result.closest_segment_index = 0; return result; } @@ -71,6 +71,14 @@ PathSearchResult distance_from_path( 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]; + + 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; } diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 20014603ffe..5655e87e78e 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -201,7 +201,7 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) nav_msgs::msg::Path single_point_path; single_point_path.poses.push_back(createPoseStamped(0.0, 0.0)); result = nav2_util::distance_from_path(single_point_path, robot_pose); - EXPECT_NEAR(result.distance, 7.071, 0.01); + EXPECT_NEAR(std::abs(result.distance), 7.071, 0.01); } TEST_F(CuttingCornerTest, TrajectoryCutsCorner) @@ -209,7 +209,7 @@ 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(result.distance, expected_distances[i], 0.1); + EXPECT_NEAR(std::abs(result.distance), expected_distances[i], 0.1); } } @@ -219,7 +219,7 @@ TEST_F(RetracingPathTest, TrajectoryFollowsRetracingPath) for (const auto & robot_pose : robot_trajectory) { auto result = nav2_util::distance_from_path(target_path, robot_pose); - EXPECT_NEAR(result.distance, expected_distance, 1e-6); + EXPECT_NEAR(std::abs(result.distance), expected_distance, 1e-6); } } @@ -227,7 +227,7 @@ TEST_F(CloverleafPathTest, TrajectoryFollowsCloverleafLoop) { for (const auto & robot_pose : robot_trajectory) { auto result = nav2_util::distance_from_path(target_path, robot_pose); - EXPECT_LT(result.distance, 0.25); + EXPECT_LT(std::abs(result.distance), 0.25); } } @@ -237,7 +237,7 @@ TEST_F(RetracingCircleTest, TrajectoryFollowsRetracingCircle) for (const auto & robot_pose : robot_trajectory) { auto result = nav2_util::distance_from_path(target_path, robot_pose); - EXPECT_NEAR(result.distance, expected_distance, 0.01); + EXPECT_NEAR(std::abs(result.distance), expected_distance, 0.01); } } @@ -245,7 +245,7 @@ TEST_F(ZigZagPathTest, TrajectoryFollowsZigZagPath) { for (const auto & robot_pose : robot_trajectory) { auto result = nav2_util::distance_from_path(target_path, robot_pose); - EXPECT_LT(result.distance, 1.0); + EXPECT_LT(std::abs(result.distance), 1.0); } } @@ -253,7 +253,7 @@ TEST_F(HairpinTurnTest, TrajectoryFollowsHairpinTurn) { for (const auto & robot_pose : robot_trajectory) { auto result = nav2_util::distance_from_path(target_path, robot_pose); - EXPECT_LT(result.distance, 1.5); + EXPECT_LT(std::abs(result.distance), 1.5); } } @@ -269,7 +269,7 @@ TEST_F(CuttingCornerWindowedTest, WindowedSearch) auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, search_window); start_index = result.closest_segment_index; - EXPECT_NEAR(result.distance, expected_distances[i], 0.15); + EXPECT_NEAR(std::abs(result.distance), expected_distances[i], 0.15); } } @@ -286,7 +286,7 @@ TEST_F(RetracingPathWindowedTest, WindowedSearch) auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, search_window); start_index = result.closest_segment_index; - EXPECT_NEAR(result.distance, expected_distance, 1e-6); + EXPECT_NEAR(std::abs(result.distance), expected_distance, 1e-6); } } @@ -302,7 +302,7 @@ TEST_F(ZigZagPathWindowedTest, WindowedSearch) auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, search_window); start_index = result.closest_segment_index; - EXPECT_LT(result.distance, 1.0); + EXPECT_LT(std::abs(result.distance), 1.0); } } @@ -318,7 +318,7 @@ TEST_F(HairpinTurnWindowedTest, WindowedSearch) auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index, search_window); start_index = result.closest_segment_index; - EXPECT_LT(result.distance, 1.5); + EXPECT_LT(std::abs(result.distance), 1.5); } } @@ -330,5 +330,5 @@ TEST(PathUtilsWindowedTest, EdgeCases) 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(result.distance, 5.0, 0.01); + EXPECT_NEAR(std::abs(result.distance), 5.0, 0.01); } From 2b6d00636e5bc7fde8bef35f8206eb52f8233d19 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Sat, 20 Sep 2025 01:10:52 +0300 Subject: [PATCH 21/70] arranged msgs Signed-off-by: silanus23 --- nav2_msgs/action/FollowPath.action | 6 +++--- nav2_msgs/msg/TrackingError.msg | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index ee09dcea0fa..44646d4a2cb 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -23,6 +23,6 @@ uint16 error_code string error_msg --- #feedback definition -float32 distance_to_goal -float32 speed -float32 tracking_error +#Real-time tracking_error and decides 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/TrackingError tracking_error \ No newline at end of file diff --git a/nav2_msgs/msg/TrackingError.msg b/nav2_msgs/msg/TrackingError.msg index 75f35b21fb4..80b1e2a7ebf 100644 --- a/nav2_msgs/msg/TrackingError.msg +++ b/nav2_msgs/msg/TrackingError.msg @@ -6,3 +6,5 @@ std_msgs/Header header float32 tracking_error uint32 current_path_index geometry_msgs/PoseStamped robot_pose +float32 distance_to_goal +float32 speed \ No newline at end of file From 219efc93a1c8f08157b0616921383c3ede0df3c5 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Sat, 20 Sep 2025 01:17:13 +0300 Subject: [PATCH 22/70] linting of msgs Signed-off-by: silanus23 --- nav2_msgs/action/FollowPath.action | 12 ++++++------ nav2_msgs/msg/TrackingError.msg | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 44646d4a2cb..01fae3a857a 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,7 +22,7 @@ std_msgs/Empty result uint16 error_code string error_msg --- -#feedback definition -#Real-time tracking_error and decides 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/TrackingError tracking_error \ No newline at end of file +# 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/TrackingError tracking_error diff --git a/nav2_msgs/msg/TrackingError.msg b/nav2_msgs/msg/TrackingError.msg index 80b1e2a7ebf..f4758517fc0 100644 --- a/nav2_msgs/msg/TrackingError.msg +++ b/nav2_msgs/msg/TrackingError.msg @@ -7,4 +7,4 @@ float32 tracking_error uint32 current_path_index geometry_msgs/PoseStamped robot_pose float32 distance_to_goal -float32 speed \ No newline at end of file +float32 speed From 2108238e4b74f81a69db0353555a322e8dd6da7e Mon Sep 17 00:00:00 2001 From: silanus23 Date: Sat, 20 Sep 2025 01:23:25 +0300 Subject: [PATCH 23/70] last cpplint Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 5d44dc3471a..27b17ac16f0 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -778,7 +778,6 @@ void ControllerServer::publishTrackingState() getThresholdedTwist(odom_sub_->getRawTwist()).linear.y); current_tracking_error_ = *tracking_error_msg; tracking_error_pub_->publish(std::move(tracking_error_msg)); - } void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) From 53cfc5d567ffe3a43e0acc7690e1ce6bbcce4537 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Sat, 20 Sep 2025 02:00:48 +0300 Subject: [PATCH 24/70] frame check for distance_to_goal Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 1 + nav2_controller/src/controller_server.cpp | 19 +++++++++++++++---- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 8b94970dc9c..acb15e2045d 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -277,6 +277,7 @@ class ControllerServer : public nav2::LifecycleNode double search_window_; int start_index_; + double current_distance_to_goal_; double failure_tolerance_; bool use_realtime_priority_; bool publish_zero_velocity_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 27b17ac16f0..326d1b0b161 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -44,6 +44,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"}, + current_distance_to_goal_(0.0), costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -747,7 +748,7 @@ void ControllerServer::publishTrackingState() return; } -// Frame checks + // Frame checks geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; if (!nav2_util::transformPoseInTargetFrame( robot_pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), @@ -757,14 +758,24 @@ void ControllerServer::publishTrackingState() return; } + geometry_msgs::msg::PoseStamped transformed_end_pose; + if (!nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), + robot_pose.header.frame_id, costmap_ros_->getTransformTolerance())) + { + RCLCPP_WARN(get_logger(), "Failed to transform goal pose to robot frame."); + return; + } else { + current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( + robot_pose, transformed_end_pose); + } + const auto path_search_result = nav2_util::distance_from_path( current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); // Set the current closest idx to the next cycle's start start_index_ = path_search_result.closest_segment_index; - double current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(robot_pose, - end_pose_); auto tracking_error_msg = std::make_unique(); @@ -772,7 +783,7 @@ void ControllerServer::publishTrackingState() tracking_error_msg->tracking_error = path_search_result.distance; tracking_error_msg->current_path_index = start_index_; tracking_error_msg->robot_pose = robot_pose; - tracking_error_msg->distance_to_goal = current_distance_to_goal; + tracking_error_msg->distance_to_goal = current_distance_to_goal_; tracking_error_msg->speed = std::hypot( getThresholdedTwist(odom_sub_->getRawTwist()).linear.x, getThresholdedTwist(odom_sub_->getRawTwist()).linear.y); From 12ff164b717baf7cbf7cd93688fba58a2f8bb80d Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 22 Sep 2025 20:52:35 +0300 Subject: [PATCH 25/70] fixes for follow_path Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 2 +- nav2_controller/src/controller_server.cpp | 144 +++++++++--------- 2 files changed, 75 insertions(+), 71 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index acb15e2045d..b4c193682b0 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -275,7 +275,7 @@ class ControllerServer : public nav2::LifecycleNode double min_y_velocity_threshold_; double min_theta_velocity_threshold_; double search_window_; - int start_index_; + size_t start_index_; double current_distance_to_goal_; double failure_tolerance_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 326d1b0b161..c988fc1c92e 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -44,6 +44,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"}, + search_window_(2.0), + start_index_(0), current_distance_to_goal_(0.0), costmap_update_timeout_(300ms) { @@ -68,6 +70,8 @@ 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(20.0)); + // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, @@ -252,7 +256,25 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) speed_limit_sub_ = create_subscription( speed_limit_topic, std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); - + + get_parameter("search_window", search_window_); + // TODO: Add an RCLCPP INFO here + + // Initialize empty tracking error message + current_tracking_error_.header.frame_id = ""; + current_tracking_error_.tracking_error = 0.0; + current_tracking_error_.current_path_index = 0; + current_tracking_error_.distance_to_goal = 0.0; + current_tracking_error_.speed = 0.0; + // Initialize robot_pose with zeros + current_tracking_error_.robot_pose.header.frame_id = ""; + current_tracking_error_.robot_pose.pose.position.x = 0.0; + current_tracking_error_.robot_pose.pose.position.y = 0.0; + current_tracking_error_.robot_pose.pose.position.z = 0.0; + current_tracking_error_.robot_pose.pose.orientation.w = 1.0; + current_tracking_error_.robot_pose.pose.orientation.x = 0.0; + current_tracking_error_.robot_pose.pose.orientation.y = 0.0; + current_tracking_error_.robot_pose.pose.orientation.z = 0.0; return nav2::CallbackReturn::SUCCESS; } @@ -271,11 +293,11 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } vel_publisher_->on_activate(); action_server_->activate(); - auto node = shared_from_this(); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&ControllerServer::dynamicParametersCallback, this, _1)); + tracking_error_pub_->on_activate(); // create bond connection createBond(); @@ -501,8 +523,6 @@ void ControllerServer::computeControl() updateGlobalPath(); - publishTrackingState(); - computeAndPublishVelocity(); if (isGoalReached()) { @@ -646,8 +666,6 @@ void ControllerServer::computeAndPublishVelocity() last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); cmd_vel_2d.header.stamp = last_valid_cmd_time_; - // Only no valid control exception types are valid to attempt to have control patience, as - // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { RCLCPP_WARN(this->get_logger(), "%s", e.what()); @@ -672,21 +690,62 @@ 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())) - { - throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); + // Compute and publish tracking error + if (!current_path_.poses.empty() && current_path_.poses.size() >= 2) { + + // Transform robot pose to path frame + 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())) + { + // Transform goal pose to robot frame for distance calculation + geometry_msgs::msg::PoseStamped transformed_end_pose; + if (nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), + pose.header.frame_id, costmap_ros_->getTransformTolerance())) + { + current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( + pose, transformed_end_pose); + } + + try { + // Compute path search result - always start from index 0 + const auto path_search_result = nav2_util::distance_from_path( + current_path_, robot_pose_in_path_frame.pose, 0, search_window_); + + // Always reset to 0 for next cycle (to start from beginning) + start_index_ = 0; + + // Create tracking error message + auto tracking_error_msg = std::make_unique(); + tracking_error_msg->header = pose.header; + tracking_error_msg->tracking_error = path_search_result.distance; + tracking_error_msg->current_path_index = path_search_result.closest_segment_index; + tracking_error_msg->robot_pose = pose; + tracking_error_msg->distance_to_goal = current_distance_to_goal_; + tracking_error_msg->speed = std::hypot(twist.linear.x, twist.linear.y); + + // Update current tracking error and publish + current_tracking_error_ = *tracking_error_msg; + + if (tracking_error_pub_->is_activated()) { + tracking_error_pub_->publish(std::move(tracking_error_msg)); + } + + } catch (const std::exception& e) { + RCLCPP_WARN(get_logger(), "Exception during tracking error computation: %s", e.what()); + } + } } + // Publish action feedback std::shared_ptr feedback = std::make_shared(); - feedback->tracking_error = current_tracking_error_; action_server_->publish_feedback(feedback); } + void ControllerServer::updateGlobalPath() { if (action_server_->is_preempt_requested()) { @@ -731,66 +790,11 @@ void ControllerServer::updateGlobalPath() action_server_->terminate_current(result); return; } + start_index_ = 0; setPlannerPath(goal->path); } } -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; - } - - // Frame checks - geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; - if (!nav2_util::transformPoseInTargetFrame( - robot_pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), - current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) - { - RCLCPP_WARN(get_logger(), "Failed to transform robot pose to path frame."); - return; - } - - geometry_msgs::msg::PoseStamped transformed_end_pose; - if (!nav2_util::transformPoseInTargetFrame( - end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), - robot_pose.header.frame_id, costmap_ros_->getTransformTolerance())) - { - RCLCPP_WARN(get_logger(), "Failed to transform goal pose to robot frame."); - return; - } else { - current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( - robot_pose, transformed_end_pose); - } - - const auto path_search_result = nav2_util::distance_from_path( - current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); - - // Set the current closest idx to the next cycle's start - start_index_ = path_search_result.closest_segment_index; - - - auto tracking_error_msg = std::make_unique(); - - tracking_error_msg->header = robot_pose.header; - tracking_error_msg->tracking_error = path_search_result.distance; - tracking_error_msg->current_path_index = start_index_; - tracking_error_msg->robot_pose = robot_pose; - tracking_error_msg->distance_to_goal = current_distance_to_goal_; - tracking_error_msg->speed = std::hypot( - getThresholdedTwist(odom_sub_->getRawTwist()).linear.x, - getThresholdedTwist(odom_sub_->getRawTwist()).linear.y); - current_tracking_error_ = *tracking_error_msg; - tracking_error_pub_->publish(std::move(tracking_error_msg)); -} - void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { auto cmd_vel = std::make_unique(velocity); From 1179fd2deee463d15760fde5ef40d3d2df8c7280 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 22 Sep 2025 22:38:58 +0300 Subject: [PATCH 26/70] controller linting Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 27 ++++------------------- 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c988fc1c92e..1eae92ef295 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -256,25 +256,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) speed_limit_sub_ = create_subscription( speed_limit_topic, std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); - + get_parameter("search_window", search_window_); - // TODO: Add an RCLCPP INFO here - - // Initialize empty tracking error message - current_tracking_error_.header.frame_id = ""; - current_tracking_error_.tracking_error = 0.0; - current_tracking_error_.current_path_index = 0; - current_tracking_error_.distance_to_goal = 0.0; - current_tracking_error_.speed = 0.0; - // Initialize robot_pose with zeros - current_tracking_error_.robot_pose.header.frame_id = ""; - current_tracking_error_.robot_pose.pose.position.x = 0.0; - current_tracking_error_.robot_pose.pose.position.y = 0.0; - current_tracking_error_.robot_pose.pose.position.z = 0.0; - current_tracking_error_.robot_pose.pose.orientation.w = 1.0; - current_tracking_error_.robot_pose.pose.orientation.x = 0.0; - current_tracking_error_.robot_pose.pose.orientation.y = 0.0; - current_tracking_error_.robot_pose.pose.orientation.z = 0.0; return nav2::CallbackReturn::SUCCESS; } @@ -692,7 +675,6 @@ void ControllerServer::computeAndPublishVelocity() // Compute and publish tracking error if (!current_path_.poses.empty() && current_path_.poses.size() >= 2) { - // Transform robot pose to path frame geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; if (nav2_util::transformPoseInTargetFrame( @@ -725,15 +707,14 @@ void ControllerServer::computeAndPublishVelocity() tracking_error_msg->robot_pose = pose; tracking_error_msg->distance_to_goal = current_distance_to_goal_; tracking_error_msg->speed = std::hypot(twist.linear.x, twist.linear.y); - + // Update current tracking error and publish current_tracking_error_ = *tracking_error_msg; - + if (tracking_error_pub_->is_activated()) { tracking_error_pub_->publish(std::move(tracking_error_msg)); } - - } catch (const std::exception& e) { + } catch (const std::exception & e) { RCLCPP_WARN(get_logger(), "Exception during tracking error computation: %s", e.what()); } } From ddc47b5f7a75a0b9dfb2dc5cf07afde815221e91 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 24 Sep 2025 20:42:37 +0300 Subject: [PATCH 27/70] changing tracking_error to tracking_error_feedback Signed-off-by: silanus23 --- .../include/nav2_controller/controller_server.hpp | 10 +++------- nav2_controller/src/controller_server.cpp | 9 +++------ nav2_msgs/CMakeLists.txt | 2 +- nav2_msgs/action/FollowPath.action | 2 +- .../{TrackingError.msg => TrackingErrorFeedback.msg} | 0 5 files changed, 8 insertions(+), 15 deletions(-) rename nav2_msgs/msg/{TrackingError.msg => TrackingErrorFeedback.msg} (100%) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index b4c193682b0..658dd1b5135 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -28,7 +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_error.hpp" +#include "nav2_msgs/msg/tracking_error_feedback.hpp" #include "nav2_msgs/msg/speed_limit.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_ros_common/simple_action_server.hpp" @@ -173,10 +173,6 @@ class ControllerServer : public nav2::LifecycleNode * @param velocity Twist velocity to be published */ void publishVelocity(const geometry_msgs::msg::TwistStamped & velocity); - /** - * @brief Calculates and publishes the TrackingError's all components - */ - void publishTrackingState(); /** * @brief Calls velocity publisher to publish zero velocity */ @@ -241,7 +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_error_pub_; + nav2::Publisher::SharedPtr tracking_error_pub_; // Progress Checker Plugin pluginlib::ClassLoader progress_checker_loader_; @@ -293,7 +289,7 @@ class ControllerServer : public nav2::LifecycleNode nav_msgs::msg::Path current_path_; // Last tracking error in the form of TrackingError.msg - nav2_msgs::msg::TrackingError current_tracking_error_; + nav2_msgs::msg::TrackingErrorFeedback current_tracking_error_; private: /** diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1eae92ef295..11403ee1693 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -234,7 +234,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); - tracking_error_pub_ = create_publisher("tracking_error", 10); + tracking_error_pub_ = create_publisher("tracking_error", 10); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); // Create the action server that we implement with our followPath method @@ -621,6 +621,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; } @@ -696,11 +697,8 @@ void ControllerServer::computeAndPublishVelocity() const auto path_search_result = nav2_util::distance_from_path( current_path_, robot_pose_in_path_frame.pose, 0, search_window_); - // Always reset to 0 for next cycle (to start from beginning) - start_index_ = 0; - // Create tracking error message - auto tracking_error_msg = std::make_unique(); + auto tracking_error_msg = std::make_unique(); tracking_error_msg->header = pose.header; tracking_error_msg->tracking_error = path_search_result.distance; tracking_error_msg->current_path_index = path_search_result.closest_segment_index; @@ -771,7 +769,6 @@ void ControllerServer::updateGlobalPath() action_server_->terminate_current(result); return; } - start_index_ = 0; setPlannerPath(goal->path); } } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 20c72960c47..cb51470b60c 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -39,7 +39,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/RouteNode.msg" "msg/RouteEdge.msg" "msg/EdgeCost.msg" - "msg/TrackingError.msg" + "msg/TrackingErrorFeedback.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 01fae3a857a..2996715ea12 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -25,4 +25,4 @@ string error_msg # 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/TrackingError tracking_error +nav2_msgs/TrackingErrorFeedback tracking_error diff --git a/nav2_msgs/msg/TrackingError.msg b/nav2_msgs/msg/TrackingErrorFeedback.msg similarity index 100% rename from nav2_msgs/msg/TrackingError.msg rename to nav2_msgs/msg/TrackingErrorFeedback.msg From 15b949f4cac72375d145ddc26343f4c256991d5c Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 24 Sep 2025 20:59:46 +0300 Subject: [PATCH 28/70] adding remaining_path_length in tracking_error_feedback Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 7 +++++-- nav2_msgs/msg/TrackingErrorFeedback.msg | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 11403ee1693..d7993ff286d 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -70,7 +70,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(20.0)); + declare_parameter("search_window", rclcpp::ParameterValue(2.0)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -234,7 +234,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); - tracking_error_pub_ = create_publisher("tracking_error", 10); + tracking_error_pub_ = create_publisher("tracking_error", + 10); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); // Create the action server that we implement with our followPath method @@ -705,6 +706,8 @@ void ControllerServer::computeAndPublishVelocity() tracking_error_msg->robot_pose = pose; tracking_error_msg->distance_to_goal = current_distance_to_goal_; tracking_error_msg->speed = std::hypot(twist.linear.x, twist.linear.y); + tracking_error_msg->remaining_path_length = + nav2_util::geometry_utils::calculate_path_length(current_path_, start_index_); // Update current tracking error and publish current_tracking_error_ = *tracking_error_msg; diff --git a/nav2_msgs/msg/TrackingErrorFeedback.msg b/nav2_msgs/msg/TrackingErrorFeedback.msg index f4758517fc0..a4b6a554d33 100644 --- a/nav2_msgs/msg/TrackingErrorFeedback.msg +++ b/nav2_msgs/msg/TrackingErrorFeedback.msg @@ -8,3 +8,4 @@ uint32 current_path_index geometry_msgs/PoseStamped robot_pose float32 distance_to_goal float32 speed +float32 remaining_path_length From 18644fa4c40204252c492867d42e92ba2038101c Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 25 Sep 2025 02:01:57 +0300 Subject: [PATCH 29/70] Last fix Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d7993ff286d..dd5924eb502 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -694,9 +694,8 @@ void ControllerServer::computeAndPublishVelocity() } try { - // Compute path search result - always start from index 0 const auto path_search_result = nav2_util::distance_from_path( - current_path_, robot_pose_in_path_frame.pose, 0, search_window_); + current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); // Create tracking error message auto tracking_error_msg = std::make_unique(); From 4f1aae1376af1e1c1c1a5549673c253dc7cd11d0 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 25 Sep 2025 14:45:33 +0300 Subject: [PATCH 30/70] start_index_ Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index dd5924eb502..2141fbab612 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -707,6 +707,7 @@ void ControllerServer::computeAndPublishVelocity() tracking_error_msg->speed = std::hypot(twist.linear.x, twist.linear.y); tracking_error_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_error_ = *tracking_error_msg; From 562a471147794bd7f1b2cda7e357a9e540185d22 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 30 Sep 2025 07:58:48 +0300 Subject: [PATCH 31/70] Name changes and optimizations in controller_server and nav2_util Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 4 ---- nav2_controller/src/controller_server.cpp | 19 ++++++------------- .../include/nav2_util/geometry_utils.hpp | 9 ++++++--- nav2_util/include/nav2_util/path_utils.hpp | 9 ++++++++- nav2_util/src/path_utils.cpp | 4 ++-- 5 files changed, 22 insertions(+), 23 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 658dd1b5135..864e9e18331 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -273,7 +273,6 @@ class ControllerServer : public nav2::LifecycleNode double search_window_; size_t start_index_; - double current_distance_to_goal_; double failure_tolerance_; bool use_realtime_priority_; bool publish_zero_velocity_; @@ -288,9 +287,6 @@ class ControllerServer : public nav2::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; - // Last tracking error in the form of TrackingError.msg - nav2_msgs::msg::TrackingErrorFeedback current_tracking_error_; - private: /** * @brief Callback for speed limiting messages diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2141fbab612..91599acdd07 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -44,9 +44,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"}, - search_window_(2.0), start_index_(0), - current_distance_to_goal_(0.0), costmap_update_timeout_(300ms) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -140,6 +138,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 @@ -235,7 +234,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); tracking_error_pub_ = create_publisher("tracking_error", - 10); + ); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); // Create the action server that we implement with our followPath method @@ -258,7 +257,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) speed_limit_topic, std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); - get_parameter("search_window", search_window_); return nav2::CallbackReturn::SUCCESS; } @@ -267,6 +265,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + tracking_error_pub_->on_activate(); const auto costmap_ros_state = costmap_ros_->activate(); if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2::CallbackReturn::FAILURE; @@ -281,7 +280,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&ControllerServer::dynamicParametersCallback, this, _1)); - tracking_error_pub_->on_activate(); // create bond connection createBond(); @@ -689,7 +687,7 @@ void ControllerServer::computeAndPublishVelocity() end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), pose.header.frame_id, costmap_ros_->getTransformTolerance())) { - current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( + double current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( pose, transformed_end_pose); } @@ -710,14 +708,9 @@ void ControllerServer::computeAndPublishVelocity() start_index_ = path_search_result.closest_segment_index; // Update current tracking error and publish - current_tracking_error_ = *tracking_error_msg; + nav2_msgs::msg::TrackingErrorFeedback current_tracking_error_ = *tracking_error_msg; - if (tracking_error_pub_->is_activated()) { - tracking_error_pub_->publish(std::move(tracking_error_msg)); - } - } catch (const std::exception & e) { - RCLCPP_WARN(get_logger(), "Exception during tracking error computation: %s", e.what()); - } + tracking_error_pub_->publish(std::move(tracking_error_msg)); } } diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 5f215e62769..29599621657 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -248,12 +248,15 @@ inline double distanceToPoint( /** * @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 point of target vector - * @param finish End point of target vector + * @param start Starting segment of the path + * @param finish End segment of the path * @return int */ -inline double distance_to_segment( +inline double distance_to_path_segment( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & end) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 797015f29ef..3e7b7bd89c2 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -38,7 +38,14 @@ struct PathSearchResult }; /** * @brief Finds the minimum distance from a robot pose to a path segment. - * + *Especially considering we need to explain to users that if start index + * window length are not speficied, it finds the global nearest path point. + * Else, specify the index to start from during an existing path tracking task and max + * window length to search for (which can be computed based on + * robot's max velocity and the frequency of the server calling this function). + * If calling the function without a fixed iteration rate, + * consider using the global search as long as you don't expect loops in + * the path generated from things like navigate through poses. * 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. diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index 4a45d40090b..d4fdf240a8f 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -44,7 +44,7 @@ PathSearchResult distance_from_path( } if (start_index >= path.poses.size()) { - throw std::invalid_argument( + 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."); @@ -56,7 +56,7 @@ PathSearchResult distance_from_path( break; } - const double current_distance = geometry_utils::distance_to_segment( + const double current_distance = geometry_utils::distance_to_path_segment( robot_pose.position, path.poses[i].pose, path.poses[i + 1].pose); From 3dbad10ffa0a4697ae9d434f3fbbeb48063978dc Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 30 Sep 2025 08:16:22 +0300 Subject: [PATCH 32/70] Changing name to tracking_feedback Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 4 +- nav2_controller/src/controller_server.cpp | 49 ++++++++++--------- nav2_msgs/CMakeLists.txt | 2 +- nav2_msgs/action/FollowPath.action | 2 +- ...ErrorFeedback.msg => TrackingFeedback.msg} | 0 5 files changed, 29 insertions(+), 28 deletions(-) rename nav2_msgs/msg/{TrackingErrorFeedback.msg => TrackingFeedback.msg} (100%) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 864e9e18331..13fa20fd835 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -28,7 +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_error_feedback.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" @@ -237,7 +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_error_pub_; + nav2::Publisher::SharedPtr tracking_feedback_pub_; // Progress Checker Plugin pluginlib::ClassLoader progress_checker_loader_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 91599acdd07..7250e962bc9 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -233,7 +233,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); - tracking_error_pub_ = create_publisher("tracking_error", + tracking_feedback_pub_ = create_publisher("tracking_feedback" ); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); @@ -265,7 +265,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - tracking_error_pub_->on_activate(); + tracking_feedback_pub_->on_activate(); const auto costmap_ros_state = costmap_ros_->activate(); if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2::CallbackReturn::FAILURE; @@ -673,6 +673,8 @@ void ControllerServer::computeAndPublishVelocity() RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); + double current_distance_to_goal_ = 0.0; + nav2_msgs::msg::TrackingFeedback current_tracking_feedback_; // Compute and publish tracking error if (!current_path_.poses.empty() && current_path_.poses.size() >= 2) { // Transform robot pose to path frame @@ -687,36 +689,35 @@ void ControllerServer::computeAndPublishVelocity() end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), pose.header.frame_id, costmap_ros_->getTransformTolerance())) { - double current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( + current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( pose, transformed_end_pose); } - try { - 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_error_msg = std::make_unique(); - tracking_error_msg->header = pose.header; - tracking_error_msg->tracking_error = path_search_result.distance; - tracking_error_msg->current_path_index = path_search_result.closest_segment_index; - tracking_error_msg->robot_pose = pose; - tracking_error_msg->distance_to_goal = current_distance_to_goal_; - tracking_error_msg->speed = std::hypot(twist.linear.x, twist.linear.y); - tracking_error_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 - nav2_msgs::msg::TrackingErrorFeedback current_tracking_error_ = *tracking_error_msg; - - tracking_error_pub_->publish(std::move(tracking_error_msg)); + 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; + + tracking_feedback_pub_->publish(std::move(tracking_feedback_msg)); } } // Publish action feedback std::shared_ptr feedback = std::make_shared(); - feedback->tracking_error = current_tracking_error_; + feedback->tracking_feedback = current_tracking_feedback_; action_server_->publish_feedback(feedback); } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index cb51470b60c..76523e1a3a5 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -39,7 +39,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/RouteNode.msg" "msg/RouteEdge.msg" "msg/EdgeCost.msg" - "msg/TrackingErrorFeedback.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 2996715ea12..48c4b59d079 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -25,4 +25,4 @@ string error_msg # 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/TrackingErrorFeedback tracking_error +nav2_msgs/TrackingFeedback tracking_feedback diff --git a/nav2_msgs/msg/TrackingErrorFeedback.msg b/nav2_msgs/msg/TrackingFeedback.msg similarity index 100% rename from nav2_msgs/msg/TrackingErrorFeedback.msg rename to nav2_msgs/msg/TrackingFeedback.msg From 0771557d1ce07560996359e7179d73490a29a4c1 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 2 Oct 2025 06:32:10 +0300 Subject: [PATCH 33/70] Documentation changes Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 4 +++- nav2_util/include/nav2_util/geometry_utils.hpp | 4 ++-- nav2_util/include/nav2_util/path_utils.hpp | 10 +++++----- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 7250e962bc9..d2bb52d0ae6 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -265,7 +265,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - tracking_feedback_pub_->on_activate(); + tracking_feedback_pub_->on_activate(); const auto costmap_ros_state = costmap_ros_->activate(); if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2::CallbackReturn::FAILURE; @@ -649,6 +649,8 @@ void ControllerServer::computeAndPublishVelocity() last_valid_cmd_time_ = now(); cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); cmd_vel_2d.header.stamp = last_valid_cmd_time_; + // Only no valid control exception types are valid to attempt to have control patience, as + // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { RCLCPP_WARN(this->get_logger(), "%s", e.what()); diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 29599621657..ae7f4ae8677 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -248,9 +248,9 @@ inline double distanceToPoint( /** * @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 diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 3e7b7bd89c2..5d2b1340a7e 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -38,14 +38,14 @@ struct PathSearchResult }; /** * @brief Finds the minimum distance from a robot pose to a path segment. - *Especially considering we need to explain to users that if start index - * window length are not speficied, it finds the global nearest path point. + *Especially considering we need to explain to users that if start index + * window length are not speficied, it finds the global nearest path point. * Else, specify the index to start from during an existing path tracking task and max - * window length to search for (which can be computed based on + * window length to search for (which can be computed based on * robot's max velocity and the frequency of the server calling this function). * If calling the function without a fixed iteration rate, - * consider using the global search as long as you don't expect loops in - * the path generated from things like navigate through poses. + * consider using the global search as long as you don't expect loops in + * the path generated from things like navigate through poses. * 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. From 00b199edd7f9498d8852236f7049f51274a4a616 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 2 Oct 2025 07:17:44 +0300 Subject: [PATCH 34/70] Last lint Signed-off-by: silanus23 --- nav2_util/include/nav2_util/path_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 5d2b1340a7e..1eadf3b6385 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -39,7 +39,7 @@ struct PathSearchResult /** * @brief Finds the minimum distance from a robot pose to a path segment. *Especially considering we need to explain to users that if start index - * window length are not speficied, it finds the global nearest path point. + * window length are not specified, it finds the global nearest path point. * Else, specify the index to start from during an existing path tracking task and max * window length to search for (which can be computed based on * robot's max velocity and the frequency of the server calling this function). From 6c0a1c7844d56d15fca281f33ca1981cc986fac9 Mon Sep 17 00:00:00 2001 From: silanus Date: Thu, 2 Oct 2025 20:53:57 +0300 Subject: [PATCH 35/70] Update nav2_util/include/nav2_util/path_utils.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: silanus --- nav2_util/include/nav2_util/path_utils.hpp | 24 ++++++++-------------- 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp index 1eadf3b6385..e6c1f2ee642 100644 --- a/nav2_util/include/nav2_util/path_utils.hpp +++ b/nav2_util/include/nav2_util/path_utils.hpp @@ -37,23 +37,17 @@ struct PathSearchResult size_t closest_segment_index; }; /** - * @brief Finds the minimum distance from a robot pose to a path segment. - *Especially considering we need to explain to users that if start index - * window length are not specified, it finds the global nearest path point. - * Else, specify the index to start from during an existing path tracking task and max - * window length to search for (which can be computed based on - * robot's max velocity and the frequency of the server calling this function). - * If calling the function without a fixed iteration rate, - * consider using the global search as long as you don't expect loops in - * the path generated from things like navigate through poses. - * 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. + * @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 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. + * @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( From 5ed323243bc81ef3a277d4df0295075a790ea821 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 2 Oct 2025 12:14:54 -0700 Subject: [PATCH 36/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d2bb52d0ae6..06eb16f983f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -233,8 +233,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); - tracking_feedback_pub_ = create_publisher("tracking_feedback" - ); + tracking_feedback_pub_ = create_publisher("tracking_feedback"); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); // Create the action server that we implement with our followPath method From e12aafa9c5af4f44f98555570dc50051e2e390cc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 2 Oct 2025 12:15:10 -0700 Subject: [PATCH 37/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 06eb16f983f..58c5edaf0e0 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -67,7 +67,6 @@ 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 From 3887f578d2e4b55f139644058d8f21dda17e8792 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 2 Oct 2025 12:15:24 -0700 Subject: [PATCH 38/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 58c5edaf0e0..903ecae399d 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -721,7 +721,6 @@ void ControllerServer::computeAndPublishVelocity() action_server_->publish_feedback(feedback); } - void ControllerServer::updateGlobalPath() { if (action_server_->is_preempt_requested()) { From 43cad194dc8be48ed774cddefe30334b5ad87200 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 2 Oct 2025 12:15:41 -0700 Subject: [PATCH 39/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 903ecae399d..b3aa687fb6f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -710,7 +710,6 @@ void ControllerServer::computeAndPublishVelocity() // Update current tracking error and publish current_tracking_feedback_ = *tracking_feedback_msg; - tracking_feedback_pub_->publish(std::move(tracking_feedback_msg)); } } From 6aaa2b530568344f7949d3dace3d360b17b7a056 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 2 Oct 2025 12:16:17 -0700 Subject: [PATCH 40/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index b3aa687fb6f..d77cd0b34a2 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -676,7 +676,7 @@ void ControllerServer::computeAndPublishVelocity() double current_distance_to_goal_ = 0.0; nav2_msgs::msg::TrackingFeedback current_tracking_feedback_; // Compute and publish tracking error - if (!current_path_.poses.empty() && current_path_.poses.size() >= 2) { + if (current_path_.poses.size() >= 2) { // Transform robot pose to path frame geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; if (nav2_util::transformPoseInTargetFrame( From 78770abf82b557bf528fc4721f97d1658d177791 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 3 Oct 2025 14:32:58 +0300 Subject: [PATCH 41/70] Last updates Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 64 +++++++++++------------ nav2_util/src/CMakeLists.txt | 1 - 2 files changed, 31 insertions(+), 34 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d77cd0b34a2..21dfe27c73b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -263,7 +263,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - tracking_feedback_pub_->on_activate(); const auto costmap_ros_state = costmap_ros_->activate(); if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2::CallbackReturn::FAILURE; @@ -275,6 +274,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) vel_publisher_->on_activate(); action_server_->activate(); auto node = shared_from_this(); + tracking_feedback_pub_->on_activate(); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&ControllerServer::dynamicParametersCallback, this, _1)); @@ -672,51 +672,49 @@ void ControllerServer::computeAndPublishVelocity() RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); - - double current_distance_to_goal_ = 0.0; - nav2_msgs::msg::TrackingFeedback current_tracking_feedback_; - // Compute and publish tracking error + double current_distance_to_goal = 0.0; + nav2_msgs::msg::TrackingFeedback current_tracking_feedback; if (current_path_.poses.size() >= 2) { // Transform robot pose to path frame geometry_msgs::msg::PoseStamped robot_pose_in_path_frame; - if (nav2_util::transformPoseInTargetFrame( + if (!nav2_util::transformPoseInTargetFrame( pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) { - // Transform goal pose to robot frame for distance calculation - geometry_msgs::msg::PoseStamped transformed_end_pose; - if (nav2_util::transformPoseInTargetFrame( - end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), - pose.header.frame_id, costmap_ros_->getTransformTolerance())) - { - current_distance_to_goal_ = nav2_util::geometry_utils::euclidean_distance( - pose, transformed_end_pose); - } + 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; + // Transform goal pose to robot frame for distance calculation + geometry_msgs::msg::PoseStamped transformed_end_pose; + + current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( + pose, transformed_end_pose); + + 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)); } } // Publish action feedback std::shared_ptr feedback = std::make_shared(); - feedback->tracking_feedback = current_tracking_feedback_; + feedback->tracking_feedback = current_tracking_feedback; action_server_->publish_feedback(feedback); } diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 42d498def0a..d1ee2e86a6e 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/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 From 0b680cf6c503b2b5f9c23d74ee73b198a2859208 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 3 Oct 2025 16:06:59 +0300 Subject: [PATCH 42/70] update example_follow_path.py Signed-off-by: silanus23 --- .../nav2_simple_commander/example_follow_path.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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..99367450574 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -14,6 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped +from nav2_msgs.msg import TrackingFeedback from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy @@ -69,9 +70,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 From 5c8582e2e7af4d33c421c3532b2e582f3cda6d8c Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 3 Oct 2025 16:11:13 +0300 Subject: [PATCH 43/70] update example_follow_path.py lint Signed-off-by: silanus23 --- .../nav2_simple_commander/example_follow_path.py | 1 - 1 file changed, 1 deletion(-) 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 99367450574..a2aae33262f 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -14,7 +14,6 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_msgs.msg import TrackingFeedback from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy From f22edea5e1d0004f3cf3fcf7b4bc7e480f6eabc1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 3 Oct 2025 10:32:29 -0700 Subject: [PATCH 44/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 21dfe27c73b..8e84a02ad3f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -673,6 +673,7 @@ void ControllerServer::computeAndPublishVelocity() RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); double current_distance_to_goal = 0.0; + nav2_msgs::msg::TrackingFeedback current_tracking_feedback; if (current_path_.poses.size() >= 2) { // Transform robot pose to path frame From 3e83bf7dac4382f0e22580480e4302abb4c92408 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 3 Oct 2025 10:32:40 -0700 Subject: [PATCH 45/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8e84a02ad3f..81f2105c98f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -687,7 +687,6 @@ void ControllerServer::computeAndPublishVelocity() // Transform goal pose to robot frame for distance calculation geometry_msgs::msg::PoseStamped transformed_end_pose; - current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( pose, transformed_end_pose); From b7ecaaff51978c15fd76d44ea97a748eeba0fe0a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 3 Oct 2025 10:33:06 -0700 Subject: [PATCH 46/70] Update nav2_util/src/path_utils.cpp Signed-off-by: Steve Macenski --- nav2_util/src/path_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp index d4fdf240a8f..9d5975b65eb 100644 --- a/nav2_util/src/path_utils.cpp +++ b/nav2_util/src/path_utils.cpp @@ -74,9 +74,9 @@ PathSearchResult distance_from_path( 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; From e2229aa21a5b62c08169c8bdf08e81498bb5eae0 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 3 Oct 2025 21:59:14 +0300 Subject: [PATCH 47/70] ordering problem Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 81f2105c98f..95e507ac896 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -229,10 +229,10 @@ 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); - tracking_feedback_pub_ = create_publisher("tracking_feedback"); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); // Create the action server that we implement with our followPath method @@ -272,9 +272,9 @@ 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(); - tracking_feedback_pub_->on_activate(); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&ControllerServer::dynamicParametersCallback, this, _1)); From 8b080c8d8b74c832b24d83b50c084e295b662001 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 3 Oct 2025 23:27:14 +0300 Subject: [PATCH 48/70] added deactivate to tracking_feedback_pub_ Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 95e507ac896..689002c2e5c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -307,6 +307,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); + tracking_error_pub_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); From aed0f1ae49990695d06062814f9eabbb359732cc Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 3 Oct 2025 23:31:20 +0300 Subject: [PATCH 49/70] typo Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 689002c2e5c..813da4ed8cd 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -307,7 +307,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); - tracking_error_pub_->on_deactivate(); + tracking_feedback_pub_->on_deactivate(); remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); From 4e3cb904cb44b0e1a75940ec9108d361f948a5e4 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 6 Oct 2025 23:58:34 +0300 Subject: [PATCH 50/70] end_pose transform Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 813da4ed8cd..ec17811ef96 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -688,9 +688,14 @@ void ControllerServer::computeAndPublishVelocity() // Transform goal pose to robot frame for distance calculation geometry_msgs::msg::PoseStamped transformed_end_pose; + if (!nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), + pose.header.frame_id, costmap_ros_->getTransformTolerance())) + { + throw nav2_core::ControllerTFError("Failed to transform goal pose to robot frame"); + } current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( - pose, transformed_end_pose); - + pose, transformed_end_pose); const auto path_search_result = nav2_util::distance_from_path( current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); From 828567a803087da49af61d888fb317bf77293fec Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 8 Oct 2025 19:43:51 +0300 Subject: [PATCH 51/70] creating the member variable end_pose_global_ and deleting nav_2d_msgs Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 3 +++ nav2_controller/src/controller_server.cpp | 19 +++++++------------ nav2_util/test/CMakeLists.txt | 3 --- 3 files changed, 10 insertions(+), 15 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 13fa20fd835..86e85ffd725 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -281,6 +281,9 @@ class ControllerServer : public nav2::LifecycleNode // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; + // end_pose_ transformed to global version + geometry_msgs::msg::PoseStamped end_pose_global_; + // 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 ec17811ef96..0d5922d9716 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -686,16 +686,8 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); } - // Transform goal pose to robot frame for distance calculation - geometry_msgs::msg::PoseStamped transformed_end_pose; - if (!nav2_util::transformPoseInTargetFrame( - end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), - pose.header.frame_id, costmap_ros_->getTransformTolerance())) - { - throw nav2_core::ControllerTFError("Failed to transform goal pose to robot frame"); - } current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( - pose, transformed_end_pose); + pose, end_pose_global_); const auto path_search_result = nav2_util::distance_from_path( current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); @@ -820,14 +812,17 @@ bool ControllerServer::isGoalReached() geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist()); - geometry_msgs::msg::PoseStamped transformed_end_pose; + // Use the current robot pose's timestamp for the transformation end_pose_.header.stamp = pose.header.stamp; + + // Transform the goal pose and store it in the member variable 'end_pose_global_' nav2_util::transformPoseInTargetFrame( - end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(), + end_pose_, end_pose_global_, *costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); + // Use the now-populated member variable 'end_pose_global_' in the goal checker return goal_checkers_[current_goal_checker_]->isGoalReached( - pose.pose, transformed_end_pose.pose, + pose.pose, end_pose_global_.pose, velocity); } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 629378c61fe..b7f290f8e78 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -37,6 +37,3 @@ 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}) From bf2b40ed4413692fac5658ba3c873b2ea511d466 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 8 Oct 2025 19:46:59 +0300 Subject: [PATCH 52/70] deleting unnecessary transform Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 0d5922d9716..ac88cd585a4 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -677,15 +677,6 @@ void ControllerServer::computeAndPublishVelocity() nav2_msgs::msg::TrackingFeedback current_tracking_feedback; if (current_path_.poses.size() >= 2) { - // Transform robot pose to path frame - 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())) - { - throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); - } - current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( pose, end_pose_global_); const auto path_search_result = nav2_util::distance_from_path( From eba514d7f9d8ebcfd96521d7653e9c1b8ea517ca Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 8 Oct 2025 19:51:24 +0300 Subject: [PATCH 53/70] typo in controller Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ac88cd585a4..4357181ff1f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -680,7 +680,7 @@ void ControllerServer::computeAndPublishVelocity() current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( pose, end_pose_global_); const auto path_search_result = nav2_util::distance_from_path( - current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); + current_path_, end_pose_global_.pose, start_index_, search_window_); // Create tracking error message auto tracking_feedback_msg = std::make_unique(); From 46ee06a77a8499fbb3656d57f49f8c084f749974 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 8 Oct 2025 20:10:03 +0300 Subject: [PATCH 54/70] readding remaining path Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 4357181ff1f..157317842ba 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -679,8 +679,18 @@ void ControllerServer::computeAndPublishVelocity() if (current_path_.poses.size() >= 2) { current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( pose, end_pose_global_); + + // 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())) + { + throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); + } + const auto path_search_result = nav2_util::distance_from_path( - current_path_, end_pose_global_.pose, start_index_, search_window_); + current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); // Create tracking error message auto tracking_feedback_msg = std::make_unique(); From ea1aa84b40d2400e61223c9cc7d83d80aeeda33e Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 8 Oct 2025 20:42:47 +0300 Subject: [PATCH 55/70] placement of transformation Signed-off-by: silanus23 --- .../nav2_controller/controller_server.hpp | 2 +- nav2_controller/src/controller_server.cpp | 24 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 86e85ffd725..9d408abcedb 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -282,7 +282,7 @@ class ControllerServer : public nav2::LifecycleNode geometry_msgs::msg::PoseStamped end_pose_; // end_pose_ transformed to global version - geometry_msgs::msg::PoseStamped end_pose_global_; + 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 157317842ba..aa9517fa516 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -676,11 +676,20 @@ void ControllerServer::computeAndPublishVelocity() double current_distance_to_goal = 0.0; nav2_msgs::msg::TrackingFeedback current_tracking_feedback; + + // Use the current robot pose's timestamp for the transformation + end_pose_.header.stamp = pose.header.stamp; + + // Transform the goal pose and store it in the member variable 'transformed_end_pose_' + nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); + if (current_path_.poses.size() >= 2) { current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( - pose, end_pose_global_); + pose, transformed_end_pose_); - // Transform robot pose to path frame for path tracking calculations + // 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(), @@ -813,17 +822,8 @@ bool ControllerServer::isGoalReached() geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist()); - // Use the current robot pose's timestamp for the transformation - end_pose_.header.stamp = pose.header.stamp; - - // Transform the goal pose and store it in the member variable 'end_pose_global_' - nav2_util::transformPoseInTargetFrame( - end_pose_, end_pose_global_, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); - - // Use the now-populated member variable 'end_pose_global_' in the goal checker return goal_checkers_[current_goal_checker_]->isGoalReached( - pose.pose, end_pose_global_.pose, + pose.pose, transformed_end_pose_.pose, velocity); } From 7c710aaefd66d1b4b68f8102e45ef32cd54e192e Mon Sep 17 00:00:00 2001 From: silanus23 Date: Wed, 8 Oct 2025 21:22:25 +0300 Subject: [PATCH 56/70] unnecessary comments Signed-off-by: silanus23 --- nav2_controller/include/nav2_controller/controller_server.hpp | 1 - nav2_controller/src/controller_server.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 9d408abcedb..a4e33a4249b 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -281,7 +281,6 @@ class ControllerServer : public nav2::LifecycleNode // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; - // end_pose_ transformed to global version geometry_msgs::msg::PoseStamped transformed_end_pose_; // Last time the controller generated a valid command diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index aa9517fa516..4605f8774f8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -680,7 +680,6 @@ void ControllerServer::computeAndPublishVelocity() // Use the current robot pose's timestamp for the transformation end_pose_.header.stamp = pose.header.stamp; - // Transform the goal pose and store it in the member variable 'transformed_end_pose_' nav2_util::transformPoseInTargetFrame( end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); From e2fabd1ed6d2617f2edaed7e05db2c8806bdf9ee Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 15:10:47 +0300 Subject: [PATCH 57/70] tests added Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 16 +++++------ nav2_util/test/test_geometry_utils.cpp | 35 +++++++++++++++++++++++ nav2_util/test/test_path_utils.cpp | 30 +++++++++++++++++++ 3 files changed, 73 insertions(+), 8 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 4605f8774f8..4e74a4efbd9 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -673,7 +673,6 @@ void ControllerServer::computeAndPublishVelocity() RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); - double current_distance_to_goal = 0.0; nav2_msgs::msg::TrackingFeedback current_tracking_feedback; @@ -685,22 +684,23 @@ void ControllerServer::computeAndPublishVelocity() costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); if (current_path_.poses.size() >= 2) { + double current_distance_to_goal = 0.0; current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( - pose, transformed_end_pose_); + pose, transformed_end_pose_); - // Transform robot pose to path frame for path tracking calculations + // 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())) + pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), + current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) { 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_); + current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_); - // Create tracking error message + // 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; @@ -712,7 +712,7 @@ void ControllerServer::computeAndPublishVelocity() 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 + // 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)); 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 index 5655e87e78e..4e1ab27e52f 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -332,3 +332,33 @@ TEST(PathUtilsWindowedTest, EdgeCases) 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, ThrowsOnStartIndexOutOfBounds) +{ + nav_msgs::msg::Path path; + path.poses.push_back(createPoseStamped(0.0, 0.0)); + path.poses.push_back(createPoseStamped(1.0, 0.0)); + 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, 2, 5.0), + std::runtime_error); + + // start_index is way out of bounds + EXPECT_THROW( + nav2_util::distance_from_path(path, robot_pose, 100, 5.0), + std::runtime_error); +} + +TEST(PathUtilsTest, SinglePosePathReturnsCorrectDistance) +{ + 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); +} From 9af2ca9d258bae9f6f450563075b366903a12608 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 16:06:53 +0300 Subject: [PATCH 58/70] Increased test coverage Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 33 ++++++++++++++++++------------ 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 4e1ab27e52f..dc5d06d011d 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -333,7 +333,26 @@ TEST(PathUtilsWindowedTest, EdgeCases) EXPECT_NEAR(std::abs(result.distance), 5.0, 0.01); } -TEST(PathUtilsTest, ThrowsOnStartIndexOutOfBounds) +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); + 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)); @@ -350,15 +369,3 @@ TEST(PathUtilsTest, ThrowsOnStartIndexOutOfBounds) nav2_util::distance_from_path(path, robot_pose, 100, 5.0), std::runtime_error); } - -TEST(PathUtilsTest, SinglePosePathReturnsCorrectDistance) -{ - 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); -} From 7365eea366701b726eba662aa75a51dd007ab0a0 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 17:01:27 +0300 Subject: [PATCH 59/70] Every part of the result is covered Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index dc5d06d011d..f05bf132228 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -196,12 +196,22 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) 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); + EXPECT_EQ(result.closest_point.x, 0.0); + EXPECT_EQ(result.closest_point.y, 0.0); + EXPECT_EQ(result.closest_point.z, 0.0); nav_msgs::msg::Path single_point_path; single_point_path.poses.push_back(createPoseStamped(0.0, 0.0)); result = nav2_util::distance_from_path(single_point_path, robot_pose); + // Check all fields for single point path EXPECT_NEAR(std::abs(result.distance), 7.071, 0.01); + EXPECT_EQ(result.closest_segment_index, 0); + EXPECT_NEAR(result.closest_point.x, 0.0, 1e-6); + EXPECT_NEAR(result.closest_point.y, 0.0, 1e-6); + EXPECT_NEAR(result.closest_point.z, 0.0, 1e-6); } TEST_F(CuttingCornerTest, TrajectoryCutsCorner) @@ -338,9 +348,14 @@ 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()); + EXPECT_EQ(result.closest_segment_index, 0); + EXPECT_EQ(result.closest_point.x, 0.0); + EXPECT_EQ(result.closest_point.y, 0.0); + EXPECT_EQ(result.closest_point.z, 0.0); } - TEST(PathUtilsTest, FourArgSinglePointPath) { nav_msgs::msg::Path path; From 382ae420bebbd3b19219b99fb9788a5cbb39096b Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 17:10:24 +0300 Subject: [PATCH 60/70] Increasing out of bounds index Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index f05bf132228..e079ebef349 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -372,15 +372,16 @@ 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, 2, 5.0), + 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), + nav2_util::distance_from_path(path, robot_pose, 100, 5.0), // 100 >= 3 std::runtime_error); } From 28dd40b1ebaf10d0d6cf293f73ea04a75055cf1a Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 17:11:17 +0300 Subject: [PATCH 61/70] lint Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index e079ebef349..58e793cfd08 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -356,6 +356,7 @@ TEST(PathUtilsTest, FourArgEmptyPath) EXPECT_EQ(result.closest_point.y, 0.0); EXPECT_EQ(result.closest_point.z, 0.0); } + TEST(PathUtilsTest, FourArgSinglePointPath) { nav_msgs::msg::Path path; From 9de776bee0aa692b3d6224a4ee08b101bdbe9603 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 18:04:59 +0300 Subject: [PATCH 62/70] add missing line to cmake Signed-off-by: silanus23 --- nav2_util/test/CMakeLists.txt | 3 +++ nav2_util/test/test_path_utils.cpp | 10 ---------- 2 files changed, 3 insertions(+), 10 deletions(-) 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_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index 58e793cfd08..ddbe7fd55fd 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -199,9 +199,6 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) // Check all fields of the result for empty path EXPECT_EQ(result.distance, std::numeric_limits::max()); EXPECT_EQ(result.closest_segment_index, 0); - EXPECT_EQ(result.closest_point.x, 0.0); - EXPECT_EQ(result.closest_point.y, 0.0); - EXPECT_EQ(result.closest_point.z, 0.0); nav_msgs::msg::Path single_point_path; single_point_path.poses.push_back(createPoseStamped(0.0, 0.0)); @@ -209,9 +206,6 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) // Check all fields for single point path EXPECT_NEAR(std::abs(result.distance), 7.071, 0.01); EXPECT_EQ(result.closest_segment_index, 0); - EXPECT_NEAR(result.closest_point.x, 0.0, 1e-6); - EXPECT_NEAR(result.closest_point.y, 0.0, 1e-6); - EXPECT_NEAR(result.closest_point.z, 0.0, 1e-6); } TEST_F(CuttingCornerTest, TrajectoryCutsCorner) @@ -351,10 +345,6 @@ TEST(PathUtilsTest, FourArgEmptyPath) // Check all fields of the result for empty path EXPECT_EQ(result.distance, std::numeric_limits::max()); - EXPECT_EQ(result.closest_segment_index, 0); - EXPECT_EQ(result.closest_point.x, 0.0); - EXPECT_EQ(result.closest_point.y, 0.0); - EXPECT_EQ(result.closest_point.z, 0.0); } TEST(PathUtilsTest, FourArgSinglePointPath) From 90b835fb7dc482e48f326384750b03ad6acb395e Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 9 Oct 2025 18:20:14 +0300 Subject: [PATCH 63/70] unnecessary test Signed-off-by: silanus23 --- nav2_util/test/test_path_utils.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp index ddbe7fd55fd..30a16bfb30c 100644 --- a/nav2_util/test/test_path_utils.cpp +++ b/nav2_util/test/test_path_utils.cpp @@ -190,7 +190,7 @@ class RetracingPathTest : public ::testing::Test std::vector robot_trajectory; }; -TEST(PathUtilsTest, EmptyAndSinglePointPaths) +TEST(PathUtilsTest, EmptyPath) { auto robot_pose = createPose(5.0, 5.0); nav_msgs::msg::Path empty_path; @@ -199,13 +199,6 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths) // Check all fields of the result for empty path EXPECT_EQ(result.distance, std::numeric_limits::max()); EXPECT_EQ(result.closest_segment_index, 0); - - nav_msgs::msg::Path single_point_path; - single_point_path.poses.push_back(createPoseStamped(0.0, 0.0)); - result = nav2_util::distance_from_path(single_point_path, robot_pose); - // Check all fields for single point path - EXPECT_NEAR(std::abs(result.distance), 7.071, 0.01); - EXPECT_EQ(result.closest_segment_index, 0); } TEST_F(CuttingCornerTest, TrajectoryCutsCorner) From ef33b103c2f9756af7cd52b0f777816c27822fbe Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 10 Oct 2025 01:36:30 +0300 Subject: [PATCH 64/70] unexptected styling and last functional change Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 24 +++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 4e74a4efbd9..d8c7a0cd2f8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -679,19 +679,23 @@ void ControllerServer::computeAndPublishVelocity() // Use the current robot pose's timestamp for the transformation end_pose_.header.stamp = pose.header.stamp; - nav2_util::transformPoseInTargetFrame( - end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(), - costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance()); + if (nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance())) + { + // Transform succeeded + } else { + RCLCPP_WARN(get_logger(), "Failed to transform end pose to global frame"); + } if (current_path_.poses.size() >= 2) { - double current_distance_to_goal = 0.0; - current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( - pose, transformed_end_pose_); + double current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( + pose, transformed_end_pose_); - // Transform robot pose to path frame for path tracking calculations + // 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(), + pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(), current_path_.header.frame_id, costmap_ros_->getTransformTolerance())) { throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame"); @@ -700,7 +704,7 @@ void ControllerServer::computeAndPublishVelocity() 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 + // 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; @@ -712,7 +716,7 @@ void ControllerServer::computeAndPublishVelocity() 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 + // 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)); From 2f94b48fea5ad8174f18566bb54bca4ead462fa9 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 10 Oct 2025 13:42:54 +0300 Subject: [PATCH 65/70] simplifying the tranformation of end_pose Signed-off-by: silanus23 --- nav2_controller/src/controller_server.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d8c7a0cd2f8..1fb7ca23273 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -679,13 +679,11 @@ void ControllerServer::computeAndPublishVelocity() // 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())) + if (!nav2_util::transformPoseInTargetFrame( + end_pose_, transformed_end_pose_, *costmap_ros_->getTfBuffer(), + costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance())) { - // Transform succeeded - } else { - RCLCPP_WARN(get_logger(), "Failed to transform end pose to global frame"); + throw nav2_core::ControllerTFError("Failed to transform end pose to global frame"); } if (current_path_.poses.size() >= 2) { From 24e5052c1bd2ad2f2609a1162a183a52fe715334 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 10 Oct 2025 13:59:55 +0300 Subject: [PATCH 66/70] doxygen fix Signed-off-by: silanus23 --- .../include/nav2_util/geometry_utils.hpp | 23 ++++--------------- 1 file changed, 4 insertions(+), 19 deletions(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index ae7f4ae8677..e6bb96c834f 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -231,30 +231,15 @@ 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 + * @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, From 84f0e79172515684b595a0fb44bd7c1bdf8249c7 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 10 Oct 2025 14:02:40 +0300 Subject: [PATCH 67/70] doxygen fix Signed-off-by: silanus23 --- nav2_util/include/nav2_util/geometry_utils.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index e6bb96c834f..2e947d4a7cd 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -232,7 +232,11 @@ inline bool isPointInsidePolygon( } /** - * @brief Find the shortest distance to a vector + * @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 degenerate (zero length), + * returns the distance from the point to the segment's start. * * See: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line * From f3563485c399289182bc6071b84f8150558891f6 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 10 Oct 2025 14:03:07 +0300 Subject: [PATCH 68/70] doxygen fix Signed-off-by: silanus23 --- nav2_util/include/nav2_util/geometry_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 2e947d4a7cd..25dc5d32836 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -235,7 +235,7 @@ inline bool isPointInsidePolygon( * @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 degenerate (zero length), + * from the point to the segment in the XY plane. If the segment is too short returns zero, * returns the distance from the point to the segment's start. * * See: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line From d147306ffa5475aeb24d8f580f840cb92a4c1ce7 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 10 Oct 2025 14:46:55 +0300 Subject: [PATCH 69/70] doxygen fix Signed-off-by: silanus23 --- nav2_util/include/nav2_util/geometry_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 25dc5d32836..f4cf4078042 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -235,7 +235,7 @@ inline bool isPointInsidePolygon( * @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 zero, + * 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 From 932ac182aa767603f886b9fa9a1be5d3f4622d3a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 10 Oct 2025 10:08:54 -0700 Subject: [PATCH 70/70] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1fb7ca23273..52c34e0c0a2 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -688,7 +688,7 @@ void ControllerServer::computeAndPublishVelocity() if (current_path_.poses.size() >= 2) { double current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( - pose, transformed_end_pose_); + pose, transformed_end_pose_); // Transform robot pose to path frame for path tracking calculations geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;