Taking back later additions creating problem

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

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

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;
   }
 }
 

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

nav2_mppi_controller/critics.xml

@@ -45,9 +45,5 @@
       <description>mppi critic for restricting command velocities in deadband range</description>
     </class>
 
-    <class type="mppi::critics::PathHugCritic" base_class_type="mppi::critics::CriticFunction">
-      <description>mppi critic for restricting command velocities in deadband range</description>
-    </class>
-
   </library>
 </class_libraries>