Repository: ros-navigation/navigation2 (C++)

Issue:
Costmap clear around pose service
## Feature request

#### Feature description
<!-- Description in a few sentences what the feature consists of and what problem it will solve -->

I would like to propose an enhancement to the existing clear_costmap_service by adding a new service that allows clearing the costmap around a specified pose. This is particularly useful in scenarios where the planner fails due to outdated costmap data at the goal location which is outside of your obstacle/raytrace range—often caused by dynamic obstacles that have since moved. Instead of clearing the entire costmap as a recovery behavior, this feature would enable targeted clearing, preserving useful map data.

#### Implementation considerations
<!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->

- Define a new service type: ClearCostmapAroundPose.srv, which includes:

geometry_msgs/PoseStamped pose: the center of the area to clear
float64 reset_distance: the radius around the pose to clear

- Add a new service server to nav2_costmap_2d under the existing clear_costmap_service node.

- Extend the clear_costmap_service BT plugin to support this new service client.


If the maintainers agree with this direction, I’d be happy to take ownership of the implementation.





Discussion: Have we talked about this somewhere else? I recall something along these lines but cannot find another ticket with this description. 

But yes, I approve of the feature and plan! The only thing I'd add are to update any tests that use this service to exercise it, add to the docs.nav2.org configuration guide for the costmap server for any new parameters, and the migration guide to highlight this new feature. In that migration guide, I might even put in a quick example of calling it with the `goal` location before planning to perform the action you mention. I'm not sure that should be default behavior, but I'm open to being convinced on it as long as the radius is sufficiently limited :-) 

I've marked this as in progress then! Thanks for the contribution! 
@dw25628 I've been playing around with Claude and made this. Can you test that this meets your needs? I'll do a quality check but if you can test that this does what you need it to do, that would be great https://github.com/ros-navigation/navigation2/pull/5309



Fix (patch):
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
index 2d4afc27cf0..fe90502d829 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
@@ -20,6 +20,7 @@
 #include "nav2_behavior_tree/bt_service_node.hpp"
 #include "nav2_msgs/srv/clear_entire_costmap.hpp"
 #include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
+#include "nav2_msgs/srv/clear_costmap_around_pose.hpp"
 #include "nav2_msgs/srv/clear_costmap_except_region.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 
@@ -129,6 +130,47 @@ class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::Clea
   }
 };
 
+/**
+ * @brief A nav2_behavior_tree::BtServiceNode class that
+ * wraps nav2_msgs::srv::ClearCostmapAroundPose
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
+ */
+class ClearCostmapAroundPoseService : public BtServiceNode<nav2_msgs::srv::ClearCostmapAroundPose>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::ClearCostmapAroundPoseService
+   * @param service_node_name Service name this node creates a client for
+   * @param conf BT node configuration
+   */
+  ClearCostmapAroundPoseService(
+    const std::string & service_node_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief The main override required by a BT service
+   * @return BT::NodeStatus Status of tick execution
+   */
+  void on_tick() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<geometry_msgs::msg::PoseStamped>(
+          "pose", "Pose around which to clear the costmap"),
+        BT::InputPort<double>(
+          "reset_distance", 1.0,
+          "Distance from the pose under which obstacles are cleared")
+      });
+  }
+};
+
 }  // namespace nav2_behavior_tree
 
 #endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index 4439393afa4..a6a0dc012d1 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -81,6 +81,13 @@
       <input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
     </Action>
 
+    <Action ID="ClearCostmapAroundPose">
+      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="pose">Pose around which to clear the costmap</input_port>
+      <input_port name="reset_distance">Distance from the pose under which obstacles are cleared</input_port>
+    </Action>
+
     <Action ID="ComputePathToPose">
       <input_port name="goal">Destination to plan to</input_port>
       <input_port name="start">Use as the planner start pose instead of the current robot pose, if use_start is not false (i.e. not provided or set to true)</input_port>
diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp
index 2d007a6623e..d637571c740 100644
--- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp
+++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp
@@ -58,6 +58,20 @@ void ClearCostmapAroundRobotService::on_tick()
   increment_recovery_count();
 }
 
+ClearCostmapAroundPoseService::ClearCostmapAroundPoseService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<nav2_msgs::srv::ClearCostmapAroundPose>(service_node_name, conf)
+{
+}
+
+void ClearCostmapAroundPoseService::on_tick()
+{
+  getInput("pose", request_->pose);
+  getInput("reset_distance", request_->reset_distance);
+  increment_recovery_count();
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
@@ -68,4 +82,6 @@ BT_REGISTER_NODES(factory)
     "ClearCostmapExceptRegion");
   factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
     "ClearCostmapAroundRobot");
+  factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundPoseService>(
+    "ClearCostmapAroundPose");
 }
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
index 231c946014d..93d3b310dc2 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
@@ -22,6 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_msgs/srv/clear_costmap_except_region.hpp"
 #include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
+#include "nav2_msgs/srv/clear_costmap_around_pose.hpp"
 #include "nav2_msgs/srv/clear_entire_costmap.hpp"
 #include "nav2_costmap_2d/costmap_layer.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -60,6 +61,11 @@ class ClearCostmapService
    */
   void clearRegion(double reset_distance, bool invert);
 
+  /**
+   * @brief Clears the region around a specific pose
+   */
+  void clearAroundPose(const geometry_msgs::msg::PoseStamped & pose, double reset_distance);
+
   /**
    * @brief Clears all layers
    */
@@ -96,6 +102,16 @@ class ClearCostmapService
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
 
+  nav2::ServiceServer<nav2_msgs::srv::ClearCostmapAroundPose>::SharedPtr
+    clear_around_pose_service_;
+  /**
+   * @brief Callback to clear costmap around a given pose
+   */
+  void clearAroundPoseCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundPose::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundPose::Response> response);
+
   nav2::ServiceServer<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr
     clear_entire_service_;
   /**
diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
index 038fbb96311..eec73de6f9d 100644
--- a/nav2_costmap_2d/src/clear_costmap_service.cpp
+++ b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -29,6 +29,7 @@ using std::shared_ptr;
 using std::any_of;
 using ClearExceptRegion = nav2_msgs::srv::ClearCostmapExceptRegion;
 using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot;
+using ClearAroundPose = nav2_msgs::srv::ClearCostmapAroundPose;
 using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap;
 
 ClearCostmapService::ClearCostmapService(
@@ -52,6 +53,12 @@ ClearCostmapService::ClearCostmapService(
       &ClearCostmapService::clearAroundRobotCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
+  clear_around_pose_service_ = node->create_service<ClearAroundPose>(
+    std::string("clear_around_pose_") + costmap_.getName(),
+    std::bind(
+      &ClearCostmapService::clearAroundPoseCallback, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
   clear_entire_service_ = node->create_service<ClearEntirely>(
     std::string("clear_entirely_") + costmap_.getName(),
     std::bind(
@@ -64,6 +71,7 @@ ClearCostmapService::~ClearCostmapService()
   // make sure services shutdown.
   clear_except_service_.reset();
   clear_around_service_.reset();
+  clear_around_pose_service_.reset();
   clear_entire_service_.reset();
 }
 
@@ -87,6 +95,18 @@ void ClearCostmapService::clearAroundRobotCallback(
   clearRegion(request->reset_distance, false);
 }
 
+void ClearCostmapService::clearAroundPoseCallback(
+  const shared_ptr<rmw_request_id_t>/*request_header*/,
+  const shared_ptr<ClearAroundPose::Request> request,
+  const shared_ptr<ClearAroundPose::Response>/*response*/)
+{
+  RCLCPP_INFO(
+    logger_, "%s",
+    ("Received request to clear around pose for " + costmap_.getName()).c_str());
+
+  clearAroundPose(request->pose, request->reset_distance);
+}
+
 void ClearCostmapService::clearEntireCallback(
   const std::shared_ptr<rmw_request_id_t>/*request_header*/,
   const std::shared_ptr<ClearEntirely::Request>/*request*/,
@@ -99,6 +119,41 @@ void ClearCostmapService::clearEntireCallback(
   clearEntirely();
 }
 
+void ClearCostmapService::clearAroundPose(
+  const geometry_msgs::msg::PoseStamped & pose,
+  const double reset_distance)
+{
+  double x, y;
+
+  // Transform pose to costmap frame if necessary
+  geometry_msgs::msg::PoseStamped global_pose;
+  try {
+    if (pose.header.frame_id == costmap_.getGlobalFrameID()) {
+      global_pose = pose;
+    } else {
+      costmap_.getTfBuffer()->transform(pose, global_pose, costmap_.getGlobalFrameID());
+    }
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      logger_,
+      "Cannot clear map around pose because pose cannot be transformed to costmap frame: %s",
+      ex.what());
+    return;
+  }
+
+  x = global_pose.pose.position.x;
+  y = global_pose.pose.position.y;
+
+  auto layers = costmap_.getLayeredCostmap()->getPlugins();
+
+  for (auto & layer : *layers) {
+    if (layer->isClearable()) {
+      auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
+      clearLayerRegion(costmap_layer, x, y, reset_distance, false);
+    }
+  }
+}
+
 void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
 {
   double x, y;
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index dc49bf40f01..6252ab32e4e 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -43,6 +43,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/IsPathValid.srv"
   "srv/ClearCostmapExceptRegion.srv"
   "srv/ClearCostmapAroundRobot.srv"
+  "srv/ClearCostmapAroundPose.srv"
   "srv/ClearEntireCostmap.srv"
   "srv/ManageLifecycleNodes.srv"
   "srv/LoadMap.srv"
diff --git a/nav2_msgs/srv/ClearCostmapAroundPose.srv b/nav2_msgs/srv/ClearCostmapAroundPose.srv
new file mode 100644
index 00000000000..5d244279f67
--- /dev/null
+++ b/nav2_msgs/srv/ClearCostmapAroundPose.srv
@@ -0,0 +1,6 @@
+# Clears the costmap within a distance around a given pose
+
+geometry_msgs/PoseStamped pose
+float64 reset_distance
+---
+std_msgs/Empty response
diff --git a/nav2_msgs/srv/__init__.py b/nav2_msgs/srv/__init__.py
index 51658c3f3b8..444c4be926b 100644
--- a/nav2_msgs/srv/__init__.py
+++ b/nav2_msgs/srv/__init__.py
@@ -1,3 +1,4 @@
+from nav2_msgs.srv._clear_costmap_around_pose import ClearCostmapAroundPose
 from nav2_msgs.srv._clear_costmap_around_robot import ClearCostmapAroundRobot
 from nav2_msgs.srv._clear_costmap_except_region import ClearCostmapExceptRegion
 from nav2_msgs.srv._clear_entire_costmap import ClearEntireCostmap
@@ -13,6 +14,7 @@
 __all__ = [
     'ClearCostmapAroundRobot',
     'ClearCostmapExceptRegion',
+    'ClearCostmapAroundPose',
     'ClearEntireCostmap',
     'GetCostmap',
     'GetCosts',
diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
index 8071d07bb36..32ce21abc0a 100644
--- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
+++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
@@ -29,8 +29,9 @@
                               DriveOnHeading, FollowGPSWaypoints, FollowPath, FollowWaypoints,
                               NavigateThroughPoses, NavigateToPose, SmoothPath, Spin, UndockRobot)
 from nav2_msgs.msg import Route
-from nav2_msgs.srv import (ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap,
-                           GetCostmap, LoadMap, ManageLifecycleNodes)
+from nav2_msgs.srv import (ClearCostmapAroundPose, ClearCostmapAroundRobot,
+                           ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap,
+                           ManageLifecycleNodes)
 from nav_msgs.msg import Goals, OccupancyGrid, Path
 import rclpy
 from rclpy.action import ActionClient  # type: ignore[attr-defined]
@@ -230,6 +231,16 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N
             self.create_client(
             ClearCostmapAroundRobot, 'local_costmap/clear_costmap_around_robot'
         )
+        self.clear_local_costmap_around_pose_srv: Client[
+            ClearCostmapAroundPose.Request, ClearCostmapAroundPose.Response] = \
+            self.create_client(
+            ClearCostmapAroundPose, 'local_costmap/clear_costmap_around_pose'
+        )
+        self.clear_global_costmap_around_pose_srv: Client[
+            ClearCostmapAroundPose.Request, ClearCostmapAroundPose.Response] = \
+            self.create_client(
+            ClearCostmapAroundPose, 'global_costmap/clear_costmap_around_pose'
+        )
         self.get_costmap_global_srv: Client[
             GetCostmap.Request, GetCostmap.Response] = \
             self.create_client(
@@ -1076,6 +1087,28 @@ def clearCostmapAroundRobot(self, reset_distance: float) -> None:
         rclpy.spin_until_future_complete(self, future)
         return
 
+    def clearLocalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) -> None:
+        """Clear the costmap around a given pose."""
+        while not self.clear_local_costmap_around_pose_srv.wait_for_service(timeout_sec=1.0):
+            self.info('ClearLocalCostmapAroundPose service not available, waiting...')
+        req = ClearCostmapAroundPose.Request()
+        req.pose = pose
+        req.reset_distance = reset_distance
+        future = self.clear_local_costmap_around_pose_srv.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return
+
+    def clearGlobalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) -> None:
+        """Clear the global costmap around a given pose."""
+        while not self.clear_global_costmap_around_pose_srv.wait_for_service(timeout_sec=1.0):
+            self.info('ClearGlobalCostmapAroundPose service not available, waiting...')
+        req = ClearCostmapAroundPose.Request()
+        req.pose = pose
+        req.reset_distance = reset_distance
+        future = self.clear_global_costmap_around_pose_srv.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return
+
     def getGlobalCostmap(self) -> OccupancyGrid:
         """Get the global costmap."""
         while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):