File tree Expand file tree Collapse file tree 2 files changed +8
-9
lines changed
nav2_dwb_controller/dwb_critics Expand file tree Collapse file tree 2 files changed +8
-9
lines changed Original file line number Diff line number Diff line change 1515#ifndef DWB_CRITICS__PATH_HUG_HPP_
1616#define DWB_CRITICS__PATH_HUG_HPP_
1717
18- #include " dwb_critics/path_dist.hpp"
18+ #include " dwb_core/trajectory_critic.hpp" // ✅ Correct include
19+ #include " nav2_util/path_utils.hpp"
1920
2021namespace dwb_critics
2122{
@@ -24,7 +25,7 @@ namespace dwb_critics
2425 * @brief DWB critic that penalizes trajectories based on their distance from the global path.
2526 * Encourages the robot to "hug" or stay close to the path.
2627 */
27- class PathHugCritic : public PathDistCritic
28+ class PathHugCritic : public dwb_core ::TrajectoryCritic // ✅ Add namespace prefix
2829{
2930public:
3031 void onInit () override ;
@@ -39,10 +40,10 @@ class PathHugCritic : public PathDistCritic
3940protected:
4041 bool zero_scale_{false };
4142 nav_msgs::msg::Path global_path_;
42- size_t start_index_;
43- geometry_msgs::msg::Pose current_pose_;
44- double average_score_;
45- double search_window_;
43+ size_t start_index_{ 0 } ;
44+ geometry_msgs::msg::Pose current_pose_{} ;
45+ double average_score_{ 0.0 } ;
46+ double search_window_{ 2.0 } ;
4647};
4748
4849} // namespace dwb_critics
Original file line number Diff line number Diff line change @@ -24,7 +24,6 @@ namespace dwb_critics
2424
2525void PathHugCritic::onInit ()
2626{
27- stop_on_failure_ = false ;
2827 auto node = node_.lock ();
2928 if (!node) {
3029 throw std::runtime_error{" Failed to lock node" };
@@ -38,7 +37,6 @@ void PathHugCritic::onInit()
3837 throw std::runtime_error{" search_window must be positive" };
3938 }
4039}
41-
4240bool PathHugCritic::prepare (
4341 const geometry_msgs::msg::Pose & /* pose*/ , const nav_2d_msgs::msg::Twist2D & /* vel*/ ,
4442 const geometry_msgs::msg::Pose & /* goal*/ ,
@@ -75,7 +73,7 @@ double PathHugCritic::getScale() const
7573 if (zero_scale_) {
7674 return 0.0 ;
7775 } else {
78- return costmap_ ->getResolution () * 0.5 ;
76+ return costmap_ros_-> getCostmap () ->getResolution () * 0.5 ;
7977 }
8078}
8179
You can’t perform that action at this time.
0 commit comments