From 43a36475487a0abb681cba8183460c76dbbbaa7b Mon Sep 17 00:00:00 2001 From: Jonathan Blixt Date: Sat, 12 Apr 2025 11:45:41 -0500 Subject: [PATCH 01/29] Initial reworked behavior trees --- .gitmodules | 6 + config/behavior_trees/main_tree.btproj | 292 ++++++++++++++ config/behavior_trees/main_tree.xml | 34 ++ config/behavior_trees/nav2/follow_point.xml | 23 ++ .../behavior_trees/nav2/nav2_tree_nodes.xml | 355 ++++++++++++++++++ ...replanning_and_if_path_becomes_invalid.xml | 48 +++ ...hrough_poses_w_replanning_and_recovery.xml | 52 +++ ...gate_to_pose_w_replanning_and_recovery.xml | 50 +++ ..._replanning_goal_patience_and_recovery.xml | 49 +++ ...eplanning_only_if_path_becomes_invalid.xml | 46 +++ .../nav2/navigate_w_replanning_distance.xml | 16 + ...e_w_replanning_only_if_goal_is_updated.xml | 16 + ...eplanning_only_if_path_becomes_invalid.xml | 23 ++ .../nav2/navigate_w_replanning_speed.xml | 16 + .../nav2/navigate_w_replanning_time.xml | 16 + .../nav2/odometry_calibration.xml | 20 + src/behaviortree_cpp | 1 + src/behaviortree_executor/CMakeLists.txt | 44 +++ src/behaviortree_executor/LICENSE | 202 ++++++++++ .../launch/behavior_tree_launch.py | 45 +++ src/behaviortree_executor/package.xml | 36 ++ .../src/action/auto_dig.hpp | 60 +++ .../src/action/auto_offload.hpp | 54 +++ .../action/calibrate_field_coordinates.hpp | 51 +++ .../src/action/go_to_dig_location.hpp | 59 +++ .../src/behaviortree_executor.cpp | 99 +++++ src/behaviortree_ros2 | 1 + 27 files changed, 1714 insertions(+) create mode 100644 config/behavior_trees/main_tree.btproj create mode 100644 config/behavior_trees/main_tree.xml create mode 100644 config/behavior_trees/nav2/follow_point.xml create mode 100644 config/behavior_trees/nav2/nav2_tree_nodes.xml create mode 100644 config/behavior_trees/nav2/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml create mode 100644 config/behavior_trees/nav2/navigate_through_poses_w_replanning_and_recovery.xml create mode 100644 config/behavior_trees/nav2/navigate_to_pose_w_replanning_and_recovery.xml create mode 100644 config/behavior_trees/nav2/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml create mode 100644 config/behavior_trees/nav2/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml create mode 100644 config/behavior_trees/nav2/navigate_w_replanning_distance.xml create mode 100644 config/behavior_trees/nav2/navigate_w_replanning_only_if_goal_is_updated.xml create mode 100644 config/behavior_trees/nav2/navigate_w_replanning_only_if_path_becomes_invalid.xml create mode 100644 config/behavior_trees/nav2/navigate_w_replanning_speed.xml create mode 100644 config/behavior_trees/nav2/navigate_w_replanning_time.xml create mode 100644 config/behavior_trees/nav2/odometry_calibration.xml create mode 160000 src/behaviortree_cpp create mode 100644 src/behaviortree_executor/CMakeLists.txt create mode 100644 src/behaviortree_executor/LICENSE create mode 100644 src/behaviortree_executor/launch/behavior_tree_launch.py create mode 100644 src/behaviortree_executor/package.xml create mode 100644 src/behaviortree_executor/src/action/auto_dig.hpp create mode 100644 src/behaviortree_executor/src/action/auto_offload.hpp create mode 100644 src/behaviortree_executor/src/action/calibrate_field_coordinates.hpp create mode 100644 src/behaviortree_executor/src/action/go_to_dig_location.hpp create mode 100644 src/behaviortree_executor/src/behaviortree_executor.cpp create mode 160000 src/behaviortree_ros2 diff --git a/.gitmodules b/.gitmodules index ccf4d9d4..1b025f7b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -30,3 +30,9 @@ path = src/isaac_ros/isaac_ros_nvblox url = https://github.com/GOFIRST-Robotics/isaac_ros_nvblox.git branch = main +[submodule "src/behaviortree_ros2"] + path = src/behaviortree_ros2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git +[submodule "src/behaviortree_cpp"] + path = src/behaviortree_cpp + url = https://github.com/BehaviorTree/BehaviorTree.CPP.git diff --git a/config/behavior_trees/main_tree.btproj b/config/behavior_trees/main_tree.btproj new file mode 100644 index 00000000..ddc2da8f --- /dev/null +++ b/config/behavior_trees/main_tree.btproj @@ -0,0 +1,292 @@ + + + + + + + Error code + Error codes to check, user defined + + + Allowed time for spinning + If true recovery count will be incremented + Service name + Server timeout + Assisted teleop error code + + + + + Distance to backup + Speed at which to backup + Allowed time for reversing + Server name + Server timeout + Disable collision checking + "Back up error code" + + + + Server name to cancel the assisted teleop behavior + Server timeout + + + Server name to cancel the backup behavior + Server timeout + + + Server name to cancel the controller server + Server timeout + + + Service name to cancel the drive on heading behavior + Server timeout + + + Server name to cancel the spin behavior + Server timeout + + + Server name to cancel the wait behavior + Server timeout + + + Service name + Server timeout + Distance from the robot under which obstacles are cleared + + + Service name + Server timeout + Distance from the robot above which obstacles are cleared + + + Service name + Server timeout + + + Destinations to plan through + Start pose of the path if overriding current robot pose + Server name + Server timeout + Mapped name to the planner plugin type to use + Path created by ComputePathToPose node + "Compute path through poses error code" + + + Destination to plan to + Start pose of the path if overriding current robot pose + Mapped name to the planner plugin type to use + Server name + Server timeout + Path created by ComputePathToPose node + "Compute path to pose error code" + + + Name of the topic to receive controller selection commands + Default controller of the controller selector + Name of the selected controller received from the topic subcription + + + Distance + Reference frame + Robot base frame + + + Distance to check if passed + reference frame to check in + Robot frame to check relative to global_frame + + + Distance to travel + Speed at which to travel + Allowed time for reversing + Server name + Server timeout + Disable collision checking + "Drive on heading error code" + + + + Path to follow + Goal checker + Progress checker + Service name + Server timeout + Follow Path error code + + + Vector of navigation goals + Navigation goal + + + + Name of the topic to receive goal checker selection commands + Default goal checker of the controller selector + Name of the selected goal checker received from the topic subcription + + + Destination + Reference frame + Robot base frame + + + Vector of navigation goals + Navigation goal + + + Vector of navigation goals + Navigation goal + + + Original goal in + Output goal set by subscription + + + SUCCESS if initial_pose_received true + + + Topic for battery info + + + Min battery % or voltage before triggering + Topic for battery info + Bool if check based on voltage or total % + + + Path to validate + Server timeout + + + Velocity threshold below which robot is considered stopped + Duration (ms) the velocity must remain below the threshold + + + + Goals + Server name + Server timeout + Behavior tree to run + Navigate through poses error code + + + Goal + Server name + Server timeout + Behavior tree to run + Navigate to pose error code + + + Time to check if expired + Check if path has been updated to enable timer reset + + + Planned Path + Proximity length (m) for the path to be longer on approach + Length multiplication factor to check if the path is significantly longer + + + + Name of the topic to receive planner selection commands + Default planner of the planner selector + Name of the selected planner received from the topic subcription + + + Name of the topic to receive progress checker selection commands + Default progress checker of the controller selector + Name of the selected progress checker received from the topic subcription + + + Rate + + + Number of retries + + + Service name + Server timeout + + + Costmap service name responsible for getting the cost + A vector of goals to check if in collision + Whether to use the footprint cost or the point cost. + The cost threshold above which a waypoint is considered in collision and should be removed. + A vector of goals containing only those that are not in collision. + + + Input goals to remove if passed + Radius tolerance on a goal to consider it passed + Global frame + Robot base frame + Set of goals after removing any passed + + + + + + Path to be smoothed + Maximum smoothing duration + Bool if collision check should be performed + Smoothed path + Smoothing duration + True if smoothing was not interrupted by time limit + + + Name of the topic to receive smoother selection commands + Default smoother of the smoother selector + Name of the selected smoother received from the topic subcription + + + Minimum rate + Maximum rate + Minimum speed + Maximum speed + Vector of navigation goals + Navigation goal + + + Spin distance + Allowed time for spinning + Server name + Server timeout + Disable collision checking + Spin error code + + + Time to check if expired + + + Child frame for transform + Parent frame for transform + + + Distance before goal to truncate + Path to truncate + Truncated path to utilize + + + Distance in forward direction + Distance in backward direction + Robot base frame id + Transform lookup tolerance + Manually specified pose to be used if overriding current robot pose + Weight of angular distance relative to positional distance when finding which path pose is closest to robot. Not applicable on paths without orientations assigned + 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 + Truncated path to utilize + + + Wait time + Server name + Server timeout + + + Error code + + + Error code + + + Error code + + + diff --git a/config/behavior_trees/main_tree.xml b/config/behavior_trees/main_tree.xml new file mode 100644 index 00000000..a22545e5 --- /dev/null +++ b/config/behavior_trees/main_tree.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + +