Add nav2_bt_waypoint_follower

Signed-off-by: ymd-stella <world.applepie@gmail.com>

doc/parameters/param_list.md

@@ -474,12 +474,12 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | `<name>`.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion |
 | `<name>`.allow_unknown | true | Whether to allow planning in unknown space |
 
-# waypoint_follower
+# bt_waypoint_follower
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
-| stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. |
-| loop_rate | 20 | Rate to check for results from current navigation task |
+| bt_xml_filename | N/A | path to the behavior tree XML description |
+| plugin_lib_names | [ "nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "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_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", "nav2_distance_traveled_condition_bt_node", "nav2_navigate_to_pose_action_bt_node", "nav2_all_goals_achieved_condition_bt_node", "nav2_get_next_goal_action_bt_node"] | list of behavior tree node shared libraries |
 
 # recoveries
 

nav2_bringup/bringup/launch/navigation_launch.py

@@ -32,13 +32,14 @@ def generate_launch_description():
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
+    waypoint_follower_bt_xml_filename = LaunchConfiguration('waypoint_follower_bt_xml_filename')
     map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
 
     lifecycle_nodes = ['controller_server',
                        'planner_server',
                        'recoveries_server',
                        'bt_navigator',
-                       'waypoint_follower']
+                       'bt_waypoint_follower']
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
     # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -53,6 +54,7 @@ def generate_launch_description():
     param_substitutions = {
         'use_sim_time': use_sim_time,
         'default_bt_xml_filename': default_bt_xml_filename,
+        'bt_xml_filename': waypoint_follower_bt_xml_filename,
         'autostart': autostart,
         'map_subscribe_transient_local': map_subscribe_transient_local}
 
@@ -88,7 +90,14 @@ def generate_launch_description():
             default_value=os.path.join(
                 get_package_share_directory('nav2_bt_navigator'),
                 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
-            description='Full path to the behavior tree xml file to use'),
+            description='Full path to the behavior tree xml file to use for bt_navigator'),
+
+        DeclareLaunchArgument(
+            'waypoint_follower_bt_xml_filename',
+            default_value=os.path.join(
+                get_package_share_directory('nav2_bt_waypoint_follower'),
+                'behavior_trees', 'follow_waypoints.xml'),
+            description='Full path to the behavior tree xml file to use for bt_waypoint_follower'),
 
         DeclareLaunchArgument(
             'map_subscribe_transient_local', default_value='false',
@@ -126,9 +135,9 @@ def generate_launch_description():
             remappings=remappings),
 
         Node(
-            package='nav2_waypoint_follower',
-            executable='waypoint_follower',
-            name='waypoint_follower',
+            package='nav2_bt_waypoint_follower',
+            executable='bt_waypoint_follower',
+            name='bt_waypoint_follower',
             output='screen',
             parameters=[configured_params],
             remappings=remappings),

nav2_bringup/bringup/params/nav2_params.yaml

@@ -280,3 +280,31 @@ recoveries_server:
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True
+
+bt_waypoint_follower:
+  ros__parameters:
+    use_sim_time: True
+    bt_xml_filename: "follow_waypoints.xml"
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - 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_distance_controller_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_all_goals_achieved_condition_bt_node
+    - nav2_get_next_goal_action_bt_node

nav2_waypoint_follower/CMakeLists.txt → nav2_bt_waypoint_follower/CMakeLists.txt

@@ -1,13 +1,18 @@
 cmake_minimum_required(VERSION 3.5)
-project(nav2_waypoint_follower)
+project(nav2_bt_waypoint_follower)
 
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(tf2_ros REQUIRED)
 
@@ -17,7 +22,7 @@ include_directories(
   include
 )
 
-set(executable_name waypoint_follower)
+set(executable_name bt_waypoint_follower)
 
 add_executable(${executable_name}
   src/main.cpp
@@ -26,19 +31,35 @@ add_executable(${executable_name}
 set(library_name ${executable_name}_core)
 
 add_library(${library_name} SHARED
-  src/waypoint_follower.cpp
+  src/bt_waypoint_follower.cpp
 )
 
 set(dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
+  std_msgs
+  geometry_msgs
+  nav2_behavior_tree
   nav_msgs
   nav2_msgs
+  behaviortree_cpp_v3
+  std_srvs
   nav2_util
   tf2_ros
 )
 
+add_library(nav2_all_goals_achieved_condition_bt_node SHARED plugins/condition/all_goals_achieved_condition.cpp)
+list(APPEND plugin_libs nav2_all_goals_achieved_condition_bt_node)
+
+add_library(nav2_get_next_goal_action_bt_node SHARED plugins/action/get_next_goal_action.cpp)
+list(APPEND plugin_libs nav2_get_next_goal_action_bt_node)
+
+foreach(bt_plugin ${plugin_libs})
+  ament_target_dependencies(${bt_plugin} ${dependencies})
+  target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+endforeach()
+
 ament_target_dependencies(${executable_name}
   ${dependencies}
 )
@@ -50,6 +71,7 @@ ament_target_dependencies(${library_name}
 )
 
 install(TARGETS ${library_name}
+                ${plugin_libs}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -63,11 +85,22 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
+install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY test/behavior_trees DESTINATION share/${PROJECT_NAME}/test)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)
 
+ament_export_libraries(
+  ${plugin_libs}
+)
+
 ament_package()

nav2_bt_waypoint_follower/README.md

@@ -0,0 +1,71 @@
+# Nav2 BT Waypoint Follower
+
+The Nav2 BT(Behavior Tree) Waypoint Follower module implements the [FollowWaypoints action](../nav2_msgs/action/FollowWaypoints.action). It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of waypoint following that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors.
+
+## Overview
+
+The BT Waypoint Follower receives goal poses and navigates the robot to the specified destination. To do so, the module reads an XML description of the Behavior Tree from a file, as specified by a Node parameter, and passes that to a generic [BehaviorTreeEngine class](../nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp) which uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) to dynamically create and execute the BT.
+
+## Specifying an input XML file
+
+The BT Waypoint Follower node has a parameter, *bt_xml_filename*, that can be specified using a ROS2 parameters YAML file, like this:
+
+```
+bt_waypoint_follower:
+  ros__parameters:
+    bt_xml_filename: <path-to-xml-file>
+```
+
+Using the XML filename as a parameter makes it easy to change or extend the logic used for navigation. Once can simply update the XML description for the BT and the BT Waypoint follower task server will use the new description.
+
+## Behavior Tree nodes
+
+A Behavior Tree consists of control flow nodes, such as fallback, sequence, parallel, and decorator, as well as two execution nodes: condition and action nodes. Execution nodes are the leaf nodes of the tree. When a leaf node is ticked, the node does some work and it returns either SUCCESS, FAILURE or RUNNING.  The current Navigation2 software implements a few custom nodes, including Conditions and Actions. The user can also define and register additional node types that can then be used in BTs and the corresponding XML descriptions.
+
+## FollowWaypoints Behavior Trees
+
+The BT Waypoint Follower package has three sample XML-based descriptions of BTs.  
+These trees are [follow_waypoints.xml](behavior_trees/follow_waypoints.xml), [follow_waypoints_with_skip.xml](behavior_trees/follow_waypoints_with_skip.xml) and [follow_waypoints_with_wait.xml](behavior_trees/follow_waypoints_with_wait.xml).  
+The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs.
+
+### FollowWaypoints
+
+This BT fails if NavigateToPose fails.
+
+![Navigation with time based replanning](./doc/follow_waypoints.png)
+
+### FollowWaypoints with skipping
+
+It will go to the next waypoint if NavigateToPose fails.
+
+![Navigation with distance based replanning](./doc/follow_waypoints_with_skip.png)
+
+### FollowWaypoints with simple recovery action
+
+With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [follow_waypoints_with_wait.xml](behavior_trees/follow_waypoints_with_wait.xml) and a recovery action `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
+
+<p align="center">
+<img src="./doc/follow_waypoints_with_wait.png" title="" width="95%">
+</p>
+<br/>
+
+#### FollowWaypoints is composed of the following custom condition, control and action nodes:
+
+#### Control Nodes
+* 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.
+
+<p align="center">
+<img src="../nav2_bt_navigator/doc/recovery_node.png" title="" width="40%">
+</p>
+<br/>
+
+#### Condition Nodes
+* AllGoalsAchieved: If the last goal has been achieved, it return SUCCESS.
+
+#### Action Nodes
+* GetNextGoal: If the current goal has been achieved, set the next goal.
+
+## Legend
+Legend for the behavior tree diagrams:
+
+![Legend](./doc/legend.png)

nav2_bt_waypoint_follower/behavior_trees/follow_waypoints.xml

@@ -0,0 +1,18 @@
+
+<!--
+  This Behavior Tree processes each of the given waypoints with NavigateToPose.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <ReactiveFallback name="FollowWaypoints">
+      <AllGoalsAchieved goal_achieved="{goal_achieved}"/>
+      <KeepRunningUntilFailure>
+        <ReactiveSequence>
+          <GetNextGoal goals="{goals}" goal="{goal}" goal_achieved="{goal_achieved}"/>
+          <NavigateToPose goal="{goal}"/>
+          <SetBlackboard output_key="goal_achieved" value="true"/>
+        </ReactiveSequence>
+      </KeepRunningUntilFailure>
+    </ReactiveFallback>
+  </BehaviorTree>
+</root>

nav2_bt_waypoint_follower/behavior_trees/follow_waypoints_with_skip.xml

@@ -0,0 +1,20 @@
+
+<!--
+  This Behavior Tree processes each of the given waypoints with NavigateToPose and skip to next waypoint if NavigateToPose is failed.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <ReactiveFallback name="FollowWaypoints">
+      <AllGoalsAchieved goal_achieved="{goal_achieved}"/>
+      <KeepRunningUntilFailure>
+        <ReactiveSequence>
+          <GetNextGoal goals="{goals}" goal="{goal}" goal_achieved="{goal_achieved}"/>
+          <ForceSuccess>
+            <NavigateToPose goal="{goal}"/>
+          </ForceSuccess>
+          <SetBlackboard output_key="goal_achieved" value="true"/>
+        </ReactiveSequence>
+      </KeepRunningUntilFailure>
+    </ReactiveFallback>
+  </BehaviorTree>
+</root>

nav2_bt_waypoint_follower/behavior_trees/follow_waypoints_with_wait.xml

@@ -0,0 +1,21 @@
+
+<!--
+  This Behavior Tree processes each of the given waypoints with NavigateToPose and wait if NavigateToPose is failed.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <ReactiveFallback name="FollowWaypoints">
+      <AllGoalsAchieved goal_achieved="{goal_achieved}"/>
+      <KeepRunningUntilFailure>
+        <ReactiveSequence>
+          <GetNextGoal goals="{goals}" goal="{goal}" goal_achieved="{goal_achieved}"/>
+          <RecoveryNode number_of_retries="1">
+            <NavigateToPose goal="{goal}"/>
+            <Wait wait_duration="5"/>
+          </RecoveryNode>
+          <SetBlackboard output_key="goal_achieved" value="true"/>
+        </ReactiveSequence>
+      </KeepRunningUntilFailure>
+    </ReactiveFallback>
+  </BehaviorTree>
+</root>