Update footprint iif changed (#4193)

Signed-off-by: Brice <brice.renaudeau@gmail.com>

Author: BriceRenaudeau

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;

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

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