Update footprint iif changed (ros-navigation#4193)

Signed-off-by: Brice <brice.renaudeau@gmail.com>
Manos-G · Aug 1, 2024 · 0aea0dd · 0aea0dd
1 parent 11d4a7b
commit 0aea0dd
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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