Tracking critics dwb and mppi

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
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"

nav2_dwb_controller/dwb_critics/default_critics.xml

@@ -38,5 +38,9 @@
       <description>Penalize trajectories with rotational velocities
       </description>
     </class>
+    <class type="dwb_critics::PathHugCritic" base_class_type="dwb_core::TrajectoryCritic">
+      <description>Penalize trajectories according to their distance to the path and their alignment with it.
+      </description>
+    </class>
   </library>
 </class_libraries>

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_

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 <vector>
+#include <string>
+#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)

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
+)

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 <gtest/gtest.h>
+#include <memory>
+#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<nav2::LifecycleNode>("test_node");
+    node_->configure();
+    node_->activate();
+
+    costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap", "",
+      false);
+    costmap_ros_->configure();
+
+    std::string name = "PathHugCritic";
+    critic_ = std::make_shared<dwb_critics::PathHugCritic>();
+    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<std::pair<double, double>> 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<std::pair<double, double>> 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<nav2::LifecycleNode> node_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::shared_ptr<dwb_critics::PathHugCritic> 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<nav2::LifecycleNode>("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<dwb_critics::PathHugCritic>();
+  auto custom_costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("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;
+}