Add isPoseOccupied BT node

nav2_behavior_tree/CMakeLists.txt

@@ -120,6 +120,9 @@ list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
 add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp)
 list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
 
+add_library(nav2_is_pose_occupied_condition_bt_node SHARED plugins/condition/is_pose_occupied_condition.cpp)
+list(APPEND plugin_libs nav2_is_pose_occupied_condition_bt_node)
+
 add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
 list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp

@@ -0,0 +1,101 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_cpp/json_export.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_msgs/srv/get_costs.hpp"
+#include "nav2_ros_common/service_client.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that returns SUCCESS when the IsPoseOccupied
+ * service returns true and FAILURE otherwise
+ */
+class IsPoseOccupiedCondition : public BT::ConditionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::IsPoseOccupiedCondition
+   * @param condition_name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  IsPoseOccupiedCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsPoseOccupiedCondition() = delete;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<std::chrono::milliseconds>();
+
+    return {
+      BT::InputPort<geometry_msgs::msg::PoseStamped>("pose", "Pose to check if occupied"),
+      BT::InputPort<std::string>("service_name", "global_costmap/get_cost_global_costmap"),
+      BT::InputPort<double>(
+          "cost_threshold", 254.0,
+          "Cost threshold for considering a pose occupied"),
+      BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
+      BT::InputPort<bool>(
+          "consider_unknown_as_obstacle", false,
+          "Whether to consider unknown cost as obstacle"),
+      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
+    };
+  }
+
+private:
+  nav2::LifecycleNode::SharedPtr node_;
+  nav2::ServiceClient<nav2_msgs::srv::GetCosts>::SharedPtr client_;
+  // The timeout value while waiting for a response from the
+  // get cost service
+  std::chrono::milliseconds server_timeout_;
+  bool use_footprint_;
+  bool consider_unknown_as_obstacle_;
+  double cost_threshold_;
+  std::string service_name_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -394,6 +394,15 @@
       <input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
     </Condition>
 
+    <Condition ID="IsPoseOccupied">
+      <input_port name="pose">Pose to check whether it is occupied</input_port>
+      <input_port name="service_name">Service name to call GetCosts</input_port>
+      <input_port name="cost_threshold">The cost threshold above which a pose is considered occupied</input_port>
+      <input_port name="use_footprint">Whether to use the footprint cost or the point cost</input_port>
+      <input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
+      <input_port name="server_timeout"> Server timeout </input_port>
+    </Condition>
+
     <Condition ID="WouldAControllerRecoveryHelp">
       <input_port name="error_code">Error code</input_port>
     </Condition>

nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp

@@ -0,0 +1,83 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp"
+#include <chrono>
+#include <memory>
+#include <string>
+
+namespace nav2_behavior_tree
+{
+
+IsPoseOccupiedCondition::IsPoseOccupiedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  use_footprint_(true), consider_unknown_as_obstacle_(false), cost_threshold_(254),
+  service_name_("global_costmap/get_cost_global_costmap")
+{
+  node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
+  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+}
+
+void IsPoseOccupiedCondition::initialize()
+{
+  getInput<std::string>("service_name", service_name_);
+  getInput<double>("cost_threshold", cost_threshold_);
+  getInput<bool>("use_footprint", use_footprint_);
+  getInput<bool>("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
+  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+  client_ =
+    node_->create_client<nav2_msgs::srv::GetCosts>(
+    service_name_,
+    false /* Does not create and spin an internal executor*/);
+}
+
+BT::NodeStatus IsPoseOccupiedCondition::tick()
+{
+  if (!BT::isStatusActive(status())) {
+    initialize();
+  }
+  geometry_msgs::msg::PoseStamped pose;
+  getInput("pose", pose);
+
+  auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
+  request->use_footprint = use_footprint_;
+  request->poses.push_back(pose);
+
+  auto response = client_->invoke(request, server_timeout_);
+
+  if(!response->success) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "GetCosts service call failed");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if((response->costs[0] == 255 && !consider_unknown_as_obstacle_) ||
+    response->costs[0] < cost_threshold_)
+  {
+    return BT::NodeStatus::FAILURE;
+  } else {
+    return BT::NodeStatus::SUCCESS;
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsPoseOccupiedCondition>("IsPoseOccupied");
+}

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -26,6 +26,8 @@ plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_ba
 
 plugin_add_test(test_condition_is_path_valid test_is_path_valid.cpp nav2_is_path_valid_condition_bt_node)
 
+plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_is_pose_occupied_condition_bt_node)
+
 plugin_add_test(test_are_error_codes_present test_are_error_codes_present.cpp nav2_would_a_controller_recovery_help_condition_bt_node)
 
 plugin_add_test(test_would_a_controller_recovery_help