(WIP) Recoveries always return success, regardless if they're able to…

… complete their required tasks or not (#1855)

* modified such that if the recovery is aborted due to potential collision, the return status will be FAILED

* Changed from SequenceStar to RoundRobin to mask any failure that might occur in the execution of the recovery action

* Change Backup recovery test such that the aborted recovery is expected and checked for

* fixing linting error

* Change depricated argument for backup recovery test

* added backup to the recovery actions, using defaults from the BackUp header

* Update nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

Co-authored-by: Steve Macenski <[email protected]>

* Reverting changes to BT. Seq. Star control node is in line to what we want

* checkpoitn commit

* Changed collision failure critera as backup collision abortion occurs @ -0.2 m as well. This was previously not caught as  the server always returned SUCCESS

* bool to control whether fake costmap should be generated

* added fake tester nodes

* Fake costmap passed through environmental variable now

* Fake Footprint being sent out

* breadcrumbs of fake spin tests

* fake odom

* launch file was messed up

* still troubleshooting spin

* trying to fake spin, goal keeps on getting rejected

* goal is now being accepted

* Fake spin not working well when asking for PI radians out

* playing around with timing wehn publishing fake transforms

* BT changes, testing TBD

* [WIP] Fix CI build issues (#2076)

* update to use on my fork to test changes to bondcpp

* using chrono literals in lifecycle manager

* nav2_rviz_plugins using chrono literals for API change

* using chrono literals in costmap_2d package from API change

* using chrono literals in observation buffer

* chrono literals for tests costmap

* chrono literal API changes

* changing API

* changing API

* API changes

* API change

* API change

* API change

* API change test

* API change test

* remove

* api updates

* update test values

* Patch for PhotoAtWaypoint plugin (#2067)

* nav2_way_point_follower; introduce photo at waypoint arrivals plugin

Signed-off-by: jediofgever <[email protected]>

* resolve cmake lint errors

Signed-off-by: jediofgever <[email protected]>

* resolve requested chages of first review

Signed-off-by: jediofgever <[email protected]>

* minor corrections on photo_at_waypoint header

* resolve requested changes of second review

Signed-off-by: jediofgever <[email protected]>

* update default save_dir

Signed-off-by: jediofgever <[email protected]>

* move directory checking code to initialize()

* add try catch block to catch possible execeptions while creating a directory for photo at waypoint plugin

Signed-off-by: jediofgever <[email protected]>

* adding logging and disabling

* Update nav2_waypoint_follower/plugins/photo_at_waypoint.cpp

Co-authored-by: Fetullah Atas  <[email protected]>

* Update photo_at_waypoint.cpp

Co-authored-by: jediofgever <[email protected]>

* Fix for double free issue in map server testcases (#2078)

* Support in keepout filter for mask and costmap published in different… (#2054)

* Support in keepout filter for mask and costmap published in different frames

This fixes incorrect keepouts position issue when filter mask and current
costmap layer are published in different frames. This might appear
(but not restricted only to) when keepout filter is enabled for local costmap
with rolling window.

* Add transform initialization

* Enhance transform failure message

* Loop fix (#2068)

* Abort analytic expansion if crossing through already visited node

* Check that we are not creating an infinite loop at the goal node

* Mark nodes in analytic expansion as visited for the sake of completeness

* Move checking of already visited nodes to final stage of analytic expansion

* test if action server failures correctly propagate into BT context

* change recovery subtree so that RoundRobin replaces SequenceStar for good

* uncrustify and line nav2_behavior_tree changes

* uncrustify and linting

* adding clearing actions to own subtree

* more linting

* reduced testing value to see if that will stop the robot from aborting due to potential collision

* tighten up tolerance on backup since the requested backup is so small

* delint

* cleaned up spin recovery such that we simulate the robot slowly spinning into place, now I just have to simulate collision scenarios

* linting

* change order of Spin and Wait to match original

* fake spin failing due to potential collision even when empty costmap

* lint

* more lint

* can get all the spins to pass now

* spin test strangely passing all cases even though costmap is populated

* spin recovery

* commmit before I try to visualize this

* now I know that there are two things publishing to the same footprint

* 1) figured out conflicting pubs to footprint, 2) why can't I get costmap occupancy to cause collision

* Fake Spin Test shows failure correctly for angles greater than pi / 2. change launch file such that two publishers are not generating costmaps

* fake launch lint

* check in with linting to show the structure

* Add overall behavior tree system tests

Signed-off-by: Sarthak Mittal <[email protected]>

* Add RecoverySubtreeGoalUpdated BT test

Signed-off-by: Sarthak Mittal <[email protected]>

* Update readme and docs

* Add tests to check if BT XML files are well-formed

Signed-off-by: Sarthak Mittal <[email protected]>

* Uncomment tests that were commented out

Signed-off-by: Sarthak Mittal <[email protected]>

Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: jediofgever <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: James Ward <[email protected]>
Co-authored-by: Sarthak Mittal <[email protected]>

nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp

@@ -32,9 +32,19 @@ class BackUpActionServer : public TestActionServer<nav2_msgs::action::BackUp>
 
 protected:
   void execute(
-    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>)
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
+    goal_handle)
   override
-  {}
+  {
+    nav2_msgs::action::BackUp::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::BackUp::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
 };
 
 class BackUpActionTestFixture : public ::testing::Test
@@ -154,6 +164,34 @@ TEST_F(BackUpActionTestFixture, test_tick)
   EXPECT_EQ(goal->speed, 0.26f);
 }
 
+TEST_F(BackUpActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <BackUp backup_dist="2" backup_speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->target.x, 2.0);
+  EXPECT_EQ(goal->speed, 0.26f);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_behavior_tree/test/plugins/action/test_spin_action.cpp

@@ -32,9 +32,19 @@ class SpinActionServer : public TestActionServer<nav2_msgs::action::Spin>
 
 protected:
   void execute(
-    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>)
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
+    goal_handle)
   override
-  {}
+  {
+    nav2_msgs::action::Spin::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::Spin::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
 };
 
 class SpinActionTestFixture : public ::testing::Test
@@ -149,6 +159,33 @@ TEST_F(SpinActionTestFixture, test_tick)
   EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
 }
 
+TEST_F(SpinActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Spin spin_dist="3.14" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  std::cout << tree_->rootNode()->status();
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+  EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_behavior_tree/test/test_action_server.hpp

@@ -48,6 +48,16 @@ class TestActionServer : public rclcpp::Node
     return current_goal_;
   }
 
+  void setReturnSuccess(bool return_success)
+  {
+    return_success_ = return_success;
+  }
+
+  bool getReturnSuccess(void)
+  {
+    return return_success_;
+  }
+
 protected:
   virtual rclcpp_action::GoalResponse handle_goal(
     const rclcpp_action::GoalUUID &,
@@ -77,6 +87,7 @@ class TestActionServer : public rclcpp::Node
 private:
   typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
   std::shared_ptr<const typename ActionT::Goal> current_goal_;
+  bool return_success_ = true;
 };
 
 #endif  // TEST_ACTION_SERVER_HPP_

nav2_bt_navigator/README.md

@@ -26,7 +26,7 @@ A Behavior Tree consists of control flow nodes, such as fallback, sequence, para
 
 The BT Navigator package has four sample XML-based descriptions of BTs.  
 These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml), [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml) and 
-[followpoint.xml](behavior_trees/followpoint.xml).  
+[follow_point.xml](behavior_trees/follow_point.xml).
 The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs.
 
 ### Navigate with Replanning (time-based)
@@ -59,7 +59,6 @@ returns FAILURE, all nodes are halted and this node returns FAILURE.
 </p>
 <br/>
 
-
 #### Decorator Nodes
 * RateController: A custom control flow node, which throttles down the tick rate.  This custom node has only one child and its tick rate is defined with a pre-defined frequency that the user can set.  This node returns RUNNING when it is not ticking its child. Currently, in the navigation, the `RateController` is used to tick the  `ComputePathToPose` and `GoalReached` node at 1 Hz.
 
@@ -69,7 +68,6 @@ returns FAILURE, all nodes are halted and this node returns FAILURE.
 
 * GoalUpdater: A custom control node, which updates the goal pose. It subscribes to a topic in which it can receive an updated goal pose to use instead of the one commanded in action. It is useful for dynamic object following tasks.
 
-
 #### Condition Nodes
 * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked.
 
@@ -90,7 +88,16 @@ The navigate with replanning BT first ticks the `RateController` node which spec
 
 ### 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_time.xml](behavior_trees/navigate_w_replanning_time.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.
+With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are:  `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin`, `wait`, and `backup`.
+
+This behavior tree implements multi-scope recovery where both the global planner and the controller have their own recovery actions on failure in addition to the main recovery subtree that gets triggered if the planner or controller continues to fail. The planner/controller specific recovery subtrees contain only a simple costmap clearing action in this BT. The main recovery subtree triggers recovery actions in a round robin fashion where the navigation subtree is retried after trying one recovery action. If the navigation subtree fails even after a retry, the `RoundRobin` node triggers the next recovery action in the sequence and the navigation subtree is retried. This cycle continues until:
+- The navigation subtree succeeds
+- All recovery actions fail
+- Specified number of retries is exceeded
+
+All recovery actions are preemptable and are halted when a new navigation goal arrives.
+
+A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
 
 <p align="center">
 <img src="./doc/parallel_w_recovery.png" title="" width="95%">
@@ -114,30 +121,13 @@ This way, the recovery actions can be interrupted if a new goal is sent to the b
 #### 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.
-
-<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).
-
 ### Navigate following a dynamic point to a certain distance
 This tree is an example of how behavior trees can be used to make the robot do more than navigate the current position to the desired pose. In this case, the robot will follow a point that changes dynamically during the execution of the action. The robot will stop its advance and will be oriented towards the target position when it reaches a distance to the target established in the tree. The UpdateGoal decorator node will be used to update the target position. The TruncatePath node will modify the generated path by removing the end part of this path, to maintain a distance from the target, and changes the orientation of the last position. This tree never returns that the action has finished successfully, but must be canceled when you want to stop following the target position.
 
 ![Navigate following a dynamic point to a certain distance](./doc/follow_point.png)
 
 This tree currently is not our default tree in the stack. The xml file is located here: [follow_point.xml](behavior_trees/follow_point.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.
-
 ## Legend
 Legend for the behavior tree diagrams:
 

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -10,22 +10,31 @@
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
             <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+            <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
+              <GoalUpdated/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+            </ReactiveFallback>
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath"/>
-          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          <ReactiveFallback name="FollowPathRecoveryFallback">
+            <GoalUpdated/>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          </ReactiveFallback>
         </RecoveryNode>
       </PipelineSequence>
       <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"/>
+        <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 spin_dist="1.57"/>
           <Wait wait_duration="5"/>
-        </SequenceStar>
+          <BackUp backup_dist="0.15" backup_speed="0.025"/>
+        </RoundRobin>
       </ReactiveFallback>
     </RecoveryNode>
   </BehaviorTree>

nav2_recoveries/plugins/back_up.cpp

@@ -108,7 +108,7 @@ Status BackUp::onCycleUpdate()
   if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
     stopRobot();
     RCLCPP_WARN(logger_, "Collision Ahead - Exiting BackUp");
-    return Status::SUCCEEDED;
+    return Status::FAILED;
   }
 
   vel_pub_->publish(std::move(cmd_vel));

nav2_recoveries/plugins/spin.cpp

@@ -119,7 +119,7 @@ Status Spin::onCycleUpdate()
   action_server_->publish_feedback(feedback_);
 
   double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
-  if (remaining_yaw <= 0) {
+  if (remaining_yaw < 1e-6) {
     stopRobot();
     return Status::SUCCEEDED;
   }
@@ -138,7 +138,7 @@ Status Spin::onCycleUpdate()
   if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
     stopRobot();
     RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
-    return Status::SUCCEEDED;
+    return Status::FAILED;
   }
 
   vel_pub_->publish(std::move(cmd_vel));

nav2_system_tests/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_map_server REQUIRED)
+find_package(nav2_behavior_tree REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
@@ -21,6 +22,7 @@ find_package(nav2_navfn_planner REQUIRED)
 find_package(nav2_planner REQUIRED)
 find_package(navigation2)
 find_package(angles REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
 
 nav2_package()
 
@@ -33,6 +35,7 @@ set(dependencies
   visualization_msgs
   nav2_amcl
   nav2_lifecycle_manager
+  nav2_behavior_tree
   gazebo_ros_pkgs
   geometry_msgs
   std_msgs
@@ -41,6 +44,7 @@ set(dependencies
   nav2_planner
   nav2_navfn_planner
   angles
+  behaviortree_cpp_v3
 )
 
 if(BUILD_TESTING)
@@ -50,6 +54,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
 
+  add_subdirectory(src/behavior_tree)
   add_subdirectory(src/planning)
   add_subdirectory(src/localization)
   add_subdirectory(src/system)

nav2_system_tests/package.xml

@@ -17,6 +17,7 @@
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav2_lifecycle_manager</build_depend>
   <build_depend>nav2_navfn_planner</build_depend>
+  <build_depend>nav2_behavior_tree</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>nav2_amcl</build_depend>
@@ -40,6 +41,7 @@
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>nav2_navfn_planner</exec_depend>
+  <exec_depend>nav2_behavior_tree</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>

nav2_system_tests/src/behavior_tree/CMakeLists.txt

@@ -0,0 +1,16 @@
+find_package(Boost COMPONENTS system filesystem REQUIRED)
+
+ament_add_gtest(test_behavior_tree_node
+  test_behavior_tree_node.cpp
+  server_handler.cpp
+)
+
+ament_target_dependencies(test_behavior_tree_node
+  ${dependencies}
+)
+
+target_include_directories(test_behavior_tree_node PUBLIC ${Boost_INCLUDE_DIRS})
+target_link_libraries(test_behavior_tree_node
+  ${Boost_FILESYSTEM_LIBRARY}
+  ${Boost_SYSTEM_LIBRARY}
+)