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