Clear costmap around pose jazzy (backport #5309) (#5318)

* Adding clear costmap around pose service option (#5309)

(cherry picked from commit c0bf67e
Signed-off-by: dw25628 <[email protected]>

* Linting

Signed-off-by: dw25628 <[email protected]>

* Removed __init__.py that came in with cherry pick

Signed-off-by: dw25628 <[email protected]>

---------

Signed-off-by: dw25628 <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: dw25628

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"
 
 namespace nav2_behavior_tree
@@ -122,6 +123,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_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -72,6 +72,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>

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");
 }

nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp

@@ -302,6 +302,101 @@ TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick)
   EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
 }
 
+class ClearCostmapAroundPoseService : public TestService<nav2_msgs::srv::ClearCostmapAroundPose>
+{
+public:
+  ClearCostmapAroundPoseService()
+  : TestService("clear_costmap_around_pose")
+  {}
+};
+
+class ClearCostmapAroundPoseServiceTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("clear_costmap_around_pose_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set(
+      "node",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set("initial_pose_received", false);
+    config_->blackboard->set("number_recoveries", 0);
+
+    factory_->registerNodeType<nav2_behavior_tree::ClearCostmapAroundPoseService>(
+      "ClearCostmapAroundPose");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ClearCostmapAroundPoseService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr
+ClearCostmapAroundPoseServiceTestFixture::node_ = nullptr;
+std::shared_ptr<ClearCostmapAroundPoseService>
+ClearCostmapAroundPoseServiceTestFixture::server_ = nullptr;
+BT::NodeConfiguration
+* ClearCostmapAroundPoseServiceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+ClearCostmapAroundPoseServiceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree>
+ClearCostmapAroundPoseServiceTestFixture::tree_ = nullptr;
+
+TEST_F(ClearCostmapAroundPoseServiceTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <ClearCostmapAroundPose service_name="clear_costmap_around_pose"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
@@ -327,13 +422,20 @@ int main(int argc, char ** argv)
       rclcpp::spin(ClearCostmapAroundRobotServiceTestFixture::server_);
     });
 
+  ClearCostmapAroundPoseServiceTestFixture::server_ =
+    std::make_shared<ClearCostmapAroundPoseService>();
+  std::thread server_thread_around_pose([]() {
+      rclcpp::spin(ClearCostmapAroundPoseServiceTestFixture::server_);
+    });
+
   int all_successful = RUN_ALL_TESTS();
 
   // shutdown ROS
   rclcpp::shutdown();
   server_thread.join();
   server_thread_except_region.join();
   server_thread_around_robot.join();
+  server_thread_around_pose.join();
 
   return all_successful;
 }

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_util/lifecycle_node.hpp"
@@ -58,6 +59,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
    */
@@ -92,6 +98,16 @@ class ClearCostmapService
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
 
+  rclcpp::Service<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);
+
   rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
   /**
    * @brief Callback to clear costmap

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>(
     "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;

nav2_msgs/CMakeLists.txt

@@ -35,6 +35,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"

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