From 6698719d447007adacac8a9eae4f707ca90ed52c Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Wed, 20 Mar 2024 22:14:15 +0100 Subject: [PATCH] Update footprint iif changed (#4193) Signed-off-by: Brice --- 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 ++++ 3 files changed, 9 insertions(+) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 20d288809d..549570fb26 100644 --- 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 index 80577fcb55..42dbd4efe0 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -304,6 +304,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 8b46fc3ae7..75c54517a6 100644 --- 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 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