From 1ed2776f0e54e0a7539f35b58426c5451fb0c4a9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 29 Feb 2024 21:58:40 +0100 Subject: [PATCH] behavior_tree: migration to BT.CPP 4.5.x (#4128) * behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti * fix formatting Signed-off-by: Davide Faconti * fix lint warning Signed-off-by: Davide Faconti * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti * handle SKIPPED state Signed-off-by: Davide Faconti * fix SequenceStar Signed-off-by: Davide Faconti * add tests Signed-off-by: Davide Faconti * fix Signed-off-by: Davide Faconti * fix unit tests Signed-off-by: Davide Faconti * add emitWakeUpSignal() Signed-off-by: Davide Faconti * fit test Signed-off-by: Davide Faconti * fix Signed-off-by: Davide Faconti * remove SequenceStar Signed-off-by: Davide Faconti * remove SequenceStar Signed-off-by: Davide Faconti * keep resetStatus() in action_node Signed-off-by: Davide Faconti * add suggestions from peer reviewer Signed-off-by: Davide Faconti * fix compilation Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti --- nav2_behavior_tree/CMakeLists.txt | 4 +- nav2_behavior_tree/README.md | 2 +- .../behavior_tree_engine.hpp | 6 +- .../nav2_behavior_tree/bt_action_node.hpp | 14 +++-- .../bt_action_server_impl.hpp | 3 +- .../bt_cancel_action_node.hpp | 2 +- .../nav2_behavior_tree/bt_service_node.hpp | 8 +-- .../include/nav2_behavior_tree/bt_utils.hpp | 2 +- .../action/controller_selector_node.hpp | 2 +- .../action/goal_checker_selector_node.hpp | 2 +- .../plugins/action/planner_selector_node.hpp | 2 +- .../action/progress_checker_selector_node.hpp | 2 +- .../action/remove_passed_goals_action.hpp | 2 +- .../plugins/action/smoother_selector_node.hpp | 2 +- .../plugins/action/truncate_path_action.hpp | 2 +- .../action/truncate_path_local_action.hpp | 2 +- .../are_error_codes_present_condition.hpp | 2 +- .../condition/distance_traveled_condition.hpp | 2 +- .../globally_updated_goal_condition.hpp | 2 +- .../condition/goal_reached_condition.hpp | 2 +- .../condition/goal_updated_condition.hpp | 2 +- .../initial_pose_received_condition.hpp | 2 +- .../is_battery_charging_condition.hpp | 2 +- .../condition/is_battery_low_condition.hpp | 2 +- .../condition/is_path_valid_condition.hpp | 2 +- .../plugins/condition/is_stuck_condition.hpp | 2 +- .../path_expiring_timer_condition.hpp | 2 +- .../condition/time_expired_condition.hpp | 2 +- .../transform_available_condition.hpp | 2 +- .../plugins/control/pipeline_sequence.hpp | 4 +- .../plugins/control/recovery_node.hpp | 2 +- .../plugins/control/round_robin_node.hpp | 4 +- .../plugins/decorator/distance_controller.hpp | 2 +- .../decorator/goal_updated_controller.hpp | 2 +- .../plugins/decorator/goal_updater_node.hpp | 2 +- .../decorator/path_longer_on_approach.hpp | 2 +- .../plugins/decorator/rate_controller.hpp | 2 +- .../plugins/decorator/single_trigger_node.hpp | 2 +- .../plugins/decorator/speed_controller.hpp | 2 +- .../nav2_behavior_tree/ros_topic_logger.hpp | 2 +- nav2_behavior_tree/nav2_tree_nodes.xml | 2 +- nav2_behavior_tree/package.xml | 4 +- .../plugins/action/assisted_teleop_action.cpp | 2 +- .../action/assisted_teleop_cancel_node.cpp | 2 +- .../plugins/action/back_up_action.cpp | 2 +- .../plugins/action/back_up_cancel_node.cpp | 2 +- .../plugins/action/clear_costmap_service.cpp | 2 +- .../compute_path_through_poses_action.cpp | 2 +- .../action/compute_path_to_pose_action.cpp | 2 +- .../plugins/action/controller_cancel_node.cpp | 2 +- .../action/controller_selector_node.cpp | 2 +- .../action/drive_on_heading_action.cpp | 2 +- .../action/drive_on_heading_cancel_node.cpp | 2 +- .../plugins/action/follow_path_action.cpp | 2 +- .../action/goal_checker_selector_node.cpp | 2 +- .../action/navigate_through_poses_action.cpp | 2 +- .../action/navigate_to_pose_action.cpp | 2 +- .../plugins/action/planner_selector_node.cpp | 2 +- .../action/progress_checker_selector_node.cpp | 2 +- ...initialize_global_localization_service.cpp | 2 +- .../action/remove_passed_goals_action.cpp | 2 +- .../plugins/action/smooth_path_action.cpp | 2 +- .../plugins/action/smoother_selector_node.cpp | 2 +- .../plugins/action/spin_action.cpp | 2 +- .../plugins/action/spin_cancel_node.cpp | 2 +- .../plugins/action/truncate_path_action.cpp | 4 +- .../action/truncate_path_local_action.cpp | 4 +- .../plugins/action/wait_action.cpp | 2 +- .../plugins/action/wait_cancel_node.cpp | 2 +- .../are_error_codes_present_condition.cpp | 2 +- .../condition/distance_traveled_condition.cpp | 4 +- .../globally_updated_goal_condition.cpp | 2 +- .../condition/goal_reached_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 4 +- .../initial_pose_received_condition.cpp | 2 +- .../is_battery_charging_condition.cpp | 2 +- .../condition/is_battery_low_condition.cpp | 2 +- .../condition/is_path_valid_condition.cpp | 2 +- .../plugins/condition/is_stuck_condition.cpp | 2 +- .../path_expiring_timer_condition.cpp | 4 +- .../condition/time_expired_condition.cpp | 6 +- .../transform_available_condition.cpp | 2 +- ...d_a_controller_recovery_help_condition.cpp | 2 +- ...ould_a_planner_recovery_help_condition.cpp | 2 +- ...uld_a_smoother_recovery_help_condition.cpp | 2 +- .../plugins/control/pipeline_sequence.cpp | 9 +++ .../plugins/control/recovery_node.cpp | 59 ++++++++++--------- .../plugins/control/round_robin_node.cpp | 32 +++++----- .../plugins/decorator/distance_controller.cpp | 9 +-- .../decorator/goal_updated_controller.cpp | 20 ++----- .../plugins/decorator/goal_updater_node.cpp | 4 +- .../decorator/path_longer_on_approach.cpp | 12 ++-- .../plugins/decorator/rate_controller.cpp | 9 +-- .../plugins/decorator/single_trigger_node.cpp | 14 ++--- .../plugins/decorator/speed_controller.cpp | 18 +----- .../src/behavior_tree_engine.cpp | 9 ++- .../action/test_assisted_teleop_action.cpp | 10 ++-- .../test_assisted_teleop_cancel_node.cpp | 4 +- .../plugins/action/test_back_up_action.cpp | 10 ++-- .../action/test_back_up_cancel_node.cpp | 4 +- .../plugins/action/test_bt_action_node.cpp | 26 ++++---- .../action/test_clear_costmap_service.cpp | 8 +-- ...test_compute_path_through_poses_action.cpp | 6 +- .../test_compute_path_to_pose_action.cpp | 6 +- .../action/test_controller_cancel_node.cpp | 4 +- .../action/test_controller_selector_node.cpp | 6 +- .../action/test_drive_on_heading_action.cpp | 10 ++-- .../test_drive_on_heading_cancel_node.cpp | 4 +- .../action/test_follow_path_action.cpp | 4 +- .../test_goal_checker_selector_node.cpp | 6 +- .../test_navigate_through_poses_action.cpp | 4 +- .../action/test_navigate_to_pose_action.cpp | 4 +- .../action/test_planner_selector_node.cpp | 6 +- .../test_progress_checker_selector_node.cpp | 18 +++--- ...initialize_global_localization_service.cpp | 4 +- .../test_remove_passed_goals_action.cpp | 4 +- .../action/test_smooth_path_action.cpp | 4 +- .../action/test_smoother_selector_node.cpp | 6 +- .../test/plugins/action/test_spin_action.cpp | 10 ++-- .../plugins/action/test_spin_cancel_node.cpp | 4 +- .../action/test_truncate_path_action.cpp | 4 +- .../test_truncate_path_local_action.cpp | 12 ++-- .../test/plugins/action/test_wait_action.cpp | 8 +-- .../plugins/action/test_wait_cancel_node.cpp | 4 +- .../test_are_error_codes_present.cpp | 4 +- .../plugins/condition/test_goal_reached.cpp | 12 ++-- .../condition/test_is_battery_charging.cpp | 12 ++-- .../plugins/condition/test_is_battery_low.cpp | 20 +++---- .../plugins/condition/test_is_path_valid.cpp | 2 +- .../condition/test_transform_available.cpp | 6 +- .../test_would_a_controller_recovery_help.cpp | 4 +- .../test_would_a_planner_recovery_help.cpp | 4 +- .../test_would_a_smoother_recovery_help.cpp | 4 +- .../control/test_pipeline_sequence.cpp | 27 +++++++++ .../plugins/control/test_recovery_node.cpp | 20 +++++++ .../plugins/control/test_round_robin_node.cpp | 27 +++++++++ .../decorator/test_goal_updater_node.cpp | 8 +-- .../test_path_longer_on_approach.cpp | 4 +- nav2_behavior_tree/test/test_bt_utils.cpp | 28 ++++----- .../test/utils/test_behavior_tree_fixture.hpp | 2 +- .../test/utils/test_dummy_tree_node.hpp | 10 +++- nav2_bt_navigator/CMakeLists.txt | 4 +- .../behavior_trees/follow_point.xml | 2 +- ...replanning_and_if_path_becomes_invalid.xml | 2 +- ...hrough_poses_w_replanning_and_recovery.xml | 2 +- ...gate_to_pose_w_replanning_and_recovery.xml | 2 +- ..._replanning_goal_patience_and_recovery.xml | 6 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_distance.xml | 2 +- ...e_w_replanning_only_if_goal_is_updated.xml | 2 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_speed.xml | 2 +- .../navigate_w_replanning_time.xml | 2 +- .../behavior_trees/odometry_calibration.xml | 2 +- nav2_bt_navigator/package.xml | 4 +- .../src/navigators/navigate_through_poses.cpp | 6 +- .../src/navigators/navigate_to_pose.cpp | 4 +- nav2_system_tests/CMakeLists.txt | 4 +- .../behavior_tree/test_behavior_tree_node.cpp | 18 +++--- tools/bt2img.py | 2 +- 160 files changed, 480 insertions(+), 394 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index fad9886c77e..91fc49b71d0 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -34,7 +34,7 @@ set(dependencies sensor_msgs nav2_msgs nav_msgs - behaviortree_cpp_v3 + behaviortree_cpp tf2 tf2_ros tf2_geometry_msgs diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 84e16e7d4e9..0ade4e0a016 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -41,7 +41,7 @@ BehaviorTreeEngine::BehaviorTreeEngine() Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the FollowPath node in use: ```XML - + diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 9a5b03ca673..9ee903fd78e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -20,9 +20,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/xml_parsing.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/xml_parsing.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 39c5c5f7d95..6193a91a868 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -188,7 +188,7 @@ class BtActionNode : public BT::ActionNodeBase BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // setting the status to RUNNING to notify the BT Loggers (if any) setStatus(BT::NodeStatus::RUNNING); @@ -325,7 +325,9 @@ class BtActionNode : public BT::ActionNodeBase on_cancelled(); } - setStatus(BT::NodeStatus::IDLE); + // this is probably redundant, since the parent node + // is supposed to call it, but we keep it, just in case + resetStatus(); } protected: @@ -376,12 +378,14 @@ class BtActionNode : public BT::ActionNodeBase if (this->goal_handle_->get_goal_id() == result.goal_id) { goal_result_available_ = true; result_ = result; + emitWakeUpSignal(); } }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) { feedback_ = feedback; + emitWakeUpSignal(); }; future_goal_handle_ = std::make_shared< @@ -434,9 +438,9 @@ class BtActionNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->template get("number_recoveries", recovery_count); // NOLINT + [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->template set("number_recoveries", recovery_count); // NOLINT + config().blackboard->set("number_recoveries", recovery_count); // NOLINT } std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 0c636536b34..5284b132a04 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -237,7 +237,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Create the Behavior Tree from the XML input try { tree_ = bt_->createTreeFromFile(filename, blackboard_); - for (auto & blackboard : tree_.blackboard_stack) { + for (auto & subtree : tree_.subtrees) { + auto & blackboard = subtree->blackboard; blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index ea3e41d859d..789e30133dd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 47fd52366b1..546cbf13420 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -157,7 +157,7 @@ class BtServiceNode : public BT::ActionNodeBase void halt() override { request_sent_ = false; - setStatus(BT::NodeStatus::IDLE); + resetStatus(); } /** @@ -230,9 +230,9 @@ class BtServiceNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->template get("number_recoveries", recovery_count); // NOLINT + [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->template set("number_recoveries", recovery_count); // NOLINT + config().blackboard->set("number_recoveries", recovery_count); // NOLINT } std::string service_name_, service_node_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index a3b42011f0a..7075bc63f48 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -20,7 +20,7 @@ #include "rclcpp/time.hpp" #include "rclcpp/node.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp index 968750a05e9..625f7e9483b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp index 11f0d0f423d..90a10f1675c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp index 3aa9e6573e1..4a1dd779a39 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp index e996bda55bf..c9c61151083 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -20,7 +20,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index d9b3d4a59cf..344afd546d8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp index f9bb293d264..da5a7274445 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index 2604bcfea13..5501d49cf32 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 113e0d83e72..94b4b66ca2a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -22,7 +22,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp index 42721f62668..209958c38d4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp @@ -21,7 +21,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 3c87c7370c7..67747c62b88 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 7fe4a654044..7e3e92e799d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index acbca8c960f..b79fabe2f9e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -19,7 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index d7191c9749d..89d4b7a573c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index ee5bb025c9a..548c15268b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -16,7 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ #include -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp index 37a53b41e12..704154a31d0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index ff479105807..59a023ff3c2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 36890f2a8a0..5a9f255a9b7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -19,7 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp index 565f261100f..e532822d42a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/odometry.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index fb2f42f3d4d..8871892949a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -18,7 +18,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/path.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 356a79857db..26a3431b5db 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -19,7 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 5fdefa9b8bf..511c381321b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp index 0384381d2a9..ce3406904f5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp @@ -16,8 +16,8 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_ #include -#include "behaviortree_cpp_v3/control_node.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp index 89c0cfa300c..62759ea7113 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp @@ -16,7 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_ #include -#include "behaviortree_cpp_v3/control_node.h" +#include "behaviortree_cpp/control_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp index 8c374ce6af0..86f2b38c5bb 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/control_node.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index f85a3975d06..7fbda19c63e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 9c59c877f67..bdd4171185c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 49dfbc1a0c5..ddce12cf02e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp index 6e41516434c..bdf4a080d18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index 24a0207e3bd..c390357b342 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp index c16ef63efa9..02c0c1812ef 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index caf30982a60..ed454c0aa1d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -24,7 +24,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "nav2_util/odometry_utils.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp index 0e14a59ee87..d1ccef107e2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/loggers/abstract_logger.h" +#include "behaviortree_cpp/loggers/abstract_logger.h" #include "rclcpp/rclcpp.hpp" #include "nav2_msgs/msg/behavior_tree_log.hpp" #include "nav2_msgs/msg/behavior_tree_status_change.h" diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 51d0166f9b9..925e9c35797 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -4,7 +4,7 @@ please refer to the groot_instructions.md and REAMDE.md respectively located in the nav2_behavior_tree package. --> - + diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 797d53270b7..b9ed847ce38 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -17,7 +17,7 @@ rclcpp rclcpp_action rclcpp_lifecycle - behaviortree_cpp_v3 + behaviortree_cpp builtin_interfaces geometry_msgs sensor_msgs @@ -36,7 +36,7 @@ rclcpp_action rclcpp_lifecycle std_msgs - behaviortree_cpp_v3 + behaviortree_cpp builtin_interfaces geometry_msgs sensor_msgs diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 64eb61d788e..e876d2ce40b 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -70,7 +70,7 @@ BT::NodeStatus AssistedTeleopAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp index e226ba19753..362499c9f2e 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp @@ -32,7 +32,7 @@ AssistedTeleopCancel::AssistedTeleopCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 09d79c7e230..e17580fe71c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -76,7 +76,7 @@ BT::NodeStatus BackUpAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp index 3baa1ffb1e3..21835c326a8 100644 --- a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp @@ -32,7 +32,7 @@ BackUpCancel::BackUpCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 08d2b00632b..2d007a6623e 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -60,7 +60,7 @@ void ClearCostmapAroundRobotService::on_tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ClearEntireCostmap"); diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 8bfffed8434..a0572ecad61 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -65,7 +65,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index c4b93afa854..c7f1a0164e8 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -71,7 +71,7 @@ void ComputePathToPoseAction::halt() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp index 966ee01e10a..b2eb4358c20 100644 --- a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp @@ -32,7 +32,7 @@ ControllerCancel::ControllerCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index 7d77adbee39..b58d78bb78f 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -84,7 +84,7 @@ ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::Shared } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ControllerSelector"); diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index 7e0b613f62f..03c00344141 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -74,7 +74,7 @@ BT::NodeStatus DriveOnHeadingAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp index b29de63df55..8ac530f8df0 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp @@ -32,7 +32,7 @@ DriveOnHeadingCancel::DriveOnHeadingCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 00acf5c3495..b662221de0f 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -96,7 +96,7 @@ void FollowPathAction::on_wait_for_result( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp index 1a2e9c703b0..3ee9d832609 100644 --- a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp @@ -75,7 +75,7 @@ GoalCheckerSelector::callbackGoalCheckerSelect(const std_msgs::msg::String::Shar } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalCheckerSelector"); diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 57db8942481..9213cd564f4 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -61,7 +61,7 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 8022697e0e8..07bea22436f 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -59,7 +59,7 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index ccb85184127..5d0610c0fa5 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -84,7 +84,7 @@ PlannerSelector::callbackPlannerSelect(const std_msgs::msg::String::SharedPtr ms } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PlannerSelector"); diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp index e8c418ddc17..fea2194158b 100644 --- a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -74,7 +74,7 @@ ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::Stri } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ProgressCheckerSelector"); diff --git a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp index 388f16ccb25..30b876d5346 100644 --- a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp @@ -26,7 +26,7 @@ ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index fd255f78c54..86f5fffd6b2 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -89,7 +89,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RemovePassedGoals"); diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index da547b15876..3a67904558b 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -64,7 +64,7 @@ BT::NodeStatus SmoothPathAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp index 0a84062e088..c717332c79b 100644 --- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -85,7 +85,7 @@ SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SmootherSelector"); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index cab9c8f8211..d10bb83f32b 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -72,7 +72,7 @@ BT::NodeStatus SpinAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp index 62ddbc4719c..1d75c5cf275 100644 --- a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp @@ -32,7 +32,7 @@ SpinCancel::SpinCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index b394a804c73..7bebfbfba38 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -20,7 +20,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" @@ -82,7 +82,7 @@ inline BT::NodeStatus TruncatePath::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TruncatePath"); diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index 934f2b86bb8..35dc8635c8c 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -150,7 +150,7 @@ TruncatePathLocal::poseDistance( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( "TruncatePathLocal"); diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b9317076737..b607d026e59 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -56,7 +56,7 @@ void WaitAction::on_tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp index 9365b7e06fa..b45db33396f 100644 --- a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp @@ -32,7 +32,7 @@ WaitCancel::WaitCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp b/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp index 6764cccd116..f39dd8fbce4 100644 --- a/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp @@ -14,7 +14,7 @@ #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 167caec938c..7db1817c65e 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -56,7 +56,7 @@ BT::NodeStatus DistanceTraveledCondition::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { if (!nav2_util::getCurrentPose( start_pose_, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) @@ -92,7 +92,7 @@ BT::NodeStatus DistanceTraveledCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("DistanceTraveled"); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index 61c45696f57..dbd84d8b2ef 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -55,7 +55,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GlobalUpdatedGoal"); diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 8609ab55902..70243562034 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -90,7 +90,7 @@ bool GoalReachedCondition::isGoalReached() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalReached"); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index f2f112c348e..88d329efc2a 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -28,7 +28,7 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { BT::getInputOrBlackboard("goals", goals_); BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::FAILURE; @@ -50,7 +50,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdated"); diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp index 0a74ce68c95..9d930229124 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus InitialPoseReceived::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("InitialPoseReceived"); diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp index c9c05f68753..27e1bd55fc7 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp @@ -59,7 +59,7 @@ void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState: } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsBatteryCharging"); diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index 9d221cf8c81..a0e3761f28c 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -77,7 +77,7 @@ void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::Shar } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsBatteryLow"); diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 59d593a1475..7274743e1e9 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -64,7 +64,7 @@ BT::NodeStatus IsPathValidCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsPathValid"); diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index a898dec9473..d0ca48cd7f5 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -143,7 +143,7 @@ bool IsStuckCondition::isStuck() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsStuck"); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index 1347998602f..540af1d83d3 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -15,7 +15,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" @@ -68,7 +68,7 @@ BT::NodeStatus PathExpiringTimerCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PathExpiringTimer"); diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index 64afd1a34be..f03c9711aa8 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -16,7 +16,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp" @@ -46,7 +46,7 @@ BT::NodeStatus TimeExpiredCondition::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { start_ = node_->now(); return BT::NodeStatus::FAILURE; } @@ -67,7 +67,7 @@ BT::NodeStatus TimeExpiredCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TimeExpired"); diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 33e94178deb..0ee491fbfd2 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -83,7 +83,7 @@ BT::NodeStatus TransformAvailableCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TransformAvailable"); diff --git a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp index f8c62064f6c..87625d4511f 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp @@ -33,7 +33,7 @@ WouldAControllerRecoveryHelp::WouldAControllerRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 91de9c2e22a..603eb60f107 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -35,7 +35,7 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp index fb903e1cbfc..50665256c25 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp @@ -33,7 +33,7 @@ WouldASmootherRecoveryHelp::WouldASmootherRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 640c3ed7b4c..bc2327af284 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -35,6 +35,7 @@ PipelineSequence::PipelineSequence( BT::NodeStatus PipelineSequence::tick() { + unsigned skipped_count = 0; for (std::size_t i = 0; i < children_nodes_.size(); ++i) { auto status = children_nodes_[i]->executeTick(); switch (status) { @@ -42,6 +43,10 @@ BT::NodeStatus PipelineSequence::tick() ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return status; + case BT::NodeStatus::SKIPPED: + skipped_count++; + // do nothing and continue on to the next child. + break; case BT::NodeStatus::SUCCESS: // do nothing and continue on to the next child. If it is the last child // we'll exit the loop and hit the wrap-up code at the end of the method. @@ -63,6 +68,10 @@ BT::NodeStatus PipelineSequence::tick() // Wrap up. ControlNode::haltChildren(); last_child_ticked_ = 0; // reset + if (skipped_count == children_nodes_.size()) { + // All the children were skipped + return BT::NodeStatus::SKIPPED; + } return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 44e56dc55ec..6eb3c6e99e5 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -45,12 +45,18 @@ BT::NodeStatus RecoveryNode::tick() if (current_child_idx_ == 0) { switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + case BT::NodeStatus::SUCCESS: - { - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - } + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; case BT::NodeStatus::FAILURE: { @@ -66,44 +72,41 @@ BT::NodeStatus RecoveryNode::tick() } } - case BT::NodeStatus::RUNNING: - { - return BT::NodeStatus::RUNNING; - } - default: - { - throw BT::LogicError("A child node must never return IDLE"); - } + throw BT::LogicError("A child node must never return IDLE"); } // end switch } else if (current_child_idx_ == 1) { switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); + return BT::NodeStatus::FAILURE; + } + case BT::NodeStatus::RUNNING: + return child_status; + case BT::NodeStatus::SUCCESS: { // halt second child, increment recovery count, and tick first child in next iteration ControlNode::haltChild(1); retry_count_++; - current_child_idx_--; + current_child_idx_ = 0; } break; case BT::NodeStatus::FAILURE: - { - // reset node and return failure if second child fails - halt(); - return BT::NodeStatus::FAILURE; - } - - case BT::NodeStatus::RUNNING: - { - return BT::NodeStatus::RUNNING; - } + // reset node and return failure if second child fails + halt(); + return BT::NodeStatus::FAILURE; default: - { - throw BT::LogicError("A child node must never return IDLE"); - } + throw BT::LogicError("A child node must never return IDLE"); } // end switch } } // end while loop @@ -122,7 +125,7 @@ void RecoveryNode::halt() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RecoveryNode"); diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp index 135ea09b744..9293a5b1181 100644 --- a/nav2_behavior_tree/plugins/control/round_robin_node.cpp +++ b/nav2_behavior_tree/plugins/control/round_robin_node.cpp @@ -36,18 +36,22 @@ BT::NodeStatus RoundRobinNode::tick() const auto num_children = children_nodes_.size(); setStatus(BT::NodeStatus::RUNNING); + unsigned num_skipped_children = 0; - while (num_failed_children_ < num_children) { + while (num_failed_children_ + num_skipped_children < num_children) { TreeNode * child_node = children_nodes_[current_child_idx_]; const BT::NodeStatus child_status = child_node->executeTick(); + if (child_status != BT::NodeStatus::RUNNING) { + // Increment index and wrap around to the first child + if (++current_child_idx_ == num_children) { + current_child_idx_ = 0; + } + } + switch (child_status) { case BT::NodeStatus::SUCCESS: { - // Wrap around to the first child - if (++current_child_idx_ >= num_children) { - current_child_idx_ = 0; - } num_failed_children_ = 0; ControlNode::haltChildren(); return BT::NodeStatus::SUCCESS; @@ -55,27 +59,27 @@ BT::NodeStatus RoundRobinNode::tick() case BT::NodeStatus::FAILURE: { - if (++current_child_idx_ >= num_children) { - current_child_idx_ = 0; - } num_failed_children_++; break; } - case BT::NodeStatus::RUNNING: + case BT::NodeStatus::SKIPPED: { - return BT::NodeStatus::RUNNING; + num_skipped_children++; + break; } + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; default: - { - throw BT::LogicError("Invalid status return from BT node"); - } + throw BT::LogicError("Invalid status return from BT node"); } } + const bool all_skipped = (num_skipped_children == num_children); halt(); - return BT::NodeStatus::FAILURE; + // If all the children were skipped, this node is considered skipped + return all_skipped ? BT::NodeStatus::SKIPPED : BT::NodeStatus::FAILURE; } void RoundRobinNode::halt() diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index 3cef0ea978c..7f87695416d 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -51,7 +51,7 @@ DistanceController::DistanceController( inline BT::NodeStatus DistanceController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting position since we're starting a new iteration of // the distance controller (moving from IDLE to RUNNING) if (!nav2_util::getCurrentPose( @@ -90,8 +90,9 @@ inline BT::NodeStatus DistanceController::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + return child_state; case BT::NodeStatus::SUCCESS: if (!nav2_util::getCurrentPose( @@ -114,7 +115,7 @@ inline BT::NodeStatus DistanceController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("DistanceController"); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index 049bc61f7ec..d0de9205452 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -33,7 +33,7 @@ GoalUpdatedController::GoalUpdatedController( BT::NodeStatus GoalUpdatedController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset since we're starting a new iteration of // the goal updated controller (moving from IDLE to RUNNING) @@ -61,19 +61,7 @@ BT::NodeStatus GoalUpdatedController::tick() // 'til completion if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) { goal_was_updated_ = false; - const BT::NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) { - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::FAILURE: - default: - return BT::NodeStatus::FAILURE; - } + return child_node_->executeTick(); } return status(); @@ -81,7 +69,7 @@ BT::NodeStatus GoalUpdatedController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdatedController"); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 1f7226e6aac..fa5badf750d 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -17,7 +17,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -84,7 +84,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdater"); diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964e..cb39a245579 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -62,7 +62,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() getInput("prox_len", prox_len_); getInput("length_factor", length_factor_); - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting point since we're starting a new iteration of // PathLongerOnApproach (moving from IDLE to RUNNING) first_time_ = true; @@ -77,14 +77,14 @@ inline BT::NodeStatus PathLongerOnApproach::tick() { const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + return child_state; case BT::NodeStatus::SUCCESS: - old_path_ = new_path_; - return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: old_path_ = new_path_; - return BT::NodeStatus::FAILURE; + resetChild(); + return child_state; default: old_path_ = new_path_; return BT::NodeStatus::FAILURE; @@ -97,7 +97,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PathLongerOnApproach"); diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index b710ec30091..9592d119c96 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -43,7 +43,7 @@ BT::NodeStatus RateController::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting point since we're starting a new iteration of // the rate controller (moving from IDLE to RUNNING) start_ = std::chrono::high_resolution_clock::now(); @@ -70,14 +70,15 @@ BT::NodeStatus RateController::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + case BT::NodeStatus::FAILURE: + return child_state; case BT::NodeStatus::SUCCESS: start_ = std::chrono::high_resolution_clock::now(); // Reset the timer return BT::NodeStatus::SUCCESS; - case BT::NodeStatus::FAILURE: default: return BT::NodeStatus::FAILURE; } @@ -88,7 +89,7 @@ BT::NodeStatus RateController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RateController"); diff --git a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp index 84f0fadfbbc..c95d646438a 100644 --- a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp @@ -30,7 +30,7 @@ SingleTrigger::SingleTrigger( BT::NodeStatus SingleTrigger::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { first_time_ = true; } @@ -40,16 +40,14 @@ BT::NodeStatus SingleTrigger::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - first_time_ = false; - return BT::NodeStatus::SUCCESS; + return child_state; case BT::NodeStatus::FAILURE: + case BT::NodeStatus::SUCCESS: first_time_ = false; - return BT::NodeStatus::FAILURE; + return child_state; default: first_time_ = false; @@ -62,7 +60,7 @@ BT::NodeStatus SingleTrigger::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SingleTrigger"); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 8d160aa76e1..b8e5b3915a3 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -59,7 +59,7 @@ SpeedController::SpeedController( inline BT::NodeStatus SpeedController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset since we're starting a new iteration of // the speed controller (moving from IDLE to RUNNING) BT::getInputOrBlackboard("goals", goals_); @@ -101,19 +101,7 @@ inline BT::NodeStatus SpeedController::tick() start_ = node_->now(); } - const BT::NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) { - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::FAILURE: - default: - return BT::NodeStatus::FAILURE; - } + return child_node_->executeTick(); } return status(); @@ -121,7 +109,7 @@ inline BT::NodeStatus SpeedController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SpeedController"); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 33100d76bb3..8ed1cd4e71b 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/utils/shared_library.h" namespace nav2_behavior_tree { @@ -32,6 +32,11 @@ BehaviorTreeEngine::BehaviorTreeEngine( for (const auto & p : plugin_libraries) { factory_.registerFromPlugin(loader.getOSName(p)); } + + // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x + // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+ + BT::ReactiveSequence::EnableException(false); + BT::ReactiveFallback::EnableException(false); } BtStatus @@ -52,7 +57,7 @@ BehaviorTreeEngine::run( return BtStatus::CANCELED; } - result = tree->tickRoot(); + result = tree->tickOnce(); onLoop(); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index cbb5606d8bd..c5d40f0f4e6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" @@ -123,7 +123,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -134,7 +134,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_ports) xml_txt = R"( - + @@ -148,7 +148,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -172,7 +172,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a9261fe4b8d..a516d868e4f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" @@ -122,7 +122,7 @@ TEST_F(CancelAssistedTeleopActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 9df8b3da154..846e7e86db9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/back_up_action.hpp" @@ -122,7 +122,7 @@ TEST_F(BackUpActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -134,7 +134,7 @@ TEST_F(BackUpActionTestFixture, test_ports) xml_txt = R"( - + @@ -149,7 +149,7 @@ TEST_F(BackUpActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -174,7 +174,7 @@ TEST_F(BackUpActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index c7f379a64c2..a3ea28791c6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelBackUpActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index ec3aa03a079..8aa1c3366eb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_action_node.hpp" #include "test_msgs/action/fibonacci.hpp" @@ -238,7 +238,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // create tree std::string xml_txt = R"( - + @@ -264,7 +264,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -307,7 +307,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -316,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // the BT should have failed EXPECT_EQ(result, BT::NodeStatus::FAILURE); - // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 - EXPECT_EQ(ticks, 2); + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should + // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks. + EXPECT_LE(ticks, 2); + EXPECT_GE(ticks, 1); } TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) @@ -325,7 +327,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // create tree std::string xml_txt = R"( - + @@ -352,7 +354,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -386,7 +388,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -401,7 +403,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // create tree std::string xml_txt = R"( - + @@ -429,7 +431,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -461,7 +463,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index e21f9802d7e..76f3b1d025a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_service.hpp" #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" @@ -100,7 +100,7 @@ TEST_F(ClearEntireCostmapServiceTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -195,7 +195,7 @@ TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -290,7 +290,7 @@ TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 20c4461acf5..24d10b63d63 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -22,7 +22,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp" @@ -128,7 +128,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -187,7 +187,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index 6065f4543d2..29ebaef9362 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp" @@ -125,7 +125,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -184,7 +184,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 7be55c08ae0..564e72d3d54 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelControllerActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 3218877c847..c7f04ae9a95 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -128,7 +128,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index c4439f1e1cf..baa5ea02a2c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp" @@ -118,7 +118,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -131,7 +131,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) xml_txt = R"( - + @@ -146,7 +146,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -169,7 +169,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index 762351bdd1b..c94ed1d89f8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp" @@ -123,7 +123,7 @@ TEST_F(CancelDriveOnHeadingTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 47766d8e9d9..4f272208198 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/follow_path_action.hpp" @@ -118,7 +118,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index 747686000ed..19089fb6f3e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -128,7 +128,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 73de1ac53f5..a805a721b9f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp" @@ -124,7 +124,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index aef672b940c..4ed5120b986 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp" @@ -119,7 +119,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index efc59adb6c8..ade80c57daf 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -78,7 +78,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -126,7 +126,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp index 574a514886b..c97bbc19699 100644 --- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -19,7 +19,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -79,7 +79,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -94,7 +94,9 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // check default value std::string selected_progress_checker_result; - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + [[maybe_unused]] auto res = config_->blackboard->get( + "selected_progress_checker", + selected_progress_checker_result); EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); @@ -119,7 +121,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) } // check progress_checker updated - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); } @@ -128,7 +130,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +145,9 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // check default value std::string selected_progress_checker_result; - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + [[maybe_unused]] auto res = config_->blackboard->get( + "selected_progress_checker", + selected_progress_checker_result); EXPECT_EQ(selected_progress_checker_result, "GridBased"); @@ -167,7 +171,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) } // check goal_checker updated - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); EXPECT_EQ("RRT", selected_progress_checker_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 73e72fa764d..04b0e704903 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_service.hpp" #include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp" @@ -97,7 +97,7 @@ TEST_F(ReinitializeGlobalLocalizationServiceTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index a8ac368a160..ce9d0debf8e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" @@ -105,7 +105,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 917b696acf8..d60ed2ffb78 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp" @@ -118,7 +118,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index c3476615069..f93f6df878f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -128,7 +128,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index e1285d28a90..e3d2e80c85d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/spin_action.hpp" @@ -122,7 +122,7 @@ TEST_F(SpinActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -133,7 +133,7 @@ TEST_F(SpinActionTestFixture, test_ports) xml_txt = R"( - + @@ -147,7 +147,7 @@ TEST_F(SpinActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -169,7 +169,7 @@ TEST_F(SpinActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index 8cc3083eb3e..54a0270e786 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelSpinActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 3610cd41a9e..2e7ac3fccd2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" @@ -87,7 +87,7 @@ TEST_F(TruncatePathTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp index 0559d6effe3..c3333605f89 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" @@ -107,7 +107,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) // create tree std::string xml_txt = R"( - + + + + + #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/wait_action.hpp" @@ -113,7 +113,7 @@ TEST_F(WaitActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -124,7 +124,7 @@ TEST_F(WaitActionTestFixture, test_ports) xml_txt = R"( - + @@ -138,7 +138,7 @@ TEST_F(WaitActionTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index ebce2f63413..4b55e3d1335 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelWaitActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index d1716397df0..111cda7d005 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -34,7 +34,7 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF std::string xml_txt = R"( - + @@ -65,7 +65,7 @@ TEST_F(AreErrorCodesPresentFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp index 1580a8d1bc0..1034c4f9b6f 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp @@ -43,7 +43,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT std::string xml_txt = R"( - + @@ -66,32 +66,32 @@ std::shared_ptr GoalReachedConditionTestFixture::tree_ = nullptr; TEST_F(GoalReachedConditionTestFixture, test_behavior) { - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); geometry_msgs::msg::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); pose.position.x = 0.5; pose.position.y = 0.5; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); pose.position.x = 0.9; pose.position.y = 0.9; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); pose.position.x = 1.0; pose.position.y = 1.0; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index 79fa1e08c20..8cd761aadf1 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -73,7 +73,7 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) { std::string xml_txt = R"( - + @@ -86,32 +86,32 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index a39a4140890..5e763f56d42 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -74,7 +74,7 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) { std::string xml_txt = R"( - + @@ -87,32 +87,32 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.49; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.percentage = 0.51; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) { std::string xml_txt = R"( - + @@ -125,25 +125,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 4.9; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.voltage = 5.1; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp index 9c022359a11..df973cb280a 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp @@ -92,7 +92,7 @@ TEST_F(IsPathValidTestFixture, test_behavior) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 2b21fd416c4..73316dfa63f 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -65,7 +65,7 @@ class TransformAvailableConditionTestFixture : public ::testing::Test { std::string xml_txt = R"( - + @@ -99,10 +99,10 @@ std::shared_ptr TransformAvailableConditionTestFixture::tree_ = nullpt TEST_F(TransformAvailableConditionTestFixture, test_behavior) { - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); transform_handler_->activate(); transform_handler_->waitForTransform(); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp index d93249879d5..839a4003fe8 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT std::string xml_txt = R"( - + @@ -69,7 +69,7 @@ TEST_F(WouldAControllerRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp index 4dc0f8bb5cb..89e09265dc4 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree std::string xml_txt = R"( - + @@ -64,7 +64,7 @@ TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp index 337d5133c38..7a6dff9be97 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre std::string xml_txt = R"( - + @@ -66,7 +66,7 @@ TEST_F(WouldASmootherRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp index d50a6d4c51c..517ee1f04a8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp @@ -125,6 +125,33 @@ TEST_F(PipelineSequenceTestFixture, test_behavior) EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(PipelineSequenceTestFixture, test_skipped) +{ + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index cc5e2d29699..1ca41e798e8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -144,6 +144,26 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(RecoveryNodeTestFixture, test_skipping) +{ + // first child skipped + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + + // first child fails, second child skipped + first_child_->changeStatus(BT::NodeStatus::FAILURE); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); +} + + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp index 2996bca25a8..7daa91c0697 100644 --- a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp @@ -114,6 +114,33 @@ TEST_F(RoundRobinNodeTestFixture, test_behavior) EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(RoundRobinNodeTestFixture, test_skikpped) +{ + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 1adde82318b..fa56a2064ab 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -86,7 +86,7 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -115,7 +115,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) // create tree std::string xml_txt = R"( - + @@ -153,7 +153,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp index 0f08bafe691..5d521e205c1 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -89,7 +89,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -115,7 +115,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // create tree xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 238dee27dcd..1c008d6478a 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_utils.hpp" template @@ -51,7 +51,7 @@ TEST(PointPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -69,7 +69,7 @@ TEST(PointPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -86,7 +86,7 @@ TEST(PointPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -107,7 +107,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -126,7 +126,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -144,7 +144,7 @@ TEST(QuaternionPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -166,7 +166,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -190,7 +190,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -213,7 +213,7 @@ TEST(PoseStampedPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -241,7 +241,7 @@ TEST(MillisecondsPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -257,7 +257,7 @@ TEST(MillisecondsPortTest, test_correct_syntax) xml_txt = R"( - + @@ -272,7 +272,7 @@ TEST(ErrorCodePortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -295,7 +295,7 @@ TEST(deconflictPortAndParamFrameTest, test_correct_syntax) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp index 6215909ae9c..7bf08ddd440 100644 --- a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp @@ -20,7 +20,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" #include "test_transform_handler.hpp" diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp index d277e7f4d25..dcfa52927ac 100644 --- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp +++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp @@ -16,8 +16,8 @@ #ifndef UTILS__TEST_DUMMY_TREE_NODE_HPP_ #define UTILS__TEST_DUMMY_TREE_NODE_HPP_ -#include -#include +#include +#include namespace nav2_behavior_tree { @@ -36,7 +36,11 @@ class DummyNode : public BT::ActionNodeBase void changeStatus(BT::NodeStatus status) { - setStatus(status); + if (status == BT::NodeStatus::IDLE) { + resetStatus(); + } else { + setStatus(status); + } } BT::NodeStatus executeTick() override diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index cfe6a0d4869..7de1592106a 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -12,7 +12,7 @@ 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(behaviortree_cpp REQUIRED) find_package(std_srvs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_core REQUIRED) @@ -43,7 +43,7 @@ set(dependencies nav2_behavior_tree nav_msgs nav2_msgs - behaviortree_cpp_v3 + behaviortree_cpp std_srvs nav2_util nav2_core diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index 6941a8c7e60..9f55ecd66ec 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -2,7 +2,7 @@ This Behavior Tree follows a dynamic pose to a certain distance --> - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 8fa2eca237c..6973057e93a 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -3,7 +3,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 7a235ff0407..28eda8ed893 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -3,7 +3,7 @@ This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously and it also has recovery actions specific to planning / control as well as general system issues. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 8cb0eb749ca..772d36fded8 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -4,7 +4,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index ea2a7315909..a705aa6b3a2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -5,7 +5,7 @@ make the robot wait for a specific time, to see if the obstacle clears out before navigating along a significantly longer path to reach the goal location. --> - + @@ -20,10 +20,10 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 444c9458ff1..872dbf80187 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -4,7 +4,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index 61132d5f95d..16f7ede4b3e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path after every 1m. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 56e360933c8..a214c9b64fd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path only when the goal is updated. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index b9f903fd334..effe9a2bf82 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -1,7 +1,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index a48ae89a76a..e0d5d6a5a41 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path periodically proprortional to speed. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index f18111f5bd1..11332fb24e9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path periodically at 1 Hz. --> - + diff --git a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml index 49000e14fa6..fdc459f40ff 100644 --- a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -2,7 +2,7 @@ his Behavior Tree drives in a square for odometry calibration experiments --> - + diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 289f08d7ea6..d4eaa63c3c5 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -17,7 +17,7 @@ nav2_behavior_tree nav_msgs nav2_msgs - behaviortree_cpp_v3 + behaviortree_cpp std_msgs geometry_msgs std_srvs @@ -25,7 +25,7 @@ pluginlib nav2_core - behaviortree_cpp_v3 + behaviortree_cpp rclcpp rclcpp_action rclcpp_lifecycle diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 53769abfcd0..a4b4df52d1b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -103,7 +103,7 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); Goals goal_poses; - blackboard->get(goals_blackboard_id_, goal_poses); + [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); if (goal_poses.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); @@ -123,7 +123,7 @@ NavigateThroughPosesNavigator::onLoop() try { // Get current path points nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + res = blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = @@ -166,7 +166,7 @@ NavigateThroughPosesNavigator::onLoop() } int recovery_count = 0; - blackboard->get("number_recoveries", recovery_count); + res = blackboard->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 326a0bd9fb6..2270b6add56 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -126,7 +126,7 @@ NavigateToPoseNavigator::onLoop() try { // Get current path points nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = @@ -169,7 +169,7 @@ NavigateToPoseNavigator::onLoop() } int recovery_count = 0; - blackboard->get("number_recoveries", recovery_count); + [[maybe_unused]] auto res = blackboard->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 2674b65e742..bfab0a79fed 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -22,7 +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) +find_package(behaviortree_cpp REQUIRED) find_package(pluginlib REQUIRED) nav2_package() @@ -45,7 +45,7 @@ set(dependencies nav2_planner nav2_navfn_planner angles - behaviortree_cpp_v3 + behaviortree_cpp pluginlib ) diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index b3ac057172b..0a4b320d8f4 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -22,9 +22,9 @@ #include "gtest/gtest.h" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -224,7 +224,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -275,7 +275,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -324,7 +324,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -382,7 +382,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -476,7 +476,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -540,7 +540,7 @@ TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); // Update goal on blackboard after Spin has been triggered once // to simulate a goal update during a recovery action diff --git a/tools/bt2img.py b/tools/bt2img.py index b3c2b34eaf5..2b12d777d82 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -27,7 +27,7 @@ 'ReactiveFallback', 'ReactiveSequence', 'Sequence', - 'SequenceStar', + 'SequenceWithMemory', 'BlackboardCheckInt', 'BlackboardCheckDouble', 'BlackboardCheckString',