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..3968be5eb77 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp @@ -0,0 +1,51 @@ +// 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 "dwb_core/trajectory_critic.hpp" +#include "nav2_util/path_utils.hpp" + +namespace dwb_critics +{ +/** + * @class PathHugCritic + * @brief DWB critic that penalizes trajectories based on their distance from the global path. + * Encourages the robot to "hug" or stay close to the path. + */ +class PathHugCritic : public dwb_core::TrajectoryCritic +{ +public: + 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 getScale() const override; + double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; + +protected: + bool zero_scale_{false}; + nav_msgs::msg::Path global_path_; + size_t start_index_{0}; + geometry_msgs::msg::Pose current_pose_{}; + double average_score_{0.0}; + double search_window_{2.0}; +}; + +} // 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..ec1062c2f59 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/src/path_hug.cpp @@ -0,0 +1,82 @@ +// 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 "dwb_critics/alignment_util.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/path_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_ + ".search_window", rclcpp::ParameterValue(2.0)); + node->get_parameter(dwb_plugin_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; +} + +double PathHugCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) +{ + if (traj.poses.empty() || global_path_.poses.empty()) { + return 0.0; + } + double distance = 0; + + current_pose_ = traj.poses[0]; + nav2_util::PathSearchResult search_result = nav2_util::distance_from_path( + global_path_, current_pose_); + start_index_ = search_result.closest_segment_index; + + for (size_t traj_index = 0; traj_index < traj.poses.size(); traj_index++) { + search_result = nav2_util::distance_from_path(global_path_, traj.poses[traj_index], + start_index_, + search_window_); + distance += search_result.distance; + start_index_ = search_result.closest_segment_index; + } + return distance / traj.poses.size(); +} + +double PathHugCritic::getScale() const +{ + if (zero_scale_) { + return 0.0; + } else { + return costmap_ros_->getCostmap()->getResolution() * 0.5; + } +} + +} // 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 67fc8cb6a3c..ef778cd2118 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..738312d07ca --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/path_hug_test.cpp @@ -0,0 +1,162 @@ +// 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 "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_forward_point_distance = 0.325; + +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 name = "PathHugCritic"; + critic_ = std::make_shared(); + critic_->initialize(node_, name, "", costmap_ros_); + critic_->onInit(); + } + + 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 = 0.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) +{ + node_->declare_parameter("PathHugCritic.forward_point_distance", default_forward_point_distance); + EXPECT_EQ( + node_->get_parameter("PathHugCritic.forward_point_distance").as_double(), + default_forward_point_distance); +} + +TEST_F(PathHugCriticTest, HandlesEmptyTrajectory) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 1.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(1, 1), 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, 1), path); + + dwb_msgs::msg::Trajectory2D traj = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}}); + EXPECT_DOUBLE_EQ(critic_->scoreTrajectory(traj), 0.0); +} + +TEST_F(PathHugCriticTest, ScoresTrajectoryNearPath) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 1.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(1, 1), path); + + dwb_msgs::msg::Trajectory2D traj = makeTrajectory({{0.0, 0.0}, {0.5, 0.5}, {1.0, 1.0}}); + double score = critic_->scoreTrajectory(traj); + EXPECT_GE(score, 0.0); +} + +TEST_F(PathHugCriticTest, ScoresTrajectoryFarFromPath) +{ + nav_msgs::msg::Path path = makePath({{0.0, 0.0}, {1.0, 1.0}}); + critic_->prepare(makePose(0, 0), nav_2d_msgs::msg::Twist2D(), makePose(1, 1), path); + + dwb_msgs::msg::Trajectory2D traj = makeTrajectory({{5.0, 5.0}, {6.0, 6.0}}); + double score = critic_->scoreTrajectory(traj); + EXPECT_GT(score, 0.0); +} + +TEST_F(PathHugCriticTest, CustomParameterValues) +{ + auto custom_node = std::make_shared("custom_test_node"); + custom_node->configure(); + custom_node->activate(); + + double custom_distance = 1.0; + std::string name = "PathHugCritic"; + + nav2::declare_parameter_if_not_declared( + custom_node, name + ".forward_point_distance", + rclcpp::ParameterValue(custom_distance)); + + auto custom_critic = std::make_shared(); + auto custom_costmap = std::make_shared("custom_costmap", "", + false); + custom_costmap->configure(); + + custom_critic->initialize(custom_node, name, "", custom_costmap); + custom_critic->onInit(); + + EXPECT_EQ( + custom_node->get_parameter(name + ".forward_point_distance").as_double(), + custom_distance); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}