Add IsGoalNearby bt node (#842)

* Add IsGoalNearby bt node

Signed-off-by: Jakubach <jakubach@gmail.com>

* Change default value of proximity_threshold

Signed-off-by: Jakubach <jakubach@gmail.com>

* Update description of max_robot_pose_search_dist

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Jakub Chudziński <jakubach@gmail.com>

---------

Signed-off-by: Jakubach <jakubach@gmail.com>
Signed-off-by: Jakub Chudziński <jakubach@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: Jakubach

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

@@ -0,0 +1,87 @@
+.. _bt_is_goal_nearby_condition:
+
+IsGoalNearby
+============
+
+Checks if the robot is near the goal by computing the remaining path length from the robot's current position to the goal. Returns SUCCESS when the remaining path length is less than the proximity threshold, otherwise returns FAILURE.
+
+Parameter
+---------
+
+:transform_tolerance:
+
+  Defined and declared in :ref:`configuring_bt_navigator`.
+
+Example
+^^^^^^^
+.. code-block:: yaml
+
+    bt_navigator:
+      ros__parameters:
+        # other bt_navigator parameters
+        transform_tolerance: 0.1
+
+Input Ports
+-----------
+
+:path:
+
+  ==================================== =======
+  Type                                 Default
+  ------------------------------------ -------
+  nav_msgs::msg::Path                  N/A
+  ==================================== =======
+
+  Description
+    The planned path to evaluate.
+
+:proximity_threshold:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  double 1.0
+  ====== =======
+
+  Description
+    The remaining path length (in meters) considered as "nearby". When the remaining distance along the path is less than this threshold, the condition returns SUCCESS.
+
+:max_robot_pose_search_dist:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  double -1.0
+  ====== =======
+
+  Description
+    Maximum forward integrated distance along the path (starting from the last detected pose) to bound the search for the closest pose to the robot. When set to a negative value (default), the entire path is searched every time. Setting this to a positive value (e.g., 1.0-2.0 meters) can improve performance when this BT node is ticked frequently to address looping or crossed paths (when present).
+
+:global_frame:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  string "map"
+  ====== =======
+
+  Description
+    The global reference frame.
+
+:robot_base_frame:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  string "base_link"
+  ====== =======
+
+  Description
+    Robot base frame.
+
+Example
+^^^^^^^
+
+.. code-block:: xml
+
+  <IsGoalNearby path="{path}" proximity_threshold="1.0" />

configuration/packages/configuring-bt-xml.rst

@@ -80,6 +80,7 @@ Condition Plugins
   bt-plugins/conditions/GoalUpdated.rst
   bt-plugins/conditions/GlobalUpdatedGoal.rst
   bt-plugins/conditions/InitialPoseReceived.rst
+  bt-plugins/conditions/IsGoalNearby.rst
   bt-plugins/conditions/IsStuck.rst
   bt-plugins/conditions/IsStopped.rst
   bt-plugins/conditions/TimeExpired.rst

plugins/index.rst

@@ -619,6 +619,10 @@ Behavior Tree Nodes
 | `Is Pose Occupied Condition`_      | Maurice Alexander  | Checks if a pose is    |
 |                                    | Purnawan           | occupied.              |
 +------------------------------------+--------------------+------------------------+
+| `Is Goal Nearby Condition`_        | Jakub Chudziński   | Checks if the robot is |
+|                                    |                    | near the goal based on |
+|                                    |                    | remaining path length. |
++------------------------------------+--------------------+------------------------+
 
 .. _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
@@ -640,6 +644,7 @@ Behavior Tree Nodes
 .. _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
+.. _Is Goal Nearby Condition: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp
 
 +--------------------------+---------------------+----------------------------------+
 | Decorator Plugin Name    |    Creator          |       Description                |