diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 385f484615d..5afb919662f 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -54,6 +54,16 @@ void GridCollisionChecker::setFootprint( const double & possible_collision_cost) { possible_collision_cost_ = static_cast(possible_collision_cost); + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + footprint_is_radius_ = radius; // Use radius, no caching required @@ -114,18 +124,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5f), static_cast(y + 0.5f))); - if (footprint_cost_ < possible_collision_cost_) { - if (possible_collision_cost_ > 0.0f) { - return false; - } else { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 1000, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } + if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + return false; } // If its inscribed, in collision, or unknown in the middle,