Added promixity BT node and BT tree

nav2_behavior_tree/CMakeLists.txt

@@ -209,6 +209,9 @@ list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
 add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
 list(APPEND plugin_libs nav2_goal_updated_controller_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)
+
 foreach(bt_plugin ${plugin_libs})
   target_include_directories(${bt_plugin}
     PRIVATE

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

@@ -0,0 +1,81 @@
+// Copyright (c) 2024 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 "nav_msgs/msg/path.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp/condition_node.h"
+
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that returns SUCCESS when the IsGoalNearby
+ * service returns true and FAILURE otherwise
+ */
+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", 3.0,
+        "Proximity length (m) of the remaining path considered as a nearby"),
+    };
+  }
+
+private:
+
+    /**
+     * @brief Checks if the robot is in the goal proximity
+     * @param goal_path current planned path to the goal
+     * @param prox_thr proximity length (m) of the remaining path considered as a nearby
+     * @return whether the robot is in the goal proximity
+     */
+    bool isRobotInGoalProximity(
+        const nav_msgs::msg::Path& goal_path,
+        const double& prox_thr);
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp

@@ -0,0 +1,42 @@
+#include "nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsGoalNearbyCondition::IsGoalNearbyCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{
+}
+
+bool IsGoalNearbyCondition::isRobotInGoalProximity(
+  const nav_msgs::msg::Path& goal_path,
+  const double& prox_thr)
+{
+  return nav2_util::geometry_utils::calculate_path_length(goal_path, 0) < prox_thr;
+}
+
+
+BT::NodeStatus IsGoalNearbyCondition::tick()
+{
+    nav_msgs::msg::Path path;
+    double prox_thr;
+    getInput("path", path);
+    getInput("proximity_threshold", prox_thr);
+    if (!path.poses.empty()){
+        if(isRobotInGoalProximity(path,prox_thr)){
+            return BT::NodeStatus::SUCCESS;
+        }
+    }
+    return 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_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml

@@ -0,0 +1,46 @@
+
+<!--
+  This Behavior Tree replans the global path until desired proximity to the goal is achived or if the path becomes invalid. It also has
+  recovery actions specific to planning / control as well as general system issues.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence>
+        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
+        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
+        <RateController hz="1.0" name="RateControllerComputePathToPose">
+          <RecoveryNode number_of_retries="1" name="RecoveryComputePathToPose">
+            <Fallback name="FallbackComputePathToPose">
+              <ReactiveSequence name="CheckIfNewPathNeeded">
+                <Inverter>
+                  <GlobalUpdatedGoal/>
+                </Inverter>
+                <IsGoalNearby path="{path}" proximity_threshold="2.0"/>
+                <IsPathValid path="{path}"/>
+              </ReactiveSequence>
+              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
+            </Fallback>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="FallbackRecoveries">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
+          <Spin name="SpinRecovery" spin_dist="1.57"/>
+          <Wait name="WaitRecovery" wait_duration="5"/>
+          <BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>