Added IsGoalNearby BT node

nav2_behavior_tree/CMakeLists.txt

@@ -156,6 +156,9 @@ list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node)
 add_library(nav2_would_a_route_recovery_help_condition_bt_node SHARED plugins/condition/would_a_route_recovery_help_condition.cpp)
 list(APPEND plugin_libs nav2_would_a_route_recovery_help_condition_bt_node)
 
+add_library(nav2_is_goal_nearby_condition_bt_node SHARED plugins/condition/is_goal_nearby_condition.cpp)
+list(APPEND plugin_libs nav2_is_goal_nearby_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp

@@ -0,0 +1,94 @@
+// Copyright (c) 2026 Jakub Chudziński
+//
+// 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_GOAL_NEARBY_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_
+
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp/condition_node.h"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that returns SUCCESS when
+ * remaining length of the current planned path
+ * is less than proximity_threshold and FAILURE otherwise.
+ *
+ * Use max_robot_pose_search_dist to limit search distance when path updates regularly
+ * to reduce computational cost. Disable (set negative) for full path search.
+ */
+class IsGoalNearbyCondition : public BT::ConditionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::IsGoalNearbyCondition
+   * @param condition_name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  IsGoalNearbyCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsGoalNearbyCondition() = delete;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("path", "Planned path"),
+      BT::InputPort<double>(
+        "proximity_threshold", 1.0,
+        "Proximity length (m) of the remaining path considered as a nearby"),
+      BT::InputPort<double>(
+        "max_robot_pose_search_dist", -1.0,
+        "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 negative value (default), whole path is searched every time"),
+      BT::InputPort<std::string>("global_frame", "map", "Global frame"),
+      BT::InputPort<std::string>("robot_base_frame", "base_link", "Robot base frame"),
+    };
+  }
+
+private:
+  nav2::LifecycleNode::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  nav_msgs::msg::Path path_;
+  std::vector<geometry_msgs::msg::PoseStamped>::iterator closest_pose_detection_begin_;
+  double transform_tolerance_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -472,6 +472,12 @@
       <input_port name="tolerance">Tolerance</input_port>
     </Condition>
 
+    <Condition ID="IsGoalNearby">
+      <input_port name="path">Planned path</input_port>
+      <input_port name="proximity_threshold">Proximity length (m) of the remaining path considered as a nearby</input_port>
+      <input_port name="max_robot_pose_search_dist">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 infinity (default), whole path is searched every time</input_port>
+    </Condition>
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 

nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp

@@ -0,0 +1,116 @@
+// Copyright (c) 2026 Jakub Chudziński
+//
+// 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_goal_nearby_condition.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsGoalNearbyCondition::IsGoalNearbyCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  transform_tolerance_(0.1)
+{
+  node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
+  tf_buffer_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+
+  global_frame_ = BT::deconflictPortAndParamFrame<std::string>(
+    node_, "global_frame", this);
+  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
+    node_, "robot_base_frame", this);
+}
+
+BT::NodeStatus IsGoalNearbyCondition::tick()
+{
+  nav_msgs::msg::Path new_path;
+  double prox_thr = 0.0;
+  double max_robot_pose_search_dist = -1.0;
+  getInput("path", new_path);
+  getInput("proximity_threshold", prox_thr);
+  getInput("max_robot_pose_search_dist", max_robot_pose_search_dist);
+
+  if(new_path.poses.empty()) {
+    RCLCPP_WARN(node_->get_logger(), "Path is empty");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  bool path_pruning = true;
+  if (max_robot_pose_search_dist < 0.0) {
+    path_pruning = false;
+  }
+
+
+  if (!path_pruning || new_path != path_) {
+    path_ = new_path;
+    closest_pose_detection_begin_ = path_.poses.begin();
+  }
+
+  geometry_msgs::msg::PoseStamped pose;
+  if (!nav2_util::getCurrentPose(
+      pose, *tf_buffer_, global_frame_, robot_base_frame_, transform_tolerance_))
+  {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to get current robot pose");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // let's get the pose of the robot in the frame of the plan
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+    path_.header.frame_id))
+  {
+    RCLCPP_ERROR(
+      node_->get_logger(), "Failed to transform robot pose to path frame '%s'",
+      path_.header.frame_id.c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  auto closest_pose_upper_bound = path_.poses.end();
+  if (path_pruning) {
+    closest_pose_upper_bound =
+      nav2_util::geometry_utils::first_after_integrated_distance(
+      closest_pose_detection_begin_, path_.poses.end(), max_robot_pose_search_dist);
+  }
+
+  // First find the closest pose on the path to the robot
+  // bounded by when the path turns around (if it does) so we don't get a pose from a later
+  // portion of the path
+  auto closest_pose_it =
+    nav2_util::geometry_utils::min_by(
+    closest_pose_detection_begin_, closest_pose_upper_bound,
+    [&robot_pose](const geometry_msgs::msg::PoseStamped & ps)
+    {
+      return nav2_util::geometry_utils::euclidean_distance(robot_pose, ps);
+    });
+
+  closest_pose_detection_begin_ = closest_pose_it;
+
+  const std::size_t closest_index =
+    static_cast<std::size_t>(closest_pose_it - path_.poses.begin());
+  const double remaining_length =
+    nav2_util::geometry_utils::calculate_path_length(path_, closest_index);
+
+  return (remaining_length < prox_thr) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsGoalNearbyCondition>("IsGoalNearby");
+}

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -28,6 +28,8 @@ plugin_add_test(test_condition_is_path_valid test_is_path_valid.cpp nav2_is_path
 
 plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_is_pose_occupied_condition_bt_node)
 
+plugin_add_test(test_condition_is_goal_nearby test_is_goal_nearby.cpp nav2_is_goal_nearby_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