diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index be02bcc98f8..539048f8127 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(${PROJECT_NAME} SHARED src/prefer_forward.cpp src/rotate_to_goal.cpp src/twirling.cpp + src/path_hug.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC "$" diff --git a/nav2_dwb_controller/dwb_critics/default_critics.xml b/nav2_dwb_controller/dwb_critics/default_critics.xml index 1a490065406..c390b444069 100644 --- a/nav2_dwb_controller/dwb_critics/default_critics.xml +++ b/nav2_dwb_controller/dwb_critics/default_critics.xml @@ -38,5 +38,9 @@ Penalize trajectories with rotational velocities + + Penalize trajectories according to their distance to the path and their alignment with it. + + diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp new file mode 100644 index 00000000000..81532622862 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp @@ -0,0 +1,81 @@ +// 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 DWB_CRITICS__PATH_HUG_HPP_ +#define DWB_CRITICS__PATH_HUG_HPP_ + +#include + +#include "dwb_core/trajectory_critic.hpp" +#include "nav_msgs/msg/path.hpp" + +namespace dwb_critics +{ +/** + * @class PathHugCritic + * @brief Critic that penalizes trajectories based on their distance from the global path + * + * This critic encourages the robot to stay close to the planned path while allowing + * deviations for obstacle avoidance. Uses a bidirectional search window to handle + * both forward and backward trajectory motion. + */ +class PathHugCritic : public dwb_core::TrajectoryCritic +{ +public: + /** + * @brief Result structure for segment search operations + */ + struct SegmentSearchResult + { + size_t closest_index; ///< Index of the closest path segment + double distance; ///< Distance to the closest segment + }; + + void onInit() override; + + bool prepare( + const geometry_msgs::msg::Pose & pose, + const nav_2d_msgs::msg::Twist2D & vel, + const geometry_msgs::msg::Pose & goal, + const nav_msgs::msg::Path & global_plan) override; + + double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; + +protected: + /** + * @brief Find closest path segment using bidirectional search window + * + * Searches both forward and backward from the hint index to handle trajectories + * that may move in any direction. The search window prevents matching to + * geometrically close but topologically distant path segments. + * + * @param path Global path to search + * @param pose Current pose to find closest segment for + * @param hint_index Starting index for search (typically previous closest segment) + * @param search_window Size of bidirectional search window in meters + * @return SegmentSearchResult containing closest segment index and distance + */ + SegmentSearchResult findClosestSegmentWithLookback( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Pose & pose, + size_t hint_index, + double search_window) const; + + nav_msgs::msg::Path global_path_; + double search_window_; +}; + +} // namespace dwb_critics + +#endif // DWB_CRITICS__PATH_HUG_HPP_ diff --git a/nav2_dwb_controller/dwb_critics/src/path_hug.cpp b/nav2_dwb_controller/dwb_critics/src/path_hug.cpp new file mode 100644 index 00000000000..9ac4b4b9375 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/src/path_hug.cpp @@ -0,0 +1,157 @@ +// 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 "dwb_critics/path_hug.hpp" + +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/path_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace dwb_critics +{ + +void PathHugCritic::onInit() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + nav2::declare_parameter_if_not_declared( + node, dwb_plugin_name_ + "." + name_ + ".search_window", rclcpp::ParameterValue(3.0)); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".search_window", search_window_); + + if (search_window_ <= 0.0) { + throw std::runtime_error{"search_window must be positive"}; + } +} + +bool PathHugCritic::prepare( + const geometry_msgs::msg::Pose & /*pose*/, const nav_2d_msgs::msg::Twist2D & /*vel*/, + const geometry_msgs::msg::Pose & /*goal*/, + const nav_msgs::msg::Path & global_plan) +{ + global_path_ = global_plan; + return true; +} + +PathHugCritic::SegmentSearchResult PathHugCritic::findClosestSegmentWithLookback( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Pose & pose, + size_t hint_index, + double search_window) const +{ + if (path.poses.empty()) { + return {0, 0.0}; + } + + const size_t path_size = path.poses.size(); + + if (path_size == 1) { + double dist = nav2_util::geometry_utils::euclidean_distance( + pose.position, path.poses[0].pose.position); + return {0, dist}; + } + + if (hint_index >= path_size) { + hint_index = path_size - 1; + } + + double min_distance = std::numeric_limits::max(); + size_t closest_index = hint_index; + const double half_window = search_window / 2.0; + + // Search backward from hint_index + double distance_traversed = 0.0; + if (hint_index > 0) { + for (size_t i = hint_index; i > 0; --i) { + if (distance_traversed > half_window) { + break; + } + + const double dist = nav2_util::geometry_utils::distance_to_path_segment( + pose.position, + path.poses[i - 1].pose, + path.poses[i].pose); + + if (dist < min_distance) { + min_distance = dist; + closest_index = i - 1; + } + + distance_traversed += nav2_util::geometry_utils::euclidean_distance( + path.poses[i - 1].pose.position, + path.poses[i].pose.position); + } + } + + // Search forward from hint_index + distance_traversed = 0.0; + for (size_t i = hint_index; i < path_size - 1; ++i) { + if (distance_traversed > half_window) { + break; + } + + const double dist = nav2_util::geometry_utils::distance_to_path_segment( + pose.position, + path.poses[i].pose, + path.poses[i + 1].pose); + + if (dist < min_distance) { + min_distance = dist; + closest_index = i; + } + + distance_traversed += nav2_util::geometry_utils::euclidean_distance( + path.poses[i].pose.position, + path.poses[i + 1].pose.position); + } + + return {closest_index, min_distance}; +} + +double PathHugCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) +{ + if (traj.poses.empty() || global_path_.poses.empty()) { + return 0.0; + } + + // Find initial hint from first trajectory pose + const geometry_msgs::msg::Pose & current_pose = traj.poses[0]; + nav2_util::PathSearchResult search_result = nav2_util::distance_from_path( + global_path_, current_pose); + + size_t start_index = search_result.closest_segment_index; + double total_distance = 0.0; + + // Score each trajectory pose using bidirectional search + for (size_t traj_index = 0; traj_index < traj.poses.size(); traj_index++) { + SegmentSearchResult result = findClosestSegmentWithLookback( + global_path_, traj.poses[traj_index], start_index, search_window_); + + total_distance += result.distance; + start_index = result.closest_index; // Update hint for next iteration + } + + return total_distance / traj.poses.size(); +} + +} // namespace dwb_critics + +PLUGINLIB_EXPORT_CLASS(dwb_critics::PathHugCritic, dwb_core::TrajectoryCritic) diff --git a/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt index 93a89f5571b..050a82d3ace 100644 --- a/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt @@ -33,3 +33,10 @@ target_link_libraries(twirling_tests dwb_core::dwb_core rclcpp::rclcpp ) + +ament_add_gtest(path_hug_tests path_hug_test.cpp) +target_link_libraries(path_hug_tests + dwb_critics + dwb_core::dwb_core + rclcpp::rclcpp +) diff --git a/nav2_dwb_controller/dwb_critics/test/path_hug_test.cpp b/nav2_dwb_controller/dwb_critics/test/path_hug_test.cpp new file mode 100644 index 00000000000..950b8bff588 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/path_hug_test.cpp @@ -0,0 +1,193 @@ +// 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 + +#include "dwb_critics/path_hug.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav_2d_msgs/msg/twist2_d.hpp" +#include "nav_msgs/msg/path.hpp" +#include "dwb_msgs/msg/trajectory2_d.hpp" + +static constexpr double default_scale = 1.0; +static constexpr double default_search_window = 3.0; + +class PathHugCriticTest : public ::testing::Test +{ +protected: + void SetUp() override + { + node_ = std::make_shared("test_node"); + node_->configure(); + node_->activate(); + + costmap_ros_ = std::make_shared("test_global_costmap", "", + false); + costmap_ros_->configure(); + + std::string plugin_name = "FollowPath"; + std::string name = "PathHugCritic"; + critic_ = std::make_shared(); + critic_->initialize(node_, name, plugin_name, costmap_ros_); + } + + geometry_msgs::msg::Pose makePose(double x, double y) + { + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.orientation.w = 1.0; + return pose; + } + + nav_msgs::msg::Path makePath(std::vector> points) + { + nav_msgs::msg::Path path; + for (const auto & pt : points) { + geometry_msgs::msg::PoseStamped ps; + ps.pose = makePose(pt.first, pt.second); + path.poses.push_back(ps); + } + return path; + } + + dwb_msgs::msg::Trajectory2D makeTrajectory(std::vector> points) + { + dwb_msgs::msg::Trajectory2D traj; + for (const auto & pt : points) { + geometry_msgs::msg::Pose pose; + pose.position.x = pt.first; + pose.position.y = pt.second; + pose.orientation.w = 1.0; + traj.poses.push_back(pose); + } + return traj; + } + + std::shared_ptr node_; + std::shared_ptr costmap_ros_; + std::shared_ptr critic_; +}; + +TEST_F(PathHugCriticTest, DefaultParameterInitialization) +{ + EXPECT_EQ( + node_->get_parameter("FollowPath.PathHugCritic.scale").as_double(), + default_scale); + EXPECT_EQ( + node_->get_parameter("FollowPath.PathHugCritic.search_window").as_double(), + default_search_window); +} + +TEST_F(PathHugCriticTest, HandlesEmptyTrajectory) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 0.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(1, 0), path); + + dwb_msgs::msg::Trajectory2D traj; + EXPECT_DOUBLE_EQ(critic_->scoreTrajectory(traj), 0.0); +} + +TEST_F(PathHugCriticTest, HandlesEmptyGlobalPath) +{ + nav_msgs::msg::Path path; + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(1, 0), path); + + dwb_msgs::msg::Trajectory2D traj = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + EXPECT_DOUBLE_EQ(critic_->scoreTrajectory(traj), 0.0); +} + +TEST_F(PathHugCriticTest, ScoresTrajectoryOnPath) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(2, 0), path); + + dwb_msgs::msg::Trajectory2D traj = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + double score = critic_->scoreTrajectory(traj); + EXPECT_DOUBLE_EQ(score, 0.0); +} + +TEST_F(PathHugCriticTest, ScoresTrajectoryNearPath) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(2, 0), path); + + // Trajectory is 0.5m away from path + dwb_msgs::msg::Trajectory2D traj = makeTrajectory({{0.0, 0.5}, {1.0, 0.5}, {2.0, 0.5}}); + double score = critic_->scoreTrajectory(traj); + EXPECT_GT(score, 0.0); + EXPECT_NEAR(score, 0.5, 0.01); +} + +TEST_F(PathHugCriticTest, ScoresTrajectoryFarFromPath) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(2, 0), path); + + dwb_msgs::msg::Trajectory2D traj_near = makeTrajectory({{0.0, 0.5}, {1.0, 0.5}, {2.0, 0.5}}); + dwb_msgs::msg::Trajectory2D traj_far = makeTrajectory({{0.0, 2.0}, {1.0, 2.0}, {2.0, 2.0}}); + + double score_near = critic_->scoreTrajectory(traj_near); + double score_far = critic_->scoreTrajectory(traj_far); + + EXPECT_GT(score_far, score_near); + EXPECT_NEAR(score_near, 0.5, 0.01); + EXPECT_NEAR(score_far, 2.0, 0.01); +} + +TEST_F(PathHugCriticTest, CustomParameterValues) +{ + auto custom_node = std::make_shared("custom_test_node"); + custom_node->configure(); + custom_node->activate(); + + double custom_scale = 50.0; + double custom_search_window = 5.0; + std::string plugin_name = "FollowPath"; + std::string name = "PathHugCritic"; + + nav2::declare_parameter_if_not_declared( + custom_node, plugin_name + "." + name + ".scale", + rclcpp::ParameterValue(custom_scale)); + nav2::declare_parameter_if_not_declared( + custom_node, plugin_name + "." + name + ".search_window", + rclcpp::ParameterValue(custom_search_window)); + + auto custom_critic = std::make_shared(); + auto custom_costmap = std::make_shared("custom_costmap", "", + false); + custom_costmap->configure(); + + custom_critic->initialize(custom_node, name, plugin_name, custom_costmap); + + EXPECT_EQ( + custom_node->get_parameter(plugin_name + "." + name + ".scale").as_double(), + custom_scale); + EXPECT_EQ( + custom_node->get_parameter(plugin_name + "." + name + ".search_window").as_double(), + custom_search_window); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}