Skip to content

Commit 088c423

Browse files
CihatAltiparmakCihatAltiparmak
authored
Fixed the problem of static layer not restoring old map values for footprint (#4824)
* Fix Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fixed some bugs and removed the unnecessary variables from class Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Keep the branch updated with main Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Called off API changes in costmap, added "restore_outdated_footprint" Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Added the documentation for the new methods in static layer Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Make CI happy Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Apply suggestions and minimize diffs Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fix ament_cpp_lint Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Revert changes in the method updateFootprint Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Used cached data to restore map and broken up setConvexPolygonCost into two methods Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Rename newly added methods Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Added restore_outdated_map parameter Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Make CI happy Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Changed the name of the newly added parameter - changed the name of the new param - fixed bug in the setConvexPolygonCost - Used reserve method of the vector instances - Added checks in dynamic parameter update. Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Remove unnecessary log Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Add new member to MapLocation Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Remove unnecessary header Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Set the parameters footprint_clearing_enabled and restore_cleared_footprint enable in nav2_system_tests Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
1 parent f3ef5d4 commit 088c423

File tree

5 files changed

+85
-11
lines changed

5 files changed

+85
-11
lines changed

‎nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ struct MapLocation
5858
{
5959
unsigned int x;
6060
unsigned int y;
61+
unsigned char cost;
6162
};
6263

6364
/**
@@ -311,6 +312,32 @@ class Costmap2D
311312
const std::vector<geometry_msgs::msg::Point> & polygon,
312313
unsigned char cost_value);
313314

315+
/**
316+
* @brief Gets the map region occupied by polygon
317+
* @param polygon The polygon to perform the operation on
318+
* @param polygon_map_region The map region occupied by the polygon
319+
* @return True if the polygon_map_region was filled... false if it could not be filled
320+
*/
321+
bool getMapRegionOccupiedByPolygon(
322+
const std::vector<geometry_msgs::msg::Point> & polygon,
323+
std::vector<MapLocation> & polygon_map_region);
324+
325+
/**
326+
* @brief Sets the given map region to desired value
327+
* @param polygon_map_region The map region to perform the operation on
328+
* @param new_cost_value The value to set costs to
329+
*/
330+
void setMapRegionOccupiedByPolygon(
331+
const std::vector<MapLocation> & polygon_map_region,
332+
unsigned char new_cost_value);
333+
334+
/**
335+
* @brief Restores the corresponding map region using given map region
336+
* @param polygon_map_region The map region to perform the operation on
337+
*/
338+
void restoreMapRegionOccupiedByPolygon(
339+
const std::vector<MapLocation> & polygon_map_region);
340+
314341
/**
315342
* @brief Get the map cells that make up the outline of a polygon
316343
* @param polygon The polygon in map coordinates to rasterize
@@ -568,6 +595,7 @@ class Costmap2D
568595
{
569596
MapLocation loc;
570597
costmap_.indexToCells(offset, loc.x, loc.y);
598+
loc.cost = costmap_.getCost(loc.x, loc.y);
571599
cells_.push_back(loc);
572600
}
573601

‎nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,7 @@ class StaticLayer : public CostmapLayer
163163

164164
std::vector<geometry_msgs::msg::Point> transformed_footprint_;
165165
bool footprint_clearing_enabled_;
166+
bool restore_cleared_footprint_;
166167
/**
167168
* @brief Clear costmap layer info below the robot's footprint
168169
*/

‎nav2_costmap_2d/plugins/static_layer.cpp

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,7 @@ StaticLayer::getParameters()
138138
declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
139139
declareParameter("map_topic", rclcpp::ParameterValue("map"));
140140
declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false));
141+
declareParameter("restore_cleared_footprint", rclcpp::ParameterValue(true));
141142

142143
auto node = node_.lock();
143144
if (!node) {
@@ -147,6 +148,7 @@ StaticLayer::getParameters()
147148
node->get_parameter(name_ + "." + "enabled", enabled_);
148149
node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
149150
node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
151+
node->get_parameter(name_ + "." + "restore_cleared_footprint", restore_cleared_footprint_);
150152
node->get_parameter(name_ + "." + "map_topic", map_topic_);
151153
map_topic_ = joinWithParentNamespace(map_topic_);
152154
node->get_parameter(
@@ -411,8 +413,11 @@ StaticLayer::updateCosts(
411413
return;
412414
}
413415

416+
std::vector<MapLocation> map_region_to_restore;
414417
if (footprint_clearing_enabled_) {
415-
setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
418+
map_region_to_restore.reserve(100);
419+
getMapRegionOccupiedByPolygon(transformed_footprint_, map_region_to_restore);
420+
setMapRegionOccupiedByPolygon(map_region_to_restore, nav2_costmap_2d::FREE_SPACE);
416421
}
417422

418423
if (!layered_costmap_->isRolling()) {
@@ -458,6 +463,11 @@ StaticLayer::updateCosts(
458463
}
459464
}
460465
}
466+
467+
if (footprint_clearing_enabled_ && restore_cleared_footprint_) {
468+
// restore the map region occupied by the polygon using cached data
469+
restoreMapRegionOccupiedByPolygon(map_region_to_restore);
470+
}
461471
current_ = true;
462472
}
463473

@@ -498,6 +508,13 @@ StaticLayer::dynamicParametersCallback(
498508
current_ = false;
499509
} else if (param_name == name_ + "." + "footprint_clearing_enabled") {
500510
footprint_clearing_enabled_ = parameter.as_bool();
511+
} else if (param_name == name_ + "." + "restore_cleared_footprint") {
512+
if (footprint_clearing_enabled_) {
513+
restore_cleared_footprint_ = parameter.as_bool();
514+
} else {
515+
RCLCPP_WARN(logger_, "restore_cleared_footprint cannot be used "
516+
"when footprint_clearing_enabled is False. Rejecting parameter update.");
517+
}
501518
}
502519
}
503520
}

‎nav2_costmap_2d/src/costmap_2d.cpp

Lines changed: 36 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -399,29 +399,54 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
399399
bool Costmap2D::setConvexPolygonCost(
400400
const std::vector<geometry_msgs::msg::Point> & polygon,
401401
unsigned char cost_value)
402+
{
403+
std::vector<MapLocation> polygon_map_region;
404+
polygon_map_region.reserve(100);
405+
if (!getMapRegionOccupiedByPolygon(polygon, polygon_map_region)) {
406+
return false;
407+
}
408+
409+
// set the cost of those cells
410+
setMapRegionOccupiedByPolygon(polygon_map_region, cost_value);
411+
return true;
412+
}
413+
414+
void Costmap2D::setMapRegionOccupiedByPolygon(
415+
const std::vector<MapLocation> & polygon_map_region,
416+
unsigned char new_cost_value)
417+
{
418+
for (const auto & cell : polygon_map_region) {
419+
setCost(cell.x, cell.y, new_cost_value);
420+
}
421+
}
422+
423+
void Costmap2D::restoreMapRegionOccupiedByPolygon(
424+
const std::vector<MapLocation> & polygon_map_region)
425+
{
426+
for (const auto & cell : polygon_map_region) {
427+
setCost(cell.x, cell.y, cell.cost);
428+
}
429+
}
430+
431+
bool Costmap2D::getMapRegionOccupiedByPolygon(
432+
const std::vector<geometry_msgs::msg::Point> & polygon,
433+
std::vector<MapLocation> & polygon_map_region)
402434
{
403435
// we assume the polygon is given in the global_frame...
404436
// we need to transform it to map coordinates
405437
std::vector<MapLocation> map_polygon;
406-
for (unsigned int i = 0; i < polygon.size(); ++i) {
438+
for (const auto & cell : polygon) {
407439
MapLocation loc;
408-
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) {
440+
if (!worldToMap(cell.x, cell.y, loc.x, loc.y)) {
409441
// ("Polygon lies outside map bounds, so we can't fill it");
410442
return false;
411443
}
412444
map_polygon.push_back(loc);
413445
}
414446

415-
std::vector<MapLocation> polygon_cells;
416-
417447
// get the cells that fill the polygon
418-
convexFillCells(map_polygon, polygon_cells);
448+
convexFillCells(map_polygon, polygon_map_region);
419449

420-
// set the cost of those cells
421-
for (unsigned int i = 0; i < polygon_cells.size(); ++i) {
422-
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
423-
costmap_[index] = cost_value;
424-
}
425450
return true;
426451
}
427452

@@ -506,6 +531,7 @@ void Costmap2D::convexFillCells(
506531
for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) {
507532
pt.x = x;
508533
pt.y = y;
534+
pt.cost = getCost(x, y);
509535
polygon_cells.push_back(pt);
510536
}
511537
}

‎nav2_system_tests/src/system/nav2_system_params.yaml

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -196,6 +196,8 @@ global_costmap:
196196
static_layer:
197197
plugin: "nav2_costmap_2d::StaticLayer"
198198
map_subscribe_transient_local: True
199+
footprint_clearing_enabled: True
200+
restore_cleared_footprint: True
199201
inflation_layer:
200202
plugin: "nav2_costmap_2d::InflationLayer"
201203
cost_scaling_factor: 3.0

0 commit comments

Comments
 (0)