Iron Sync 6: May 23, 2024 (#4366)

* Scale cost critic's weight when dynamically updated (#4246) * Scale cost critic's weight when dynamically updated Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co> * sign off Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co> --------- Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co> * Add expanding the ~/ to the full home dir of user in the path to the map yaml. (#4258) * Add user home expander of home sequence Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Add passing home dir as string instead of const char* Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Add docs Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Fix function declaration Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Fix linter issues Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Uncrustify linter Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Uncrustify linter Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Uncrustify linter: remove remove whitespace Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> --------- Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> * Implement Critic for Velocity Deadband Hardware Constraints (#4256) * Adding new velocity deadband critic. - add some tests - cast double to float - add new features from "main" branch - fix formating - add cost test - fix linting issue - add README Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> * Remove velocity deadband critic from defaults Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> * remove old weight Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> * fix velocity deadband critic tests Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> --------- Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> * removing clearable layer param (unused) (#4280) * provide message validation check API (#4276) * provide validation_message.hpp Signed-off-by: goes <GoesM@buaa.edu.cn> * fix typo Signed-off-by: goes <GoesM@buaa.edu.cn> * add test_validation_messages.cpp Signed-off-by: goes <GoesM@buaa.edu.cn> * change include-order Signed-off-by: goes <GoesM@buaa.edu.cn> * reformat Signed-off-by: goes <GoesM@buaa.edu.cn> * update test Signed-off-by: goes <GoesM@buaa.edu.cn> --------- Signed-off-by: goes <GoesM@buaa.edu.cn> Co-authored-by: goes <GoesM@buaa.edu.cn> * [RotationShimController] rotate also on short paths (#4290) When the path is shorter than ´forward_sampling_distance´ the rotatitonShimController would directly give control to the primary controller to navigate to the goal. This would lead to the exact behavior that was tried to be fixed by the rotationShimController: "The result is an awkward, stuttering, or whipping around behavior" [1]. It often resulted in navigation failure. In this case, the controller should try to rotate towards the last point of the path, so that the primary controller can have a better starting orientation. [1] https://navigation.ros.org/tuning/index.html#rotate-in-place-behavior * Add footprint clearing for static layer (#4282) * Add footprint clearing for static layer Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix flckering --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix #4268 (#4296) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix undefined symbols in `libpf_lib.so` (#4312) When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed `libpf_lib.so` has undefined symbols. This PR correctly links `libpf_lib.so` to `libm` so all symbols can be found. You can verify this by executing the following command: ``` ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so linux-vdso.so.1 (0x00007ffd1f8c0000) libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000) /lib64/ld-linux-x86-64.so.2 (0x000074e909e60000) undefined symbol: ceil (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: atan2 (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sin (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: hypot (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: cos (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: log (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sqrt (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: floor (./build/nav2_amcl/src/pf/libpf_lib.so) ``` Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> * msg validation check for `/initialpose` in `nav2_amcl` (#4301) * add validation check for PoseWithCovarianceStamped Signed-off-by: goes <GoesM@buaa.edu.cn> * remove rebundant check before Signed-off-by: goes <GoesM@buaa.edu.cn> * reformat Signed-off-by: goes <GoesM@buaa.edu.cn> * typo fixed Signed-off-by: goes <GoesM@buaa.edu.cn> * change the type-name Signed-off-by: goes <GoesM@buaa.edu.cn> * update test Signed-off-by: goes <GoesM@buaa.edu.cn> * reformat Signed-off-by: goes <GoesM@buaa.edu.cn> * . Signed-off-by: goes <GoesM@buaa.edu.cn> * add comment Signed-off-by: goes <GoesM@buaa.edu.cn> * update comment Signed-off-by: goes <GoesM@buaa.edu.cn> * change header Signed-off-by: goes <GoesM@buaa.edu.cn> * update test Signed-off-by: goes <GoesM@buaa.edu.cn> * typo fixed Signed-off-by: goes <GoesM@buaa.edu.cn> --------- Signed-off-by: goes <GoesM@buaa.edu.cn> Co-authored-by: goes <GoesM@buaa.edu.cn> * 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324) Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com> Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl> * [LifecycleNode] add bond_heartbeat_period (#4342) * add bond_heartbeat_period Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * lint Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Update path_longer_on_approach.cpp (#4344) Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> * [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Move projectState after getPointsInside (#4356) * Modify test to check fix Signed-off-by: Brice <brice.renaudeau@gmail.com> * Add static polygon check before simulation Signed-off-by: Brice <brice.renaudeau@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> * adding final pose in analytic expansion to check (#4353) * bump iron to 1.2.8 * fix merge conflict resolution --------- Signed-off-by: pepisg <pedro.gonzalez@eia.edu.co> Signed-off-by: Wiktor Bajor <wiktorbajor1@gmail.com> Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> Signed-off-by: goes <GoesM@buaa.edu.cn> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes <GoesM@buaa.edu.cn> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> Co-authored-by: AzaelCicero <k.t.pawelczyk@gmail.com> Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
ros-navigation · May 23, 2024 · 4cb6aba · 4cb6aba
1 parent 4e16d3c
commit 4cb6aba
Show file tree

Hide file tree

Showing 69 changed files with 958 additions and 164 deletions.
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -49,6 +49,7 @@
 #pragma GCC diagnostic pop
 
 #include "nav2_amcl/portable_utils.hpp"
+#include "nav2_util/validate_messages.hpp"
 
 using namespace std::placeholders;
 using rcl_interfaces::msg::ParameterType;
@@ -523,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
 
   RCLCPP_INFO(get_logger(), "initialPoseReceived");
 
-  if (msg->header.frame_id == "") {
-    // This should be removed at some point
-    RCLCPP_WARN(
-      get_logger(),
-      "Received initial pose with empty frame_id. You should always supply a frame_id.");
+  if (!nav2_util::validateMsg(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting.");
     return;
   }
   if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) {
@@ -1390,6 +1388,10 @@ void
 AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
 {
   RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received.");
+  if (!nav2_util::validateMsg(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting.");
+    return;
+  }
   if (first_map_only_ && first_map_received_) {
     return;
   }

diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt
@@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include)
 if(HAVE_DRAND48)
   target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
 endif()
+target_link_libraries(pf_lib m)
 
 install(TARGETS
   pf_lib

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp
@@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated(
 {
   return new_path != old_path && old_path.poses.size() != 0 &&
          new_path.poses.size() != 0 &&
-         old_path.poses.back() == new_path.poses.back();
+         old_path.poses.back().pose == new_path.poses.back().pose;
 }
 
 bool PathLongerOnApproach::isRobotInGoalProximity(

diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
@@ -248,7 +248,12 @@ double Polygon::getCollisionTime(
   Velocity vel = velocity;
 
   // Array of points transformed to the frame concerned with pose on each simulation step
-  std::vector<Point> points_transformed;
+  std::vector<Point> points_transformed = collision_points;
+
+  // Check static polygon
+  if (getPointsInside(points_transformed) >= min_points_) {
+    return 0.0;
+  }
 
   // Robot movement simulation
   for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -792,7 +792,8 @@ TEST_F(Tester, testProcessApproach)
   // 3. Obstacle is inside robot footprint
   publishScan(0.5, curr_time);
   ASSERT_TRUE(waitData(0.5, 500ms, curr_time));
-  publishCmdVel(0.5, 0.2, 0.0);
+  // Publish impossible cmd_vel to ensure robot footprint is checked
+  publishCmdVel(1000000000.0, 0.2, 0.0);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);

diff --git a/nav2_common/package.xml b/nav2_common/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_core/package.xml b/nav2_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
@@ -67,7 +67,6 @@ class ClearCostmapService
 
   // Clearing parameters
   unsigned char reset_value_;
-  std::vector<std::string> clearable_layers_;
 
   // Server for clearing the costmap
   rclcpp::Service<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr clear_except_service_;

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -48,6 +48,7 @@
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -160,6 +161,17 @@ class StaticLayer : public CostmapLayer
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
+  std::vector<geometry_msgs::msg::Point> transformed_footprint_;
+  bool footprint_clearing_enabled_;
+  /**
+   * @brief Clear costmap layer info below the robot's footprint
+   */
+  void updateFootprint(
+    double robot_x, double robot_y, double robot_yaw, double * min_x,
+    double * min_y,
+    double * max_x,
+    double * max_y);
+
   std::string global_frame_;  ///< @brief The global frame for the costmap
   std::string map_frame_;  /// @brief frame that map is located in
 

diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -45,6 +45,7 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "tf2/convert.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/validate_messages.hpp"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
@@ -132,6 +133,7 @@ StaticLayer::getParameters()
   declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true));
   declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
   declareParameter("map_topic", rclcpp::ParameterValue(""));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false));
 
   auto node = node_.lock();
   if (!node) {
@@ -140,6 +142,7 @@ StaticLayer::getParameters()
 
   node->get_parameter(name_ + "." + "enabled", enabled_);
   node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
+  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
   std::string private_map_topic, global_map_topic;
   node->get_parameter(name_ + "." + "map_topic", private_map_topic);
   node->get_parameter("map_topic", global_map_topic);
@@ -277,6 +280,10 @@ StaticLayer::interpretValue(unsigned char value)
 void
 StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
 {
+  if (!nav2_util::validateMsg(*new_map)) {
+    RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
+    return;
+  }
   if (!map_received_) {
     processMap(*new_map);
     map_received_ = true;
@@ -328,7 +335,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u
 
 void
 StaticLayer::updateBounds(
-  double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x,
+  double robot_x, double robot_y, double robot_yaw, double * min_x,
   double * min_y,
   double * max_x,
   double * max_y)
@@ -366,6 +373,24 @@ StaticLayer::updateBounds(
   *max_y = std::max(wy, *max_y);
 
   has_updated_data_ = false;
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void
+StaticLayer::updateFootprint(
+  double robot_x, double robot_y, double robot_yaw,
+  double * min_x, double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  if (!footprint_clearing_enabled_) {return;}
+
+  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
+
+  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
+    touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
+  }
 }
 
 void
@@ -387,6 +412,10 @@ StaticLayer::updateCosts(
     return;
   }
 
+  if (footprint_clearing_enabled_) {
+    setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
+  }
+
   if (!layered_costmap_->isRolling()) {
     // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
     if (!use_maximum_) {
@@ -469,6 +498,10 @@ StaticLayer::dynamicParametersCallback(
         has_updated_data_ = true;
         current_ = false;
       }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      }
     }
   }
   result.successful = true;

diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -40,8 +40,6 @@ ClearCostmapService::ClearCostmapService(
   logger_ = node->get_logger();
   reset_value_ = costmap_.getCostmap()->getDefaultValue();
 
-  node->get_parameter("clearable_layers", clearable_layers_);
-
   clear_except_service_ = node->create_service<ClearExceptRegion>(
     "clear_except_" + costmap_.getName(),
     std::bind(

diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -108,8 +108,6 @@ void Costmap2DROS::init()
 {
   RCLCPP_INFO(get_logger(), "Creating Costmap");
 
-  std::vector<std::string> clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"};
-
   declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
   declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
   declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]")));
@@ -134,7 +132,6 @@ void Costmap2DROS::init()
   declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
   declare_parameter("update_frequency", rclcpp::ParameterValue(5.0));
   declare_parameter("use_maximum", rclcpp::ParameterValue(false));
-  declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers));
 }
 
 Costmap2DROS::~Costmap2DROS()

diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>The costmap_queue package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>TODO</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>The dwb_critics package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core

diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_dwb_controller</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>

diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_msgs</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_utils</name>
-  <version>1.2.7</version>
+  <version>1.2.8</version>
   <description>A handful of useful utility functions for nav_2d packages.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>