Add isPoseOccupied bt node

configuration/packages/bt-plugins/conditions/IsPoseOccupied.rst

@@ -0,0 +1,106 @@
+.. _bt_is_pose_occupied_condition:
+
+IsPoseOccupied
+==============
+
+Checks to see if the pose is occupied. If it is occupied, the condition returns SUCCESS, otherwise
+it returns FAILURE.
+
+Input Ports
+-----------
+
+:pose:
+
+  ========================= =======
+  Type                      Default
+  ------------------------- -------
+  geometry_msgs/PoseStamped N/A
+  ========================= =======
+
+  Description
+      Pose to check if it is occupied.
+
+:service_name:
+
+  ====== =======================================
+  Type   Default
+  ------ ---------------------------------------
+  string /global_costmap/get_cost_global_costmap
+  ====== =======================================
+
+  Description
+    costmap service name responsible for getting the cost.
+
+:cost_threshold:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  double 254.0
+  ====== =======
+
+  Description
+    The cost threshold above which a waypoint is considered in collision and should be removed. If ``use_footprint = false``, consider setting to 253 for occupied.
+
+:use_footprint:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  bool   true
+  ====== =======
+
+  Description
+    Whether to use the footprint cost or the point cost.
+
+:server_timeout:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  double 20.0
+  ====== =======
+
+  Description
+    Service response timeout (ms).
+
+:path:
+
+  ==================================== =======
+  Type                                 Default
+  ------------------------------------ -------
+  nav_msgs::msg::Path                  N/A
+  ==================================== =======
+
+  Description
+    The global path to check for validity.
+
+:max_cost:
+
+  ============== ==========
+  Type           Default
+  -------------- ----------
+  unsigned int   253
+  ============== ==========
+
+  Description
+    The maximum allowable cost for the path to be considered valid.
+
+:consider_unknown_as_obstacle:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  bool   false
+  ====== =======
+
+  Description
+    Whether to consider unknown cost (255) as obstacle.
+
+
+Example
+-------
+
+.. code-block:: xml
+
+    <IsPoseOccupied server_timeout="10" pose="{goal}" max_cost="254"/>

configuration/packages/configuring-bt-xml.rst

@@ -82,6 +82,7 @@ Condition Plugins
   bt-plugins/conditions/TimeExpired.rst
   bt-plugins/conditions/IsBatteryLow.rst
   bt-plugins/conditions/IsPathValid.rst
+  bt-plugins/conditions/IsPoseOccupied.rst
   bt-plugins/conditions/PathExpiringTimer.rst
   bt-plugins/conditions/AreErrorCodesPresent.rst
   bt-plugins/conditions/WouldAControllerRecoveryHelp.rst

plugins/index.rst

@@ -589,6 +589,9 @@ Behavior Tree Nodes
 | `Are Poses Near Condition`_        |  Steve Macenski    | Checks if 2 poses are  |
 |                                    |                    | nearby to each other.  |
 +------------------------------------+--------------------+------------------------+
+| `Is Pose Occupied Condition`_      | Maurice Alexander  | Checks if a pose is    |
+|                                    | Purnawan           | occupied.              |
++------------------------------------+--------------------+------------------------+
 
 .. _Goal Reached Condition: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
 .. _Goal Updated Condition: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp
@@ -609,6 +612,7 @@ Behavior Tree Nodes
 .. _Would A Route Recovery Help: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.cpp
 .. _Is Battery Charging Condition: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
 .. _Are Poses Near Condition: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp
+.. _Is Pose Occupied Condition: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp
 
 +--------------------------+---------------------+----------------------------------+
 | Decorator Plugin Name    |    Creator          |       Description                |