Update footprint iif changed (#4193)

Signed-off-by: Brice <brice.renaudeau@gmail.com>
ros-navigation · Apr 4, 2024 · c18a19d · c18a19d
1 parent 14d73de
commit c18a19d
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 0 deletions.
diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
@@ -62,6 +62,7 @@ void GridCollisionChecker::setFootprint(
  return;
  }
 
+ oriented_footprints_.clear();
  oriented_footprints_.reserve(angles_.size());
  double sin_th, cos_th;
  geometry_msgs::msg::Point new_pt;

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -283,6 +283,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
  }
 
  // Set collision checker and costmap information
+ _collision_checker.setFootprint(
+ _costmap_ros->getRobotFootprint(),
+ _costmap_ros->getUseRadius(),
+ findCircumscribedCost(_costmap_ros));
  _a_star->setCollisionChecker(&_collision_checker);
 
  // Set starting point, in A* bin search coordinates

diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -243,6 +243,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
 
  // Set collision checker and costmap information
+ _collision_checker.setFootprint(
+ _costmap_ros->getRobotFootprint(),
+ _costmap_ros->getUseRadius(),
+ findCircumscribedCost(_costmap_ros));
  _a_star->setCollisionChecker(&_collision_checker);
 
  // Set starting point, in A* bin search coordinates