From dcb060bdcac385108aea217d4ba614a72f974795 Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Mon, 18 Mar 2024 13:23:19 +0100
Subject: [PATCH] Update footprint iif changed

---
 .../include/nav2_smac_planner/analytic_expansion.hpp          | 2 +-
 nav2_smac_planner/src/analytic_expansion.cpp                  | 2 +-
 nav2_smac_planner/src/collision_checker.cpp                   | 1 +
 nav2_smac_planner/src/smac_planner_hybrid.cpp                 | 4 ++++
 nav2_smac_planner/src/smac_planner_lattice.cpp                | 4 ++++
 5 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
index 10c4224a077..9dc317dd8a4 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -73,7 +73,7 @@ class AnalyticExpansion
    * @brief Sets the collision checker and costmap to use in expansion validation
    * @param collision_checker Collision checker to use
    */
-  void setCollisionChecker(GridCollisionChecker * & collision_checker);
+  void setCollisionChecker(GridCollisionChecker * collision_checker);
 
   /**
    * @brief Attempt an analytic path completion
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index 8dcc1314357..e14033d0f1c 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -41,7 +41,7 @@ AnalyticExpansion<NodeT>::AnalyticExpansion(
 
 template<typename NodeT>
 void AnalyticExpansion<NodeT>::setCollisionChecker(
-  GridCollisionChecker * & collision_checker)
+  GridCollisionChecker * collision_checker)
 {
   _collision_checker = collision_checker;
 }
diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
index 2f23442097a..c9ce030fab5 100644
--- a/nav2_smac_planner/src/collision_checker.cpp
+++ b/nav2_smac_planner/src/collision_checker.cpp
@@ -66,6 +66,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
index b0bdf042dc2..592ec40f1e9 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -338,6 +338,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
index cd98cf64408..87f2e365639 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -274,6 +274,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