Skip to content

Commit 8f77cec

Browse files
committed
Taking back later additions creating problem
Signed-off-by: silanus23 <[email protected]>
1 parent df12f59 commit 8f77cec

File tree

2 files changed

+8
-9
lines changed

2 files changed

+8
-9
lines changed

nav2_dwb_controller/dwb_critics/include/dwb_critics/path_hug.hpp

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,8 @@
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

2021
namespace 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
{
2930
public:
3031
void onInit() override;
@@ -39,10 +40,10 @@ class PathHugCritic : public PathDistCritic
3940
protected:
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

nav2_dwb_controller/dwb_critics/src/path_hug.cpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,6 @@ namespace dwb_critics
2424

2525
void 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-
4240
bool 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

0 commit comments

Comments
 (0)