From 2813a13f48b32251a963bc7b4631605c8c8b208f Mon Sep 17 00:00:00 2001 From: corot Date: Fri, 27 Sep 2024 21:43:12 +0200 Subject: [PATCH] Check the cost of the center point of the robot and return if it's non-traversable space --- base_local_planner/src/costmap_model.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/base_local_planner/src/costmap_model.cpp b/base_local_planner/src/costmap_model.cpp index 2f7896f6af..e3a9648da3 100644 --- a/base_local_planner/src/costmap_model.cpp +++ b/base_local_planner/src/costmap_model.cpp @@ -49,7 +49,7 @@ namespace base_local_planner { // returns: // -1 if footprint covers at least a lethal obstacle cell, or // -2 if footprint covers at least a no-information cell, or - // -3 if footprint is [partially] outside of the map, or + // -3 if footprint is [partially] outside the map, or // a positive value for traversable space //used to put things into grid coordinates @@ -59,13 +59,15 @@ namespace base_local_planner { if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y)) return -3.0; + //check its cost, so we can skip footprint check if it's already non-traversable space + unsigned char cost = costmap_.getCost(cell_x, cell_y); + if(cost == NO_INFORMATION) + return -2.0; + if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE) + return -1.0; + //if number of points in the footprint is less than 3, we'll just assume a circular robot if(footprint.size() < 3){ - unsigned char cost = costmap_.getCost(cell_x, cell_y); - if(cost == NO_INFORMATION) - return -2.0; - if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE) - return -1.0; return cost; }