From a4bf04b9cf429a9d441eb4adea9f655a20944404 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 17:39:05 +0300 Subject: [PATCH 1/3] DWB critics addition Signed-off-by: silanus23 --- .../dwb_critics/CMakeLists.txt | 1 + .../dwb_critics/default_critics.xml | 4 + .../include/dwb_critics/path_hug.hpp | 50 ++++++ .../dwb_critics/src/path_hug.cpp | 84 +++++++++ .../dwb_critics/test/CMakeLists.txt | 7 + .../dwb_critics/test/path_hug_test.cpp | 162 ++++++++++++++++++ 6 files changed, 308 insertions(+) create mode 100644 nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp create mode 100644 nav2_dwb_controller/dwb_critics/src/path_hug.cpp create mode 100644 nav2_dwb_controller/dwb_critics/test/path_hug_test.cpp 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..e0a6fc87633 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp @@ -0,0 +1,50 @@ +// 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_critics/path_dist.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 PathDistCritic +{ +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_; + geometry_msgs::msg::Pose current_pose_; + double average_score_; + 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..bcf59c4ce69 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/src/path_hug.cpp @@ -0,0 +1,84 @@ +// 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() +{ + stop_on_failure_ = false; + 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_->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; +} From df12f59516deac407748f64cce84ee2392cdb58a Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 17:47:22 +0300 Subject: [PATCH 2/3] mppi critic addition Signed-off-by: silanus23 --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/critics.xml | 4 + .../critics/path_hug_critic.hpp | 81 +++++++ .../src/critics/path_hug_critic.cpp | 208 ++++++++++++++++++ 4 files changed, 294 insertions(+) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/path_hug_critic.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index c28c711dc50..a67c255361f 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -84,6 +84,7 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/velocity_deadband_critic.cpp + src/critics/path_hug_critic.cpp ) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE) # Apple Clang: use C++20 and optimization, omit -fconcepts diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index deae2c11cb9..33f01632d98 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,5 +45,9 @@ mppi critic for restricting command velocities in deadband range + + mppi critic for restricting command velocities in deadband range + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp new file mode 100644 index 00000000000..9eba61bb123 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.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 not use 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_MPPI_CONTROLLER__CRITICS__PATH_HUG_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_HUG_CRITIC_HPP_ + +#include +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/models/path.hpp" +#include "nav2_mppi_controller/models/trajectories.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::PathHugCritic + * @brief Critic plugin for penalizing trajectories that deviate from the global path. + * + * This critic calculates a cost for each trajectory based on its distance from the + * reference path. The cost is an accumulation of the minimum distances from sampled points + * along the trajectory to the reference path. This encourages the controller to generate + * trajectories that "hug" or closely follow the provided global plan. + */ +class PathHugCritic : public CriticFunction +{ +public: + /** + * @brief Initialize the critic, loading parameters. + */ + void initialize() override; + + /** + * @brief Evaluate the critic score for all trajectories. + * @param data The critic data object containing information about the trajectories, path, and costs. + */ + void score(CriticData & data) override; + +private: + void updatePathCache(const models::Path & path); + + void computeDistancesToPathVectorized( + const Eigen::ArrayXf & points_x, + const Eigen::ArrayXf & points_y, + Eigen::ArrayXf & distances); + + // Parameters + unsigned int power_; + float weight_; + double search_window_; + int sample_stride_; + + // Path cache for efficient computation + size_t path_size_cache_{0}; + Eigen::ArrayXf path_x_cache_; + Eigen::ArrayXf path_y_cache_; + Eigen::ArrayXf segment_lengths_; + Eigen::ArrayXf cumulative_distances_; + Eigen::ArrayXf segment_dx_; + Eigen::ArrayXf segment_dy_; + Eigen::ArrayXf segment_len_sq_; + + // Per-evaluation cache + Eigen::ArrayXf cost_array_; + Eigen::Array closest_indices_; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_HUG_CRITIC_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_hug_critic.cpp b/nav2_mppi_controller/src/critics/path_hug_critic.cpp new file mode 100644 index 00000000000..9923cc11255 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_hug_critic.cpp @@ -0,0 +1,208 @@ +// 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_mppi_controller/critics/path_hug_critic.hpp" + +#include +#include +#include + +#include "nav2_util/geometry_utils.hpp" + +namespace mppi::critics +{ + +void PathHugCritic::initialize() +{ + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0f); + getParam(search_window_, "search_window", 2.0); + getParam(sample_stride_, "sample_stride", 3); + + RCLCPP_INFO( + logger_, + "PathHugCritic instantiated with power=%u, weight=%.2f, search_window=%.2f, stride=%d", + power_, weight_, search_window_, sample_stride_); +} + +void PathHugCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + if (data.path.x.size() < 2) { + return; + } + + if (static_cast(data.path.x.size()) != + static_cast(path_size_cache_) || + (data.path.x.size() > 0 && path_x_cache_.size() == data.path.x.size() && + (data.path.x.array() != path_x_cache_.array()).any())) + { + updatePathCache(data.path); + } + + const auto & traj_x = data.trajectories.x; + const auto & traj_y = data.trajectories.y; + const Eigen::Index batch_size = traj_x.rows(); + const Eigen::Index traj_length = traj_x.cols(); + + // Pre-allocate arrays + if (cost_array_.size() != batch_size) { + cost_array_.resize(batch_size); + closest_indices_.resize(batch_size); + closest_indices_.setZero(); + } + cost_array_.setZero(); + + // Sample trajectory points with stride + const int effective_stride = std::max(1, std::min(sample_stride_, static_cast(traj_length))); + const int num_samples = (traj_length + effective_stride - 1) / effective_stride; + + if (num_samples == 0) {return;} + + for (int sample_idx = 0; sample_idx < num_samples; ++sample_idx) { + const Eigen::Index traj_col = sample_idx * effective_stride; + if (traj_col >= traj_length) {break;} + + // Get all trajectory points at that step + const auto & points_x = traj_x.col(traj_col); + const auto & points_y = traj_y.col(traj_col); + + computeDistancesToPathVectorized(points_x, points_y, cost_array_); + } + + // Normalize by the number of samples + cost_array_ /= static_cast(num_samples); + + if (power_ > 1u) { + data.costs += (cost_array_ * weight_).pow(power_); + } else { + data.costs += cost_array_ * weight_; + } +} + +void PathHugCritic::updatePathCache(const models::Path & path) +{ + path_size_cache_ = path.x.size(); + path_x_cache_ = path.x; + path_y_cache_ = path.y; + + const Eigen::Index path_size = path.x.size(); + if (path_size < 2) {return;} + + if (segment_lengths_.size() != path_size - 1) { + segment_lengths_.resize(path_size - 1); + cumulative_distances_.resize(path_size); + segment_dx_.resize(path_size - 1); + segment_dy_.resize(path_size - 1); + segment_len_sq_.resize(path_size - 1); + } + + cumulative_distances_(0) = 0.0; + for (Eigen::Index i = 0; i < path_size - 1; ++i) { + segment_dx_(i) = path.x(i + 1) - path.x(i); + segment_dy_(i) = path.y(i + 1) - path.y(i); + segment_len_sq_(i) = segment_dx_(i) * segment_dx_(i) + segment_dy_(i) * segment_dy_(i); + segment_lengths_(i) = std::sqrt(segment_len_sq_(i)); + cumulative_distances_(i + 1) = cumulative_distances_(i) + segment_lengths_(i); + } +} + +void PathHugCritic::computeDistancesToPathVectorized( + const Eigen::ArrayXf & points_x, + const Eigen::ArrayXf & points_y, + Eigen::ArrayXf & distances) +{ + const Eigen::Index batch_size = points_x.size(); + const Eigen::Index path_size = path_x_cache_.size(); + + for (Eigen::Index traj_idx = 0; traj_idx < batch_size; ++traj_idx) { + const float px = points_x(traj_idx); + const float py = points_y(traj_idx); + + float min_dist_sq = std::numeric_limits::max(); + + Eigen::Index start_idx = (traj_idx < closest_indices_.size()) ? + std::max(0L, closest_indices_(traj_idx) - 2) : 0; + + double distance_traversed = 0.0; + Eigen::Index closest_seg_idx = start_idx; + + for (Eigen::Index seg_idx = start_idx; seg_idx < path_size - 1; ++seg_idx) { + if (distance_traversed > search_window_) { + break; + } + + if (segment_len_sq_(seg_idx) < 1e-6) { + continue; + } + + // Standard point-to-line-segment distance calculation + const float dx_to_start = px - path_x_cache_(seg_idx); + const float dy_to_start = py - path_y_cache_(seg_idx); + const float dot = dx_to_start * segment_dx_(seg_idx) + dy_to_start * segment_dy_(seg_idx); + const float t = std::clamp(dot / segment_len_sq_(seg_idx), 0.0f, 1.0f); + const float proj_x = path_x_cache_(seg_idx) + t * segment_dx_(seg_idx); + const float proj_y = path_y_cache_(seg_idx) + t * segment_dy_(seg_idx); + const float dist_sq = (px - proj_x) * (px - proj_x) + (py - proj_y) * (py - proj_y); + + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + closest_seg_idx = seg_idx; + } + + distance_traversed += segment_lengths_(seg_idx); + } + + float final_min_dist = std::sqrt(min_dist_sq); + + // Fallback if the search window found nothing + if (min_dist_sq == std::numeric_limits::max()) { + // If search window fails, do a full search using the same accurate logic + for (Eigen::Index seg_idx = 0; seg_idx < path_size - 1; ++seg_idx) { + if (segment_len_sq_(seg_idx) < 1e-6) {continue;} + + const float dx_to_start = px - path_x_cache_(seg_idx); + const float dy_to_start = py - path_y_cache_(seg_idx); + const float dot = dx_to_start * segment_dx_(seg_idx) + dy_to_start * segment_dy_(seg_idx); + const float t = std::clamp(dot / segment_len_sq_(seg_idx), 0.0f, 1.0f); + const float proj_x = path_x_cache_(seg_idx) + t * segment_dx_(seg_idx); + const float proj_y = path_y_cache_(seg_idx) + t * segment_dy_(seg_idx); + const float dist_sq = (px - proj_x) * (px - proj_x) + (py - proj_y) * (py - proj_y); + + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + closest_seg_idx = seg_idx; + } + } + // Recalculate the final distance from the true minimum squared distance + final_min_dist = std::sqrt(min_dist_sq); + } + + distances(traj_idx) += final_min_dist; + if (traj_idx < closest_indices_.size()) { + closest_indices_(traj_idx) = closest_seg_idx; + } + } +} +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathHugCritic, + mppi::critics::CriticFunction) From dd0faed8cde028240a0185e711082487b179c016 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 14 Oct 2025 01:47:31 +0300 Subject: [PATCH 3/3] Taking back later additions creating problem Signed-off-by: silanus23 --- .../include/dwb_critics/path_hug.hpp | 13 +- .../dwb_critics/src/path_hug.cpp | 4 +- nav2_mppi_controller/CMakeLists.txt | 1 - nav2_mppi_controller/critics.xml | 4 - .../critics/path_hug_critic.hpp | 81 ------- .../src/critics/path_hug_critic.cpp | 208 ------------------ 6 files changed, 8 insertions(+), 303 deletions(-) delete mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp delete mode 100644 nav2_mppi_controller/src/critics/path_hug_critic.cpp 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 index e0a6fc87633..3968be5eb77 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp @@ -15,7 +15,8 @@ #ifndef DWB_CRITICS__PATH_HUG_HPP_ #define DWB_CRITICS__PATH_HUG_HPP_ -#include "dwb_critics/path_dist.hpp" +#include "dwb_core/trajectory_critic.hpp" +#include "nav2_util/path_utils.hpp" namespace dwb_critics { @@ -24,7 +25,7 @@ namespace dwb_critics * @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 PathDistCritic +class PathHugCritic : public dwb_core::TrajectoryCritic { public: void onInit() override; @@ -39,10 +40,10 @@ class PathHugCritic : public PathDistCritic protected: bool zero_scale_{false}; nav_msgs::msg::Path global_path_; - size_t start_index_; - geometry_msgs::msg::Pose current_pose_; - double average_score_; - double search_window_; + size_t start_index_{0}; + geometry_msgs::msg::Pose current_pose_{}; + double average_score_{0.0}; + double search_window_{2.0}; }; } // namespace dwb_critics diff --git a/nav2_dwb_controller/dwb_critics/src/path_hug.cpp b/nav2_dwb_controller/dwb_critics/src/path_hug.cpp index bcf59c4ce69..ec1062c2f59 100644 --- a/nav2_dwb_controller/dwb_critics/src/path_hug.cpp +++ b/nav2_dwb_controller/dwb_critics/src/path_hug.cpp @@ -24,7 +24,6 @@ namespace dwb_critics void PathHugCritic::onInit() { - stop_on_failure_ = false; auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; @@ -38,7 +37,6 @@ void PathHugCritic::onInit() 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*/, @@ -75,7 +73,7 @@ double PathHugCritic::getScale() const if (zero_scale_) { return 0.0; } else { - return costmap_->getResolution() * 0.5; + return costmap_ros_->getCostmap()->getResolution() * 0.5; } } diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index a67c255361f..c28c711dc50 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -84,7 +84,6 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/velocity_deadband_critic.cpp - src/critics/path_hug_critic.cpp ) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE) # Apple Clang: use C++20 and optimization, omit -fconcepts diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 33f01632d98..deae2c11cb9 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,9 +45,5 @@ mppi critic for restricting command velocities in deadband range - - mppi critic for restricting command velocities in deadband range - - diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp deleted file mode 100644 index 9eba61bb123..00000000000 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_hug_critic.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// 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 not use 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_MPPI_CONTROLLER__CRITICS__PATH_HUG_CRITIC_HPP_ -#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_HUG_CRITIC_HPP_ - -#include -#include "nav2_mppi_controller/critic_function.hpp" -#include "nav2_mppi_controller/models/state.hpp" -#include "nav2_mppi_controller/models/path.hpp" -#include "nav2_mppi_controller/models/trajectories.hpp" - -namespace mppi::critics -{ - -/** - * @class mppi::critics::PathHugCritic - * @brief Critic plugin for penalizing trajectories that deviate from the global path. - * - * This critic calculates a cost for each trajectory based on its distance from the - * reference path. The cost is an accumulation of the minimum distances from sampled points - * along the trajectory to the reference path. This encourages the controller to generate - * trajectories that "hug" or closely follow the provided global plan. - */ -class PathHugCritic : public CriticFunction -{ -public: - /** - * @brief Initialize the critic, loading parameters. - */ - void initialize() override; - - /** - * @brief Evaluate the critic score for all trajectories. - * @param data The critic data object containing information about the trajectories, path, and costs. - */ - void score(CriticData & data) override; - -private: - void updatePathCache(const models::Path & path); - - void computeDistancesToPathVectorized( - const Eigen::ArrayXf & points_x, - const Eigen::ArrayXf & points_y, - Eigen::ArrayXf & distances); - - // Parameters - unsigned int power_; - float weight_; - double search_window_; - int sample_stride_; - - // Path cache for efficient computation - size_t path_size_cache_{0}; - Eigen::ArrayXf path_x_cache_; - Eigen::ArrayXf path_y_cache_; - Eigen::ArrayXf segment_lengths_; - Eigen::ArrayXf cumulative_distances_; - Eigen::ArrayXf segment_dx_; - Eigen::ArrayXf segment_dy_; - Eigen::ArrayXf segment_len_sq_; - - // Per-evaluation cache - Eigen::ArrayXf cost_array_; - Eigen::Array closest_indices_; -}; - -} // namespace mppi::critics - -#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_HUG_CRITIC_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_hug_critic.cpp b/nav2_mppi_controller/src/critics/path_hug_critic.cpp deleted file mode 100644 index 9923cc11255..00000000000 --- a/nav2_mppi_controller/src/critics/path_hug_critic.cpp +++ /dev/null @@ -1,208 +0,0 @@ -// 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_mppi_controller/critics/path_hug_critic.hpp" - -#include -#include -#include - -#include "nav2_util/geometry_utils.hpp" - -namespace mppi::critics -{ - -void PathHugCritic::initialize() -{ - auto getParentParam = parameters_handler_->getParamGetter(parent_name_); - auto getParam = parameters_handler_->getParamGetter(name_); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 10.0f); - getParam(search_window_, "search_window", 2.0); - getParam(sample_stride_, "sample_stride", 3); - - RCLCPP_INFO( - logger_, - "PathHugCritic instantiated with power=%u, weight=%.2f, search_window=%.2f, stride=%d", - power_, weight_, search_window_, sample_stride_); -} - -void PathHugCritic::score(CriticData & data) -{ - if (!enabled_) { - return; - } - if (data.path.x.size() < 2) { - return; - } - - if (static_cast(data.path.x.size()) != - static_cast(path_size_cache_) || - (data.path.x.size() > 0 && path_x_cache_.size() == data.path.x.size() && - (data.path.x.array() != path_x_cache_.array()).any())) - { - updatePathCache(data.path); - } - - const auto & traj_x = data.trajectories.x; - const auto & traj_y = data.trajectories.y; - const Eigen::Index batch_size = traj_x.rows(); - const Eigen::Index traj_length = traj_x.cols(); - - // Pre-allocate arrays - if (cost_array_.size() != batch_size) { - cost_array_.resize(batch_size); - closest_indices_.resize(batch_size); - closest_indices_.setZero(); - } - cost_array_.setZero(); - - // Sample trajectory points with stride - const int effective_stride = std::max(1, std::min(sample_stride_, static_cast(traj_length))); - const int num_samples = (traj_length + effective_stride - 1) / effective_stride; - - if (num_samples == 0) {return;} - - for (int sample_idx = 0; sample_idx < num_samples; ++sample_idx) { - const Eigen::Index traj_col = sample_idx * effective_stride; - if (traj_col >= traj_length) {break;} - - // Get all trajectory points at that step - const auto & points_x = traj_x.col(traj_col); - const auto & points_y = traj_y.col(traj_col); - - computeDistancesToPathVectorized(points_x, points_y, cost_array_); - } - - // Normalize by the number of samples - cost_array_ /= static_cast(num_samples); - - if (power_ > 1u) { - data.costs += (cost_array_ * weight_).pow(power_); - } else { - data.costs += cost_array_ * weight_; - } -} - -void PathHugCritic::updatePathCache(const models::Path & path) -{ - path_size_cache_ = path.x.size(); - path_x_cache_ = path.x; - path_y_cache_ = path.y; - - const Eigen::Index path_size = path.x.size(); - if (path_size < 2) {return;} - - if (segment_lengths_.size() != path_size - 1) { - segment_lengths_.resize(path_size - 1); - cumulative_distances_.resize(path_size); - segment_dx_.resize(path_size - 1); - segment_dy_.resize(path_size - 1); - segment_len_sq_.resize(path_size - 1); - } - - cumulative_distances_(0) = 0.0; - for (Eigen::Index i = 0; i < path_size - 1; ++i) { - segment_dx_(i) = path.x(i + 1) - path.x(i); - segment_dy_(i) = path.y(i + 1) - path.y(i); - segment_len_sq_(i) = segment_dx_(i) * segment_dx_(i) + segment_dy_(i) * segment_dy_(i); - segment_lengths_(i) = std::sqrt(segment_len_sq_(i)); - cumulative_distances_(i + 1) = cumulative_distances_(i) + segment_lengths_(i); - } -} - -void PathHugCritic::computeDistancesToPathVectorized( - const Eigen::ArrayXf & points_x, - const Eigen::ArrayXf & points_y, - Eigen::ArrayXf & distances) -{ - const Eigen::Index batch_size = points_x.size(); - const Eigen::Index path_size = path_x_cache_.size(); - - for (Eigen::Index traj_idx = 0; traj_idx < batch_size; ++traj_idx) { - const float px = points_x(traj_idx); - const float py = points_y(traj_idx); - - float min_dist_sq = std::numeric_limits::max(); - - Eigen::Index start_idx = (traj_idx < closest_indices_.size()) ? - std::max(0L, closest_indices_(traj_idx) - 2) : 0; - - double distance_traversed = 0.0; - Eigen::Index closest_seg_idx = start_idx; - - for (Eigen::Index seg_idx = start_idx; seg_idx < path_size - 1; ++seg_idx) { - if (distance_traversed > search_window_) { - break; - } - - if (segment_len_sq_(seg_idx) < 1e-6) { - continue; - } - - // Standard point-to-line-segment distance calculation - const float dx_to_start = px - path_x_cache_(seg_idx); - const float dy_to_start = py - path_y_cache_(seg_idx); - const float dot = dx_to_start * segment_dx_(seg_idx) + dy_to_start * segment_dy_(seg_idx); - const float t = std::clamp(dot / segment_len_sq_(seg_idx), 0.0f, 1.0f); - const float proj_x = path_x_cache_(seg_idx) + t * segment_dx_(seg_idx); - const float proj_y = path_y_cache_(seg_idx) + t * segment_dy_(seg_idx); - const float dist_sq = (px - proj_x) * (px - proj_x) + (py - proj_y) * (py - proj_y); - - if (dist_sq < min_dist_sq) { - min_dist_sq = dist_sq; - closest_seg_idx = seg_idx; - } - - distance_traversed += segment_lengths_(seg_idx); - } - - float final_min_dist = std::sqrt(min_dist_sq); - - // Fallback if the search window found nothing - if (min_dist_sq == std::numeric_limits::max()) { - // If search window fails, do a full search using the same accurate logic - for (Eigen::Index seg_idx = 0; seg_idx < path_size - 1; ++seg_idx) { - if (segment_len_sq_(seg_idx) < 1e-6) {continue;} - - const float dx_to_start = px - path_x_cache_(seg_idx); - const float dy_to_start = py - path_y_cache_(seg_idx); - const float dot = dx_to_start * segment_dx_(seg_idx) + dy_to_start * segment_dy_(seg_idx); - const float t = std::clamp(dot / segment_len_sq_(seg_idx), 0.0f, 1.0f); - const float proj_x = path_x_cache_(seg_idx) + t * segment_dx_(seg_idx); - const float proj_y = path_y_cache_(seg_idx) + t * segment_dy_(seg_idx); - const float dist_sq = (px - proj_x) * (px - proj_x) + (py - proj_y) * (py - proj_y); - - if (dist_sq < min_dist_sq) { - min_dist_sq = dist_sq; - closest_seg_idx = seg_idx; - } - } - // Recalculate the final distance from the true minimum squared distance - final_min_dist = std::sqrt(min_dist_sq); - } - - distances(traj_idx) += final_min_dist; - if (traj_idx < closest_indices_.size()) { - closest_indices_(traj_idx) = closest_seg_idx; - } - } -} -} // namespace mppi::critics - -#include - -PLUGINLIB_EXPORT_CLASS( - mppi::critics::PathHugCritic, - mppi::critics::CriticFunction)