Added goal updated condition node

nav2_behavior_tree/CMakeLists.txt

@@ -77,6 +77,9 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node)
 add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp)
 list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
 
+add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp)
+list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
+
 add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
 list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
 

nav2_behavior_tree/README.md

@@ -71,6 +71,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
 | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. |
+| GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
 | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
 | RandomCrawl | Action | This BT action invokes the RandomCrawl ROS2 action server, which is implemented by the nav2_experimental/nav2_rl/nav2_turtlebot3_rl (in the nav2_experimental branch) experimental module. The RandomCrawl action server will direct the robot to randomly navigate its environment without hitting any obstacles. |

nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

@@ -0,0 +1,71 @@
+// Copyright (c) 2020 Aitor Miguel Blanco
+//
+// 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__GOAL_UPDATED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class GoalUpdatedCondition : public BT::ConditionNode
+{
+public:
+  GoalUpdatedCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf)
+  : BT::ConditionNode(condition_name, conf)
+  {
+  }
+
+  GoalUpdatedCondition() = delete;
+
+  BT::NodeStatus tick() override
+  {
+    if (status() == BT::NodeStatus::IDLE) {
+      goal_ = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
+    if (goal_ != current_goal) {
+      goal_ = current_goal;
+      return BT::NodeStatus::SUCCESS;
+    }
+
+    return BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+private:
+  geometry_msgs::msg::PoseStamped goal_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>("GoalUpdated");
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -59,6 +59,7 @@ bt_navigator:
     - nav2_clear_costmap_service_bt_node
     - nav2_is_stuck_condition_bt_node
     - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
     - nav2_initial_pose_received_condition_bt_node
     - nav2_reinitialize_global_localization_service_bt_node
     - nav2_rate_controller_bt_node

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -59,6 +59,7 @@ bt_navigator:
     - nav2_clear_costmap_service_bt_node
     - nav2_is_stuck_condition_bt_node
     - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
     - nav2_initial_pose_received_condition_bt_node
     - nav2_reinitialize_global_localization_service_bt_node
     - nav2_rate_controller_bt_node

nav2_bringup/bringup/params/nav2_params.yaml

@@ -59,6 +59,7 @@ bt_navigator:
     - nav2_clear_costmap_service_bt_node
     - nav2_is_stuck_condition_bt_node
     - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
     - nav2_initial_pose_received_condition_bt_node
     - nav2_reinitialize_global_localization_service_bt_node
     - nav2_rate_controller_bt_node

nav2_bt_navigator/README.md

@@ -60,7 +60,9 @@ returns FAILURE, all nodes are halted and this node returns FAILURE.
 
 * Recovery: This is a control flow type node with two children.  It returns success if and only if the first child returns success. The second child will be executed only if the first child returns failure.  The second child is responsible for recovery actions such as re-initializing system or other recovery behaviors. If the recovery behaviors are succeeded, then the first child will be executed again.  The user can specify how many times the recovery actions should be taken before returning failure. The figure below depicts a simple recovery node.
 
-<img src="./doc/recovery_node.png" title="" width="40%" align="middle">
+<p align="center">
+<img src="./doc/recovery_node.png" title="" width="40%">
+</p>
 <br/>
 
 
@@ -75,32 +77,52 @@ returns FAILURE, all nodes are halted and this node returns FAILURE.
 
 The graphical version of this Behavior Tree:
 
-<img src="./doc/simple_parallel.png" title="" width="65%" align="middle">
+<p align="center">
+<img src="./doc/simple_parallel.png" title="" width="40%">
+</p>
 <br/>
 
 The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc.
 
 ### Navigate with replanning and simple recovery actions
 
-With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are:  `clearEntirelyCostmapServiceRequest` for both global and local costmaps and `spin`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
+With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are:  `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
 
-<img src="./doc/parallel_w_recovery.png" title="" width="95%" align="middle">
+<p align="center">
+<img src="./doc/parallel_w_recovery.png" title="" width="95%">
+</p>
 <br/>
 
 
 This tree is currently our default tree in the stack and the xml file is located here: [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml).
 
-## Multi-Scope Recoveries
+
+#### Halting recoveries on navigation goal (Preemption reference design)
+In general, the recovery behaviours and any other long running process should be stopped when the navigation goal is issued (e.g. preemption). In the default tree in the stack, this behaviour is accomplished using a condition node checking the global navigation goal and a reactive fallback controller node:
+
+<p align="center">
+<img src="./doc/recovery_w_goal_updated.png" title="" width="40%">
+</p>
+
+This way, the recovery actions can be interrupted if a new goal is sent to the bt_navigator. Adding other condition nodes to this structure, it is possible to halt the recoveries in other cases (e.g, giving a time limit for their execution). This is the recommended design pattern for preempting a node or tree branch under specific conditions such as a new navigation request. Please notice that the order of definition of the nodes in the xml file can alter the behaviour of this structure, and that all conditions should be placed before the recovery behaviour node or branch.
+
+
+#### Multi-Scope Recoveries
 Scope-based failure handling: Utilizing Behavior Trees with a recovery node allows one to handle failures at multiple scopes. With this capability, any action in a large system can be constructed with specific recovery actions suitable for that action. Thus, failures in these actions can be handled locally within the scope. With such design, a system can be recovered at multiple levels based on the nature of the failure. Higher level recovery actions could be recovery actions such as re-initializing the system, re-calibrating the robot, bringing the system to a good known state, etc.
 
 ### Navigate with replanning and simple Multi-Scope Recoveries
 In the navigation stack, multi-scope recovery actions are implemented for each module.  Currently, the recovery actions for the Global planner are: Clear Entire Global Costmap and Wait.  The recovery actions for the Local planner are: Clear Entire Local Costmap and Spin; the recovery actions for the system level is just Wait. The figure below highlights a simple multi-scope recovery handling for the navigation task.  With this tree, if the Global Planner fails, the ClearEntireCostmap which is the first recovery action for this module will be ticked, then the ComputePathToPose will be ticked again. If the ComputePathToPose fails again, the Wait which is the second recovery action for this module will be ticked. After trying the second recovery action, the ComputePathToPose will be ticked again. These actions can be repeated n times until ComputePathToPose succeeds.  If the ComputePathToPose fails and the Global Planner cannot be recovered locally, the higher-level recovery actions will be ticked.  In this simple example, our higher-level recovery action is just a longer wait. The same strategy is applied to the Local Planner. If the Local Planner fails, the tree will first tick the ClearEntireCostmap and then if it fails again the tree will tick the Spin.
 
-<img src="./doc/parallel_w_round_robin_recovery.png" title="" width="95%" align="middle">
 <br/>
 
+<p align="center">
+<img src="./doc/parallel_w_round_robin_recovery.png" title="" width="95%">
+</p>
+
 This tree currently is not our default tree in the stack. The xml file is located here: [navigate_w_replanning_and_round_robin_recovery.xml](behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml).
 
+<br/>
+
 ## Open Issues
 
 * **Schema definition and XML document validation** - Currently, there is no dynamic validation of incoming XML. The Behavior-Tree.CPP library is using tinyxml2, which doesn't have a validator. Instead, we can create a schema for the Mission Planning-level XML and use build-time validation of the XML input to ensure that it is well-formed and valid.

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -18,12 +18,15 @@
           <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </PipelineSequence>
-      <SequenceStar name="RecoveryActions">
-        <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
-        <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
-        <Spin spin_dist="1.57"/>
-        <Wait wait_duration="5"/>
-      </SequenceStar>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <SequenceStar name="RecoveryActions">
+          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          <Spin spin_dist="1.57"/>
+          <Wait wait_duration="5"/>
+        </SequenceStar>
+      </ReactiveFallback>
     </RecoveryNode>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml

@@ -10,21 +10,30 @@
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="4" name="ComputePathToPose">
             <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-            <RoundRobin name="GlobalPlannerRecoveryActions">
-              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
-              <Wait wait_duration="2"/>
-            </RoundRobin>
+            <ReactiveFallback name="PathToPoseFallback">
+              <GoalUpdated/>
+              <RoundRobin name="GlobalPlannerRecoveryActions">
+                <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+                <Wait wait_duration="2"/>
+              </RoundRobin>
+            </ReactiveFallback>
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="4" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath"/>
-          <RoundRobin name="PlannerRecoveryActions">
-            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
-            <Spin spin_dist="1.57"/>
-          </RoundRobin>
+          <ReactiveFallback name="FollowPathFallback">
+            <GoalUpdated/>
+            <RoundRobin name="PlannerRecoveryActions">
+              <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+              <Spin spin_dist="1.57"/>
+            </RoundRobin>
+          </ReactiveFallback>
         </RecoveryNode>
       </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
         <Wait wait_duration="5"/>
+      </ReactiveFallback>
     </RecoveryNode>
   </BehaviorTree>
 </root>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -42,6 +42,7 @@ BtNavigator::BtNavigator()
     "nav2_is_stuck_condition_bt_node",
     "nav2_goal_reached_condition_bt_node",
     "nav2_initial_pose_received_condition_bt_node",
+    "nav2_goal_updated_condition_bt_node",
     "nav2_reinitialize_global_localization_service_bt_node",
     "nav2_rate_controller_bt_node",
     "nav2_recovery_node_bt_node",

tools/bt2img.py

@@ -57,6 +57,7 @@
     "IsStuck",
     "GoalReached",
     "initialPoseReceived",
+    "GoalUpdated",
     ]
 
 def main():