Updating error logging in Smac collision detector object (ros-navigat…

…ion#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski <[email protected]> * linting Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Jakubach <[email protected]>
Jakubach · Nov 22, 2024 · a67450e · a67450e
1 parent 6b2e244
commit a67450e
Showing 1 changed file with 12 additions and 12 deletions.
diff --git 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<float>(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<float>(costmap_->getCost(
         static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(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,