Skip to content
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
apply for smac 2D
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
  • Loading branch information
SteveMacenski committed Jun 24, 2025
commit f3e736256028af6c831a7da464ec888376e832f6
12 changes: 5 additions & 7 deletions nav2_smac_planner/src/smac_planner_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -140,12 +140,10 @@ void SmacPlanner2D::configure(
_smoother->initialize(1e-50 /*No valid minimum turning radius for 2D*/);

// Initialize costmap downsampler
if (_downsample_costmap && _downsampling_factor > 1) {
std::string topic_name = "downsampled_costmap";
_costmap_downsampler = std::make_unique<CostmapDownsampler>();
_costmap_downsampler->on_configure(
node, _global_frame, topic_name, _costmap, _downsampling_factor);
}
std::string topic_name = "downsampled_costmap";
_costmap_downsampler = std::make_unique<CostmapDownsampler>();
_costmap_downsampler->on_configure(
node, _global_frame, topic_name, _costmap, _downsampling_factor);

_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan");

Expand Down Expand Up @@ -215,7 +213,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(

// Downsample costmap, if required
nav2_costmap_2d::Costmap2D * costmap = _costmap;
if (_costmap_downsampler) {
if (_downsample_costmap && _downsampling_factor > 1) {
costmap = _costmap_downsampler->downsample(_downsampling_factor);
_collision_checker.setCostmap(costmap);
}
Expand Down
Loading