Add new member to MapLocation

ros-navigation · Feb 4, 2025 · de7f1b4 · de7f1b4
1 parent 04d3c38
commit de7f1b4
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 20 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
@@ -59,6 +59,7 @@ struct MapLocation
 {
   unsigned int x;
   unsigned int y;
+  unsigned char cost;
 };
 
 /**
@@ -320,23 +321,23 @@ class Costmap2D
    */
   bool getMapRegionOccupiedByPolygon(
     const std::vector<geometry_msgs::msg::Point> & polygon,
-    std::vector<std::pair<MapLocation, unsigned char>> & polygon_map_region);
+    std::vector<MapLocation> & polygon_map_region);
 
   /**
    * @brief  Sets the given map region to desired value
    * @param polygon_map_region The map region to perform the operation on
    * @param new_cost_value The value to set costs to
    */
   void setMapRegionOccupiedByPolygon(
-    const std::vector<std::pair<MapLocation, unsigned char>> & polygon_map_region,
+    const std::vector<MapLocation> & polygon_map_region,
     unsigned char new_cost_value);
 
   /**
    * @brief  Restores the corresponding map region using given map region
    * @param polygon_map_region The map region to perform the operation on
    */
   void restoreMapRegionOccupiedByPolygon(
-    const std::vector<std::pair<MapLocation, unsigned char>> & polygon_map_region);
+    const std::vector<MapLocation> & polygon_map_region);
 
   /**
    * @brief  Get the map cells that make up the outline of a polygon
@@ -595,6 +596,7 @@ class Costmap2D
     {
       MapLocation loc;
       costmap_.indexToCells(offset, loc.x, loc.y);
+      loc.cost = costmap_.getCost(loc.x, loc.y);
       cells_.push_back(loc);
     }
 

diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -413,9 +413,9 @@ StaticLayer::updateCosts(
     return;
   }
 
-  std::vector<std::pair<MapLocation, unsigned char>> map_region_to_restore;
-  map_region_to_restore.reserve(100);
+  std::vector<MapLocation> map_region_to_restore;
   if (footprint_clearing_enabled_) {
+    map_region_to_restore.reserve(100);
     getMapRegionOccupiedByPolygon(transformed_footprint_, map_region_to_restore);
     setMapRegionOccupiedByPolygon(map_region_to_restore, nav2_costmap_2d::FREE_SPACE);
   }

diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -400,7 +400,8 @@ bool Costmap2D::setConvexPolygonCost(
   const std::vector<geometry_msgs::msg::Point> & polygon,
   unsigned char cost_value)
 {
-  std::vector<std::pair<MapLocation, unsigned char>> polygon_map_region;
+  std::vector<MapLocation> polygon_map_region;
+  polygon_map_region.reserve(100);
   if (!getMapRegionOccupiedByPolygon(polygon, polygon_map_region)) {
     return false;
   }
@@ -411,25 +412,25 @@ bool Costmap2D::setConvexPolygonCost(
 }
 
 void Costmap2D::setMapRegionOccupiedByPolygon(
-  const std::vector<std::pair<MapLocation, unsigned char>> & polygon_map_region,
+  const std::vector<MapLocation> & polygon_map_region,
   unsigned char new_cost_value)
 {
-  for (const auto & [cell, _] : polygon_map_region) {
+  for (const auto & cell : polygon_map_region) {
     setCost(cell.x, cell.y, new_cost_value);
   }
 }
 
 void Costmap2D::restoreMapRegionOccupiedByPolygon(
-  const std::vector<std::pair<MapLocation, unsigned char>> & polygon_map_region)
+  const std::vector<MapLocation> & polygon_map_region)
 {
-  for (const auto & [cell, cost_value] : polygon_map_region) {
-    setCost(cell.x, cell.y, cost_value);
+  for (const auto & cell : polygon_map_region) {
+    setCost(cell.x, cell.y, cell.cost);
   }
 }
 
 bool Costmap2D::getMapRegionOccupiedByPolygon(
   const std::vector<geometry_msgs::msg::Point> & polygon,
-  std::vector<std::pair<MapLocation, unsigned char>> & polygon_map_region)
+  std::vector<MapLocation> & polygon_map_region)
 {
   // we assume the polygon is given in the global_frame...
   // we need to transform it to map coordinates
@@ -444,14 +445,7 @@ bool Costmap2D::getMapRegionOccupiedByPolygon(
   }
 
   // get the cells that fill the polygon
-  std::vector<MapLocation> polygon_cells;
-  polygon_cells.reserve(100);
-  convexFillCells(map_polygon, polygon_cells);
-
-  // store the cost informations of the cells
-  for (const auto & cell : polygon_cells) {
-    polygon_map_region.push_back({cell, getCost(cell.x, cell.y)});
-  }
+  convexFillCells(map_polygon, polygon_map_region);
 
   return true;
 }
@@ -537,6 +531,7 @@ void Costmap2D::convexFillCells(
     for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) {
       pt.x = x;
       pt.y = y;
+      pt.cost = getCost(x, y);
       polygon_cells.push_back(pt);
     }
   }