diff --git a/README.md b/README.md index 3fbb186d45..6df210791b 100644 --- a/README.md +++ b/README.md @@ -27,11 +27,13 @@ If you need professional services related to Nav2, please contact Open Navigatio Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.

- +

### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights. +### [Nvidia](https://www.nvidia.com/en-us/deep-learning-ai/industries/robotics/) develops GPU and AI technologies that power modern robotics, autonomous driving, data centers, gaming, and more. + ### [Polymath Robotics](https://www.polymathrobotics.com/) creates safety-critical navigation systems for industrial vehicles that are radically simple to enable and deploy. ### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more. diff --git a/doc/sponsors_feb_2024.png b/doc/sponsors_feb_2024.png new file mode 100644 index 0000000000..50dfbc47a4 Binary files /dev/null and b/doc/sponsors_feb_2024.png differ diff --git a/doc/sponsors_jan_2024.png b/doc/sponsors_jan_2024.png new file mode 100644 index 0000000000..601df4909c Binary files /dev/null and b/doc/sponsors_jan_2024.png differ diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index fe539b0965..91fc49b71d 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 @@ -222,6 +222,17 @@ install(TARGETS ${library_name} RUNTIME DESTINATION bin ) +# we will embed the list of plugin names inside a header file +set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen) +configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp) + +add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp) +ament_target_dependencies(generate_nav2_tree_nodes_xml ${dependencies}) +# allow generate_nav2_tree_nodes_xml to find plugins_list.hpp +target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR}) +install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME}) + + install(DIRECTORY include/ DESTINATION include/ ) @@ -231,6 +242,7 @@ install(DIRECTORY test/utils/ ) install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) +install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 84e16e7d4e..0ade4e0a01 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 daf609d067..9ee903fd78 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,10 +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_v3/loggers/bt_zmq_publisher.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 39c5c5f7d9..6193a91a86 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 eef3ad2c86..5e005fc610 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 ea3e41d859..789e30133d 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 1afdb5adfd..d51c7d2839 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 c0425d1711..7075bc63f4 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" @@ -132,20 +132,27 @@ inline std::set convertFromString(StringView key) } /** - * @brief Return parameter value from behavior tree node or ros2 parameter file + * @brief Return parameter value from behavior tree node or ros2 parameter file. * @param node rclcpp::Node::SharedPtr * @param param_name std::string * @param behavior_tree_node T2 * @return */ -template +template T1 deconflictPortAndParamFrame( rclcpp::Node::SharedPtr node, std::string param_name, - T2 * behavior_tree_node) + const T2 * behavior_tree_node) { T1 param_value; - if (!behavior_tree_node->getInput(param_name, param_value)) { + bool param_from_input = behavior_tree_node->getInput(param_name, param_value).has_value(); + + if constexpr (std::is_same_v) { + // not valid if port doesn't exist or it is an empty string + param_from_input &= !param_value.empty(); + } + + if (!param_from_input) { RCLCPP_DEBUG( node->get_logger(), "Parameter '%s' not provided by behavior tree xml file, " @@ -162,6 +169,37 @@ T1 deconflictPortAndParamFrame( } } +/** + * @brief Try reading an import port first, and if that doesn't work + * fallback to reading directly the blackboard. + * The blackboard must be passed explitly because config() is private in BT.CPP 4.X + * + * @param bt_node node + * @param blackboard the blackboard ovtained with node->config().blackboard + * @param param_name std::string + * @param behavior_tree_node the node + * @return + */ +template inline +bool getInputPortOrBlackboard( + const BT::TreeNode & bt_node, + const BT::Blackboard & blackboard, + const std::string & param_name, + T & value) +{ + if (bt_node.getInput(param_name, value)) { + return true; + } + if (blackboard.get(param_name, value)) { + return true; + } + return false; +} + +// Macro to remove boiler plate when using getInputPortOrBlackboard +#define getInputOrBlackboard(name, value) \ + getInputPortOrBlackboard(*this, *(this->config().blackboard), name, value); + } // namespace BT #endif // NAV2_BEHAVIOR_TREE__BT_UTILS_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 968750a05e..625f7e9483 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 11f0d0f423..90a10f1675 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 3aa9e6573e..4a1dd779a3 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 e996bda55b..c9c6115108 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 d9b3d4a59c..344afd546d 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 f9bb293d26..da5a727444 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 2604bcfea1..5501d49cf3 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 113e0d83e7..94b4b66ca2 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 42721f6266..209958c38d 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 3c87c7370c..67747c62b8 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 64f77f1473..7e3e92e799 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" @@ -57,7 +57,12 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: 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 acbca8c960..b79fabe2f9 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 1e8c9712c1..89d4b7a573 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 @@ -54,7 +54,12 @@ class GoalUpdatedCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: 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 2bb7d995b8..548c15268b 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 @@ -15,7 +15,8 @@ #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ -#include "behaviortree_cpp_v3/behavior_tree.h" +#include +#include "behaviortree_cpp/behavior_tree.h" namespace nav2_behavior_tree { @@ -23,7 +24,21 @@ namespace nav2_behavior_tree * @brief A BT::ConditionNode that returns SUCCESS if initial pose * has been received and FAILURE otherwise */ -BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); -} +class InitialPoseReceived : public BT::ConditionNode +{ +public: + InitialPoseReceived( + const std::string & name, + const BT::NodeConfiguration & config); + + static BT::PortsList providedPorts() + { + return {BT::InputPort("initial_pose_received")}; + } + + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ 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 37a53b41e1..704154a31d 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 ff47910580..59a023ff3c 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 36890f2a8a..5a9f255a9b 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 565f261100..e532822d42 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 fb2f42f3d4..8871892949 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 356a79857d..26a3431b5d 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 5fdefa9b8b..511c381321 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 0384381d2a..ce3406904f 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 89c0cfa300..62759ea711 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 8c374ce6af..86f2b38c5b 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 f85a3975d0..7fbda19c63 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 4ac0ab44ee..bdd4171185 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" @@ -48,7 +48,12 @@ class GoalUpdatedController : public BT::DecoratorNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: 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 49dfbc1a0c..ddce12cf02 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 6e41516434..bdf4a080d1 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 24a0207e3b..c390357b34 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 c16ef63efa..02c0c1812e 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 8150eb3109..ed454c0aa1 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 { @@ -57,6 +57,10 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), }; } 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 0e14a59ee8..d1ccef107e 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 3e23aafbd0..925e9c3579 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. --> - + @@ -224,9 +224,15 @@ Parent frame for transform - + + Vector of navigation goals + Navigation goal + - + + Vector of navigation goals + Navigation goal + Min battery % or voltage before triggering @@ -255,6 +261,7 @@ + SUCCESS if initial_pose_received true @@ -313,6 +320,8 @@ Maximum rate Minimum speed Maximum speed + Vector of navigation goals + Navigation goal @@ -322,6 +331,8 @@ + Vector of navigation goals + Navigation goal diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 797d53270b..b9ed847ce3 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 64eb61d788..e876d2ce40 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 e226ba1975..362499c9f2 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 09d79c7e23..e17580fe71 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 3baa1ffb1e..21835c326a 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 08d2b00632..2d007a6623 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 8bfffed843..a0572ecad6 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 c4b93afa85..c7f1a0164e 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 966ee01e10..b2eb4358c2 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 7d77adbee3..b58d78bb78 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 7e0b613f62..03c0034414 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 b29de63df5..8ac530f8df 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 00acf5c349..b662221de0 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 1a2e9c703b..3ee9d83260 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 57db894248..9213cd564f 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 8022697e0e..07bea22436 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 ccb8518412..5d0610c0fa 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 e8c418ddc1..fea2194158 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 388f16ccb2..30b876d534 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 bf8b95c84f..86f5fffd6b 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -41,7 +41,7 @@ void RemovePassedGoals::initialize() auto node = config().blackboard->get("node"); node->get_parameter("transform_tolerance", transform_tolerance_); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -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 da547b1587..3a67904558 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 0a84062e08..c717332c79 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 cab9c8f821..d10bb83f32 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 62ddbc4719..1d75c5cf27 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 b394a804c7..7bebfbfba3 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 934f2b86bb..35dc8635c8 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 b931707673..b607d026e5 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 9365b7e06f..b45db33396 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 6764cccd11..f39dd8fbce 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 64394ee378..7db1817c65 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -43,9 +43,9 @@ void DistanceTraveledCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( + global_frame_ = BT::deconflictPortAndParamFrame( node_, "global_frame", this); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); initialized_ = true; } @@ -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 fee838c937..dbd84d8b2e 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -16,6 +16,7 @@ #include #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -33,15 +34,15 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() { if (first_time) { first_time = false; - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::SUCCESS; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; @@ -54,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 2697680415..7024356203 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -33,7 +33,7 @@ GoalReachedCondition::GoalReachedCondition( { auto node = config().blackboard->get("node"); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -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 34930bb582..88d329efc2 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -27,16 +28,16 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { - if (status() == BT::NodeStatus::IDLE) { - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + if (!BT::isStatusActive(status())) { + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::FAILURE; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goals", current_goals); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; @@ -49,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 41796834c6..9d93022912 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -13,22 +13,28 @@ // limitations under the License. #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { +InitialPoseReceived::InitialPoseReceived( + const std::string & name, + const BT::NodeConfiguration & config) +: BT::ConditionNode(name, config) +{ +} -BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node) +BT::NodeStatus InitialPoseReceived::tick() { - auto initPoseReceived = tree_node.config().blackboard->get("initial_pose_received"); + bool initPoseReceived = false; + BT::getInputOrBlackboard("initial_pose_received", initPoseReceived); return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { - factory.registerSimpleCondition( - "InitialPoseReceived", - std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1)); + 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 c9c05f6875..27e1bd55fc 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 9d221cf8c8..a0e3761f28 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 59d593a147..7274743e1e 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 a898dec947..d0ca48cd7f 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 1347998602..540af1d83d 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 64afd1a34b..f03c9711aa 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 33e94178de..0ee491fbfd 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 f8c62064f6..87625d4511 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 91de9c2e22..603eb60f10 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 fb903e1cbf..50665256c2 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 640c3ed7b4..bc2327af28 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 44e56dc55e..6eb3c6e99e 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 135ea09b74..9293a5b118 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 b59fba77b4..7f87695416 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" @@ -43,15 +43,15 @@ DistanceController::DistanceController( tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( + global_frame_ = BT::deconflictPortAndParamFrame( node_, "global_frame", this); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); } 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 f8a2d8cefc..d0de920545 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -17,9 +17,9 @@ #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" namespace nav2_behavior_tree { @@ -33,12 +33,12 @@ 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) - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); goal_was_updated_ = true; } @@ -46,9 +46,9 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; @@ -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 1f7226e6aa..fa5badf750 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 d4b40b0964..cb39a24557 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 b710ec3009..9592d119c9 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 84f0fadfbb..c95d646438 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 f47dfed38a..b8e5b3915a 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -19,6 +19,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -58,20 +59,20 @@ 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) - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); period_ = 1.0 / max_rate_; start_ = node_->now(); first_tick_ = true; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { // Reset state and set period to max since we have a new goal @@ -100,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(); @@ -120,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/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in new file mode 100644 index 0000000000..148583b927 --- /dev/null +++ b/nav2_behavior_tree/plugins_list.hpp.in @@ -0,0 +1,6 @@ + +// This was automativally generated by cmake +namespace nav2::details +{ + const char* BT_BUILTIN_PLUGINS = "@plugin_libs@"; +} diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 429368ea96..8ed1cd4e71 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 @@ -48,11 +53,11 @@ BehaviorTreeEngine::run( try { while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { - tree->rootNode()->halt(); + tree->haltTree(); return BtStatus::CANCELED; } - result = tree->tickRoot(); + result = tree->tickOnce(); onLoop(); diff --git a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp new file mode 100644 index 0000000000..4992e2e777 --- /dev/null +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2024 Davide Faconti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/xml_parsing.h" + +#include "plugins_list.hpp" + +int main() +{ + BT::BehaviorTreeFactory factory; + + std::vector plugins_list; + boost::split(plugins_list, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + + for (const auto & plugin : plugins_list) { + std::cout << "Loading: " << plugin << "\n"; + factory.registerFromPlugin(BT::SharedLibrary::getOSName(plugin)); + } + std::cout << "\nGenerating file: nav2_tree_nodes.xml\n" + << "\nCompare it with the one in the git repo and update the latter if necessary.\n"; + + std::ofstream xml_file; + xml_file.open("nav2_tree_nodes.xml"); + xml_file << BT::writeTreeNodesModelXML(factory) << std::endl; + xml_file.close(); + + return 0; +} 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 cbb5606d8b..c5d40f0f4e 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 a9261fe4b8..a516d868e4 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 9df8b3da15..846e7e86db 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 c7f379a64c..a3ea28791c 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 ec3aa03a07..8aa1c3366e 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 e21f9802d7..76f3b1d025 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 001c3de67f..24d10b63d6 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"( - + @@ -156,13 +156,13 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal @@ -176,7 +176,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -187,7 +187,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + @@ -222,13 +222,13 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start @@ -244,7 +244,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); 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 f1da97cb25..29ebaef936 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"( - + @@ -153,13 +153,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal @@ -173,7 +173,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -184,7 +184,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + @@ -219,13 +219,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start @@ -241,7 +241,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); 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 7be55c08ae..564e72d3d5 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 8c98db78b3..c7f04ae9a9 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"( - + @@ -95,7 +95,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // check default value std::string selected_controller_result; - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ(selected_controller_result, "DWB"); @@ -119,7 +119,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) } // check controller updated - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ("DWC", selected_controller_result); } @@ -128,7 +128,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +143,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // check default value std::string selected_controller_result; - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ(selected_controller_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) } // check controller updated - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ("RRT", selected_controller_result); } 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 c4439f1e1c..baa5ea02a2 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 762351bdd1..c94ed1d89f 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 fea7a2b2b9..4f27220819 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"( - + @@ -145,7 +145,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal 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 1fd0e286cb..19089fb6f3 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"( - + @@ -95,7 +95,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // check default value std::string selected_goal_checker_result; - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ(selected_goal_checker_result, "SimpleGoalCheck"); @@ -119,7 +119,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) } // check goal_checker updated - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ("AngularGoalChecker", selected_goal_checker_result); } @@ -128,7 +128,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +143,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // check default value std::string selected_goal_checker_result; - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ(selected_goal_checker_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) } // check goal_checker updated - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ("RRT", selected_goal_checker_result); } 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 90cdfbbb0c..a805a721b9 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"( - + @@ -148,7 +148,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->poses, poses); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } 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 5b3764befb..4ed5120b98 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"( - + @@ -139,7 +139,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal 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 b8851d81c3..ade80c57da 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"( - + @@ -93,7 +93,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // check default value std::string selected_planner_result; - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ(selected_planner_result, "GridBased"); @@ -117,7 +117,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) } // check planner updated - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ("RRT", selected_planner_result); } @@ -126,7 +126,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -141,7 +141,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // check default value std::string selected_planner_result; - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ(selected_planner_result, "GridBased"); @@ -165,7 +165,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) } // check planner updated - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ("RRT", selected_planner_result); } 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 574a514886..c97bbc1969 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 73e72fa764..04b0e70490 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 7bfda58e54..ce9d0debf8 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"( - + @@ -137,7 +137,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) // check that it removed the point in range std::vector output_poses; - config_->blackboard->get("goals", output_poses); + EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); EXPECT_EQ(output_poses.size(), 2u); EXPECT_EQ(output_poses[0], poses[2]); 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 6460a5d9b9..d60ed2ffb7 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"( - + @@ -138,7 +138,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->path, path); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal 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 de73cc4fe5..f93f6df878 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"( - + @@ -95,7 +95,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // check default value std::string selected_smoother_result; - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ(selected_smoother_result, "DWB"); @@ -119,7 +119,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) } // check smoother updated - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ("DWC", selected_smoother_result); } @@ -128,7 +128,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +143,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // check default value std::string selected_smoother_result; - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ(selected_smoother_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) } // check smoother updated - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ("RRT", selected_smoother_result); } 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 e1285d28a9..e3d2e80c85 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 8cc3083eb3..54a0270e78 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 9ca640d47d..2e7ac3fccd 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"( - + @@ -127,7 +127,7 @@ TEST_F(TruncatePathTestFixture, test_tick) } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_NE(path, truncated_path); EXPECT_EQ(truncated_path.poses.size(), 2u); 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 00564d4e8f..c3333605f8 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"( - + blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -160,7 +160,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -182,7 +182,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -201,7 +201,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(path, truncated_path); @@ -245,7 +245,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); SUCCEED(); @@ -287,7 +287,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); SUCCEED(); @@ -328,7 +328,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -395,7 +395,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -428,7 +428,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 2e9fb8b168..bc0acfd94d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_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/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 ebce2f6341..4b55e3d133 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 d1716397df..111cda7d00 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 1580a8d1bc..1034c4f9b6 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_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp index 1f9aa41bb8..7b8a67529e 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp @@ -21,48 +21,30 @@ #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" -class TestNode : public BT::SyncActionNode -{ -public: - TestNode(const std::string & name, const BT::NodeConfiguration & config) - : SyncActionNode(name, config) - {} - - BT::NodeStatus tick() - { - return BT::NodeStatus::SUCCESS; - } - - static BT::PortsList providedPorts() - { - return {}; - } -}; - class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture { public: void SetUp() { - test_node_ = std::make_shared("TestNode", *config_); + bt_node_ = std::make_shared("TestNode", *config_); } void TearDown() { - test_node_.reset(); + bt_node_.reset(); } protected: - static std::shared_ptr test_node_; + static std::shared_ptr bt_node_; }; -std::shared_ptr InitialPoseReceivedConditionTestFixture::test_node_ = nullptr; +std::shared_ptr InitialPoseReceivedConditionTestFixture::bt_node_ = nullptr; TEST_F(InitialPoseReceivedConditionTestFixture, test_behavior) { - EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); config_->blackboard->set("initial_pose_received", true); - EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), 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 79fa1e08c2..8cd761aadf 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 a39a414089..5e763f56d4 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 9c022359a1..df973cb280 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 2b21fd416c..73316dfa63 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 d93249879d..839a4003fe 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 4dc0f8bb5c..89e09265dc 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 337d5133c3..7a6dff9be9 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 d50a6d4c51..517ee1f04a 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 cc5e2d2969..1ca41e798e 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 2996bca25a..7daa91c069 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 221371a414..fa56a2064a 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"( - + @@ -107,7 +107,7 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); } TEST_F(GoalUpdaterTestFixture, test_older_goal_update) @@ -115,7 +115,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) // create tree std::string xml_txt = R"( - + @@ -141,7 +141,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) goal_updater_pub->publish(goal_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); // expect to succeed and not update goal EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -153,7 +153,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) // create tree std::string xml_txt = R"( - + @@ -184,7 +184,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) goal_updater_pub->publish(goal_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); // expect to succeed EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); 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 0f08bafe69..5d521e205c 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 076b9c3b7c..1c008d6478 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"( - + @@ -310,12 +310,12 @@ TEST(deconflictPortAndParamFrameTest, test_correct_syntax) node->declare_parameter("test", 2); node->declare_parameter("test_alternative", 3); - int value = BT::deconflictPortAndParamFrame( + int value = BT::deconflictPortAndParamFrame( node, "test_alternative", tree.rootNode()); EXPECT_EQ(value, 3); - value = BT::deconflictPortAndParamFrame( + value = BT::deconflictPortAndParamFrame( node, "test", tree.rootNode()); EXPECT_EQ(value, 1); 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 6215909ae9..7bf08ddd44 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 d277e7f4d2..dcfa52927a 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_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 172a832100..91401fc21f 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index da426b4722..732fa24eb1 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 8249df84d1..bb812d276b 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 28b23ab289..28e7d273fb 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index cfe6a0d486..7de1592106 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 6941a8c7e6..9f55ecd66e 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 8fa2eca237..6973057e93 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 7a235ff040..28eda8ed89 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 8cb0eb749c..772d36fded 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 ea2a731590..a705aa6b3a 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 444c9458ff..872dbf8018 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 61132d5f95..16f7ede4b3 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 56e360933c..a214c9b64f 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 b9f903fd33..effe9a2bf8 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 a48ae89a76..e0d5d6a5a4 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 f18111f5bd..11332fb24e 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 49000e14fa..fdc459f40f 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 289f08d7ea..d4eaa63c3c 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/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index c588e6ff4d..4c1b4f47bc 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -18,12 +18,15 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + using nav2_util::declare_parameter_if_not_declared; namespace nav2_bt_navigator @@ -36,64 +39,8 @@ BtNavigator::BtNavigator(rclcpp::NodeOptions options) { RCLCPP_INFO(get_logger(), "Creating"); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_goal_updated_controller_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_is_battery_charging_condition_bt_node", - "nav2_progress_checker_selector_bt_node", - "nav2_smoother_selector_bt_node" - }; - declare_parameter_if_not_declared( - this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + this, "plugin_lib_names", rclcpp::ParameterValue(std::vector{})); declare_parameter_if_not_declared( this, "transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( @@ -126,7 +73,14 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) odom_topic_ = get_parameter("odom_topic").as_string(); // Libraries to pull plugins (BT Nodes) from - auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); + std::vector plugin_lib_names; + boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + + auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); + // append user_defined_plugins to plugin_lib_names + plugin_lib_names.insert( + plugin_lib_names.end(), user_defined_plugins.begin(), + user_defined_plugins.end()); nav2_core::FeedbackUtils feedback_utils; feedback_utils.tf = tf_; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 53769abfcd..a4b4df52d1 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 326a0bd9fb..2270b6add5 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_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 7030ddd2b2..4fd4f8bc0b 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -40,7 +40,6 @@ using namespace std::chrono_literals; static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; -static const char BASE2_FRAME_ID[]{"base2_link"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestVelocityPolygon"}; static const char SUB_POLYGON_FORWARD_NAME[]{"Forward"}; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b137..85aae171cc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,6 +419,7 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -479,10 +480,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) { diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index dff1207b88..0eff064a5d 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -71,11 +71,13 @@ class GlobalPlanner * @brief Method create the plan from a starting and ending goal. * @param start The starting pose of the robot * @param goal The goal pose of the robot + * @param cancel_checker Function to check if the action has been canceled * @return The sequence of poses to get from start to goal, if any */ virtual nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) = 0; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index 2680b3c669..c16c1c1096 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -113,6 +113,13 @@ class NoViapointsGiven : public PlannerException : PlannerException(description) {} }; +class PlannerCancelled : public PlannerException +{ +public: + explicit PlannerCancelled(const std::string & description) + : PlannerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index df0ddb2dc3..723f81f0f9 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,17 +26,10 @@ find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.3 REQUIRED) nav2_package() -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} -) - -add_definitions(${EIGEN3_DEFINITIONS}) - add_library(nav2_costmap_2d_core SHARED src/costmap_2d.cpp src/layer.cpp @@ -51,6 +44,13 @@ add_library(nav2_costmap_2d_core SHARED src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_core ALIAS nav2_costmap_2d_core) + +target_include_directories(nav2_costmap_2d_core + PUBLIC + "$" + "$" +) set(dependencies geometry_msgs @@ -78,6 +78,7 @@ set(dependencies ament_target_dependencies(nav2_costmap_2d_core ${dependencies} ) +target_link_libraries(nav2_costmap_2d_core Eigen3::Eigen) add_library(layers SHARED plugins/inflation_layer.cpp @@ -88,11 +89,12 @@ add_library(layers SHARED plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp ) +add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(filters SHARED @@ -100,11 +102,14 @@ add_library(filters SHARED plugins/costmap_filters/speed_filter.cpp plugins/costmap_filters/binary_filter.cpp ) +add_library(${PROJECT_NAME}::filters ALIAS filters) + + ament_target_dependencies(filters ${dependencies} ) target_link_libraries(filters - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(nav2_costmap_2d_client SHARED @@ -112,18 +117,19 @@ add_library(nav2_costmap_2d_client SHARED src/costmap_subscriber.cpp src/costmap_topic_collision_checker.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_client ALIAS nav2_costmap_2d_client) ament_target_dependencies(nav2_costmap_2d_client ${dependencies} ) target_link_libraries(nav2_costmap_2d_client - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_target_dependencies(nav2_costmap_2d_markers @@ -132,7 +138,7 @@ ament_target_dependencies(nav2_costmap_2d_markers add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) target_link_libraries(nav2_costmap_2d_cloud - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d src/costmap_2d_node.cpp) @@ -141,26 +147,24 @@ ament_target_dependencies(nav2_costmap_2d ) target_link_libraries(nav2_costmap_2d - nav2_costmap_2d_core - layers - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers + ${PROJECT_NAME}::filters ) install(TARGETS - nav2_costmap_2d_core layers filters + nav2_costmap_2d_core nav2_costmap_2d_client + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS - nav2_costmap_2d - nav2_costmap_2d_markers - nav2_costmap_2d_cloud - RUNTIME DESTINATION lib/${PROJECT_NAME} +install(TARGETS nav2_costmap_2d_markers nav2_costmap_2d_cloud nav2_costmap_2d + DESTINATION lib/${PROJECT_NAME} ) install(FILES costmap_plugins.xml @@ -168,7 +172,7 @@ install(FILES costmap_plugins.xml ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -183,8 +187,7 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() -ament_export_include_directories(include) -ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 868b5081a1..7fbef3d41d 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,90 +1,66 @@ ament_add_gtest_executable(footprint_tests_exec footprint_tests.cpp ) -ament_target_dependencies(footprint_tests_exec - ${dependencies} -) target_link_libraries(footprint_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_collision_checker_exec test_costmap_topic_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker_exec - ${dependencies} -) target_link_libraries(test_collision_checker_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(inflation_tests_exec inflation_tests.cpp ) -ament_target_dependencies(inflation_tests_exec - ${dependencies} -) target_link_libraries(inflation_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(obstacle_tests_exec obstacle_tests.cpp ) -ament_target_dependencies(obstacle_tests_exec - ${dependencies} -) target_link_libraries(obstacle_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(range_tests_exec range_tests.cpp ) -ament_target_dependencies(range_tests_exec - ${dependencies} -) target_link_libraries(range_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) -ament_target_dependencies(dyn_params_tests - ${dependencies} -) target_link_libraries(dyn_params_tests - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest_executable(test_costmap_publisher_exec test_costmap_2d_publisher.cpp ) -ament_target_dependencies(test_costmap_publisher_exec - ${dependencies} -) target_link_libraries(test_costmap_publisher_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_costmap_subscriber_exec test_costmap_subscriber.cpp ) -ament_target_dependencies(test_costmap_subscriber_exec - ${dependencies} -) target_link_libraries(test_costmap_subscriber_exec - nav2_costmap_2d_core - nav2_costmap_2d_client + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client ) ament_add_test(test_collision_checker @@ -166,6 +142,6 @@ ament_add_test(test_costmap_subscriber_exec # ${dependencies} # ) # target_link_libraries(costmap_tester -# nav2_costmap_2d_core +# ${PROJECT_NAME}::nav2_costmap_2d_core # layers # ) diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index d674ee7669..849edf282c 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -53,7 +53,7 @@ class TestCostmapSubscriberShould : public ::testing::Test costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); } - void TearDown() + void TearDown() override { costmapSubscriber.reset(); costmapToSend.reset(); diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index 78fbfa6cf7..aafc4d561b 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,7 +1,7 @@ # Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) # OrderLayer for checking Costmap2D plugins API calling order @@ -11,7 +11,7 @@ ament_target_dependencies(order_layer ${dependencies} ) target_link_libraries(order_layer - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) install(TARGETS order_layer @@ -23,5 +23,5 @@ install(TARGETS # Costmap2D plugins API calling order test ament_add_gtest(plugin_api_order plugin_api_order.cpp) target_link_libraries(plugin_api_order - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index e69501277e..d3c5edd813 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -1,57 +1,58 @@ ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) target_link_libraries(costmap_convesion_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) target_link_libraries(declare_parameter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_test costmap_filter_test.cpp) target_link_libraries(costmap_filter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) target_link_libraries(keepout_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(speed_filter_test speed_filter_test.cpp) target_link_libraries(speed_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(binary_filter_test binary_filter_test.cpp) target_link_libraries(binary_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp) target_link_libraries(costmap_filter_service_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_test.cpp) target_link_libraries(denoise_layer_test - nav2_costmap_2d_core layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(lifecycle_test lifecycle_test.cpp) target_link_libraries(lifecycle_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) \ No newline at end of file diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 322485cec8..9cb5a72ec7 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,7 +1,7 @@ # Graceful Motion Controller The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. ## Smooth control law The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: @@ -39,4 +39,4 @@ The smooth control law is a pose-following kinematic control law that generates | `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | | `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | | `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | -| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | \ No newline at end of file +| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1501e4321a..d2bb6d9c9d 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(mppi_controller SHARED add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp + src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index fd7ad80214..3bf779e093 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -98,6 +98,9 @@ This process is then repeated a number of times and returns a converged solution #### Obstacles Critic + +Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles + | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | @@ -109,6 +112,20 @@ This process is then repeated a number of times and returns a converged solution | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | +#### Cost Critic + +Uses inflated costmap cost directly to avoid obstacles + + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | + | cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. | + | cost_power | int | Default 1. Power order to apply to term. | + | collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. | + | critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. | + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | + #### Path Align Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -184,7 +201,7 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: enabled: true cost_power: 1 @@ -204,15 +221,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - ObstaclesCritic: + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: enabled: true cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 - consider_footprint: false - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: PathAlignCritic: enabled: true cost_power: 1 @@ -262,6 +288,8 @@ If you don't require path following behavior (e.g. just want to follow a goal po By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. +If you want to slow further on approach to goal, consider increasing the ``threshold_to_consider`` parameters to give a hand off from the path tracking critics to the goal approach critics sooner - then tune those critics for your profile of interest. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 790568e796..d578d23a9e 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -5,6 +5,10 @@ mppi critic for obstacle avoidance + + mppi critic for obstacle avoidance using costmap score + + mppi critic for driving towards the goal diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp new file mode 100644 index 0000000000..14c0fe93f4 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023 Robocc Brice Renaudeau +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::CostCritic + * @brief Critic objective function for avoiding obstacles using costmap's inflated cost + */ +class CostCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Checks if cost represents a collision + * @param cost Point cost at pose center + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return bool if in collision + */ + bool inCollision(float cost, float x, float y, float theta); + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @return Collision information at pose + */ + float costAtPose(float x, float y); + + /** + * @brief Find the min cost of the inflation decay function for which the robot MAY be + * in collision in any orientation + * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) + * @return double circumscribed cost, any higher than this and need to do full footprint collision checking + * since some element of the robot could be in collision + */ + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + float possible_collision_cost_; + + bool consider_footprint_{true}; + float circumscribed_radius_{0}; + float circumscribed_cost_{0}; + float collision_cost_{0}; + float critical_cost_{0}; + float weight_{0}; + + float near_goal_distance_; + std::string inflation_layer_name_; + + unsigned int power_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 358994c238..31b6a3d0df 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -90,7 +90,7 @@ class ObstaclesCritic : public CriticFunction float collision_cost_{0}; float inflation_scale_factor_{0}, inflation_radius_{0}; - float possibly_inscribed_cost_; + float possible_collision_cost_; float collision_margin_distance_; float near_goal_distance_; float circumscribed_cost_{0}, circumscribed_radius_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index fb22b38b24..49ff2cb010 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -314,6 +314,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i < dists.shape(0); i++) { min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); for (size_t j = 0; j < dists.shape(1); j++) { cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp new file mode 100644 index 0000000000..7a9b7729ed --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -0,0 +1,200 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81); + getParam(critical_cost_, "critical_cost", 300.0); + getParam(collision_cost_, "collision_cost", 1000000.0); + getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + if (!enabled_) { + return; + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { + near_goal = true; + } + + auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + repulsive_cost.fill(0.0); + + const size_t traj_len = data.trajectories.x.shape(1); + bool all_trajectories_collide = true; + for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + bool trajectory_collide = false; + const auto & traj = data.trajectories; + float pose_cost; + + for (size_t j = 0; j < traj_len; j++) { + // The costAtPose doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); + if (pose_cost < 1.0f) {continue;} // In free space + + if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + using namespace nav2_costmap_2d; // NOLINT + if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { + repulsive_cost[i] += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + repulsive_cost[i] += pose_cost; + } + } + + if (!trajectory_collide) { + all_trajectories_collide = false; + } else { + repulsive_cost[i] = collision_cost_; + } + } + + data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool CostCritic::inCollision(float cost, float x, float y, float theta) +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + // If consider_footprint_ check footprint scort for collision + if (consider_footprint_ && + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + } + + switch (static_cast(cost)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +float CostCritic::costAtPose(float x, float y) +{ + using namespace nav2_costmap_2d; // NOLINT + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + return nav2_costmap_2d::NO_INFORMATION; + } + + return collision_checker_.pointCost(x_i, y_i); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 21d8239615..cdf0113564 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -32,9 +32,9 @@ void ObstaclesCritic::initialize() getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1.0f) { + if (possible_collision_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -111,7 +111,7 @@ void ObstaclesCritic::score(CriticData & data) if (consider_footprint_) { // footprint may have changed since initialization if user has dynamic footprints - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } // If near the goal, don't apply the preferential term since the goal is near obstacles @@ -212,7 +212,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) cost = collision_checker_.pointCost(x_i, y_i); if (consider_footprint_ && - (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 08f50945fd..fee853cda4 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -24,6 +24,7 @@ #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_mppi_controller/critics/cost_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" #include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d694496395..9a77f6d8b0 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index a4ce84404a..966a845852 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace nav2_navfn_planner { @@ -131,14 +132,16 @@ class NavFn /** * @brief Calculates a plan using the A* heuristic, returns true if one is found + * @param cancelChecker Function to check if the task has been canceled * @return True if a plan is found, false otherwise */ - bool calcNavFnAstar(); + bool calcNavFnAstar(std::function cancelChecker); /** - * @brief Caclulates the full navigation function using Dijkstra + * @brief Calculates the full navigation function using Dijkstra + * @param cancelChecker Function to check if the task has been canceled */ - bool calcNavFnDijkstra(bool atStart = false); + bool calcNavFnDijkstra(std::function cancelChecker, bool atStart = false); /** * @brief Accessor for the x-coordinates of a path @@ -179,6 +182,9 @@ class NavFn float curT; /**< current threshold */ float priInc; /**< priority threshold increment */ + /**< number of cycles between checks for cancellation */ + static constexpr int terminal_checking_interval = 5000; + /** goal and start positions */ /** * @brief Sets the goal position for the planner. @@ -229,18 +235,20 @@ class NavFn * @brief Run propagation for iterations, or until start is reached using * breadth-first Dijkstra method * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @param atStart Whether or not to stop when the start point is reached * @return true if the start point is reached */ - bool propNavFnDijkstra(int cycles, bool atStart = false); + bool propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart = false); /** * @brief Run propagation for iterations, or until start is reached using * the best-first A* method with Euclidean distance heuristic * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @return true if the start point is reached */ - bool propNavFnAstar(int cycles); /**< returns true if start point found */ + bool propNavFnAstar(int cycles, std::function cancelChecker); /** gradient and paths */ float * gradx, * grady; /**< gradient arrays, size of potential array */ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 2330a5165b..c1cebb1bef 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -81,11 +81,13 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the task has been canceled * @return nav_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -93,12 +95,14 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @param start Start pose * @param goal Goal pose * @param tolerance Relaxation constraint in x and y + * @param cancel_checker Function to check if the task has been canceled * @param plan Path to be computed * @return true if can find the path */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan); /** diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8259950b03..2cce713b0f 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -44,6 +44,7 @@ #include "nav2_navfn_planner/navfn.hpp" #include +#include "nav2_core/planner_exceptions.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_navfn_planner @@ -293,12 +294,12 @@ NavFn::setCostmap(const COSTTYPE * cmap, bool isROS, bool allow_unknown) } bool -NavFn::calcNavFnDijkstra(bool atStart) +NavFn::calcNavFnDijkstra(std::function cancelChecker, bool atStart) { setupNavFn(true); // calculate the nav fn and path - return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart); + return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), cancelChecker, atStart); } @@ -307,12 +308,12 @@ NavFn::calcNavFnDijkstra(bool atStart) // bool -NavFn::calcNavFnAstar() +NavFn::calcNavFnAstar(std::function cancelChecker) { setupNavFn(true); // calculate the nav fn and path - return propNavFnAstar(std::max(nx * ny / 20, nx + ny)); + return propNavFnAstar(std::max(nx * ny / 20, nx + ny), cancelChecker); } // @@ -571,7 +572,7 @@ NavFn::updateCellAstar(int n) // bool -NavFn::propNavFnDijkstra(int cycles, bool atStart) +NavFn::propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -581,6 +582,10 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) int startCell = start[1] * nx + start[0]; for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % terminal_checking_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } @@ -652,7 +657,7 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) // bool -NavFn::propNavFnAstar(int cycles) +NavFn::propNavFnAstar(int cycles, std::function cancelChecker) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -667,6 +672,10 @@ NavFn::propNavFnAstar(int cycles) // do main cycle for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % terminal_checking_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ee8d69f53..9985280159 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -129,7 +129,8 @@ NavfnPlanner::cleanup() nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); @@ -183,7 +184,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( return path; } - if (!makePlan(start.pose, goal.pose, tolerance_, path)) { + if (!makePlan(start.pose, goal.pose, tolerance_, cancel_checker, path)) { throw nav2_core::NoValidPathCouldBeFound( "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } @@ -214,6 +215,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan) { // clear the plan, just in case @@ -261,9 +263,9 @@ NavfnPlanner::makePlan( planner_->setStart(map_goal); planner_->setGoal(map_start); if (use_astar_) { - planner_->calcNavFnAstar(); + planner_->calcNavFnAstar(cancel_checker); } else { - planner_->calcNavFnDijkstra(true); + planner_->calcNavFnDijkstra(cancel_checker, true); } double resolution = costmap_->getResolution(); diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 7d2c9d03f0..bdffda281a 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -68,12 +68,15 @@ class PlannerServer : public nav2_util::LifecycleNode * @brief Method to get plan from the desired plugin * @param start starting pose * @param goal goal request + * @param planner_id The planner to plan with + * @param cancel_checker A function to check if the action has been canceled * @return Path */ nav_msgs::msg::Path getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id); + const std::string & planner_id, + std::function cancel_checker); protected: /** diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index de239b3587..7f8dc4a72d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,6 +394,10 @@ void PlannerServer::computePlanThroughPoses() throw nav2_core::PlannerTFError("Unable to get start pose"); } + auto cancel_checker = [this]() { + return action_server_poses_->is_cancel_requested(); + }; + // Get consecutive paths through these points for (unsigned int i = 0; i != goal->goals.size(); i++) { // Get starting point @@ -413,7 +417,9 @@ void PlannerServer::computePlanThroughPoses() } // Get plan from start -> goal - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav_msgs::msg::Path curr_path = getPlan( + curr_start, curr_goal, goal->planner_id, + cancel_checker); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -476,6 +482,9 @@ void PlannerServer::computePlanThroughPoses() exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesResult::NO_VIAPOINTS_GIVEN; action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerCancelled &) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_poses_->terminate_all(); } catch (std::exception & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesResult::UNKNOWN; @@ -516,7 +525,11 @@ PlannerServer::computePlan() throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } - result->path = getPlan(start, goal_pose, goal->planner_id); + auto cancel_checker = [this]() { + return action_server_pose_->is_cancel_requested(); + }; + + result->path = getPlan(start, goal_pose, goal->planner_id, cancel_checker); if (!validatePath(goal_pose, result->path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -567,6 +580,9 @@ PlannerServer::computePlan() exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseResult::TF_ERROR; action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerCancelled &) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_pose_->terminate_all(); } catch (std::exception & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseResult::UNKNOWN; @@ -578,7 +594,8 @@ nav_msgs::msg::Path PlannerServer::getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id) + const std::string & planner_id, + std::function cancel_checker) { RCLCPP_DEBUG( get_logger(), "Attempting to a find path from (%.2f, %.2f) to " @@ -586,14 +603,14 @@ PlannerServer::getPlan( goal.pose.position.x, goal.pose.position.y); if (planners_.find(planner_id) != planners_.end()) { - return planners_[planner_id]->createPlan(start, goal); + return planners_[planner_id]->createPlan(start, goal, cancel_checker); } else { if (planners_.size() == 1 && planner_id.empty()) { RCLCPP_WARN_ONCE( get_logger(), "No planners specified in action call. " "Server will use only plugin %s in server." " This warning will appear once.", planner_ids_concat_.c_str()); - return planners_[planners_.begin()->first]->createPlan(start, goal); + return planners_[planners_.begin()->first]->createPlan(start, goal, cancel_checker); } else { RCLCPP_ERROR( get_logger(), "planner %s is not a valid planner. " diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index f0f3d4e335..1df075f82d 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -169,7 +169,7 @@ void Selector::pluginLoader( } RCLCPP_INFO( node->get_logger(), - (server_name + " service not available").c_str()); + "%s service not available", server_name.c_str()); server_unavailable = true; server_failed_ = true; break; diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 3a992088b6..6e92209068 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -109,6 +109,7 @@ planner_server: allow_unknown: false # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index e4ecf38f03..3fca0e8b55 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ class AStarAlgorithm typedef NodeT * NodePtr; typedef robin_hood::unordered_node_map Graph; typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; typedef std::pair> NodeElement; typedef typename NodeT::Coordinates Coordinates; typedef typename NodeT::CoordinateVector CoordinateVector; @@ -88,6 +90,8 @@ class AStarAlgorithm * @param max_on_approach_iterations Maximum number of iterations before returning a valid * path once within thresholds to refine path * comes at more compute time but smoother paths. + * @param terminal_checking_interval Number of iterations to check if the task has been canceled or + * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout */ @@ -95,20 +99,24 @@ class AStarAlgorithm const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, - const unsigned int & dim_3_size); + const unsigned int & dim_3_size, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT); /** * @brief Creating path from given costmap, start, and goal * @param path Reference to a vector of indicies of generated path * @param num_iterations Reference to number of iterations to create plan * @param tolerance Reference to tolerance in costmap nodes + * @param cancel_checker Function to check if the task has been canceled * @param expansions_log Optional expansions logged for debug * @return if plan was successful */ bool createPath( CoordinateVector & path, int & num_iterations, const float & tolerance, + std::function cancel_checker, std::vector> * expansions_log = nullptr); /** @@ -155,7 +163,7 @@ class AStarAlgorithm * @brief Get pointer reference to goal node * @return Node pointer reference to goal node */ - NodePtr & getGoal(); + NodeSet & getGoals(); /** * @brief Get maximum number of on-approach iterations after within threshold @@ -187,6 +195,13 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); + /** + * @brief Return the first goal coordinate defined by the user + * before applying the heading mode + * @return Coordinate to the first goal + */ + Coordinates getInitialGoalCoordinate(); + protected: /** * @brief Get pointer to next goal in open set @@ -250,11 +265,10 @@ class AStarAlgorithm */ void clearStart(); - int _timing_interval = 5000; - bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; double _max_planning_time; float _tolerance; unsigned int _x_size; @@ -262,9 +276,10 @@ class AStarAlgorithm unsigned int _dim3_size; SearchInfo _search_info; - Coordinates _goal_coordinates; + CoordinateVector _goals_coordinates; NodePtr _start; - NodePtr _goal; + NodeSet _goalsSet; + GoalHeadingMode _goal_heading_mode; Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 10c4224a07..967d4f65e9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" @@ -34,6 +35,7 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; + typedef std::unordered_set NodeSet; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; @@ -87,7 +89,8 @@ class AnalyticExpansion */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodePtr & goal_node, + const NodeSet & goals_node, + const Coordinates & initial_goal_coords, const NodeGetter & getter, int & iterations, int & best_cost); /** diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 53e019a3ae..205c3f0471 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -66,7 +66,7 @@ class GridCollisionChecker void setFootprint( const nav2_costmap_2d::Footprint & footprint, const bool & radius, - const double & possible_inscribed_cost); + const double & possible_collision_cost); /** * @brief Check if in collision with costmap and footprint at pose @@ -130,7 +130,7 @@ class GridCollisionChecker float footprint_cost_; bool footprint_is_radius_; std::vector angles_; - float possible_inscribed_cost_{-1}; + float possible_collision_cost_{-1}; rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; rclcpp::Clock::SharedPtr clock_; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index a7ff03fdca..9c28b1ab3a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,6 +28,14 @@ enum class MotionModel STATE_LATTICE = 4, }; +enum class GoalHeadingMode +{ + UNKNOWN = 0, + DEFAULT = 1, + BIDIRECTIONAL = 2, + ALL_DIRECTION = 3, +}; + inline std::string toString(const MotionModel & n) { switch (n) { @@ -59,6 +67,33 @@ inline MotionModel fromString(const std::string & n) } } +inline std::string toString(const GoalHeadingMode & n) +{ + switch (n) { + case GoalHeadingMode::DEFAULT: + return "DEFAULT"; + case GoalHeadingMode::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeadingMode::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeadingMode fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeadingMode::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeadingMode::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeadingMode::ALL_DIRECTION; + } else { + return GoalHeadingMode::UNKNOWN; + } +} + const float UNKNOWN = 255.0; const float OCCUPIED = 254.0; const float INSCRIBED = 253.0; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index c46a947186..56bf8c74f1 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -165,12 +165,12 @@ class NodeHybrid : x(x_in), y(y_in), theta(theta_in) {} - inline bool operator==(const Coordinates & rhs) + inline bool operator==(const Coordinates & rhs) const { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) + inline bool operator!=(const Coordinates & rhs) const { return !(*this == rhs); } @@ -459,6 +459,12 @@ class NodeHybrid */ bool backtracePath(CoordinateVector & path); + /** + * \brief Sets the goal mode for the current search + * \param goal_heading_mode The goal heading mode to use + */ + static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); + NodeHybrid * parent; Coordinates pose; @@ -476,6 +482,7 @@ class NodeHybrid // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; + static GoalHeadingMode goal_heading_mode; private: float _cell_cost; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 99f50d6a92..2ffc213087 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -415,12 +415,19 @@ class NodeLattice */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); + /** + * \brief Sets the goal mode for the current search + * \param goal_heading_mode The goal heading mode to use + */ + static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); + NodeLattice * parent; Coordinates pose; static LatticeMotionTable motion_table; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; + static GoalHeadingMode goal_heading_mode; private: float _cell_cost; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index c499d0796f..059871d867 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -82,11 +82,13 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -112,6 +114,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; bool _use_final_approach_orientation; SearchInfo _search_info; std::string _motion_model_for_search; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index fae139aa2f..73f85aa8c0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -81,11 +81,13 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -113,6 +115,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; @@ -120,6 +123,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; + GoalHeadingMode _goal_heading_mode; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index ce8bafd2fd..04325bc714 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -80,11 +80,13 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -108,6 +110,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; float _tolerance; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; @@ -117,6 +120,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _planned_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; + GoalHeadingMode _goal_heading_mode; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index ebade0f4aa..e9529bbdca 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -37,13 +37,14 @@ AStarAlgorithm::AStarAlgorithm( const SearchInfo & search_info) : _traverse_unknown(true), _max_iterations(0), + _terminal_checking_interval(5000), _max_planning_time(0), _x_size(0), _y_size(0), _search_info(search_info), - _goal_coordinates(Coordinates()), + _goals_coordinates(CoordinateVector()), _start(nullptr), - _goal(nullptr), + _goalsSet(NodeSet()), _motion_model(motion_model) { _graph.reserve(100000); @@ -59,15 +60,21 @@ void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, - const unsigned int & dim_3_size) + const unsigned int & dim_3_size, + const GoalHeadingMode & goal_heading_mode) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + _goal_heading_mode = goal_heading_mode; + NodeT::setGoalHeadingMode(_goal_heading_mode); + NodeT::precomputeDistanceHeuristic( + lookup_table_size, _motion_model, dim_3_size, _search_info); _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); @@ -78,14 +85,18 @@ void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & /*lookup_table_size*/, - const unsigned int & dim_3_size) + const unsigned int & dim_3_size, + const GoalHeadingMode & goal_heading_mode) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; + _goal_heading_mode = goal_heading_mode; if (dim_3_size != 1) { throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); @@ -184,9 +195,11 @@ void AStarAlgorithm::setGoal( if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } + _goals_coordinates.clear(); + _goalsSet.clear(); - _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + _goalsSet.insert(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); + _goals_coordinates.push_back(Node2D::Coordinates(mx, my)); } template @@ -195,14 +208,64 @@ void AStarAlgorithm::setGoal( const unsigned int & my, const unsigned int & dim_3) { - _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - - typename NodeT::Coordinates goal_coords( - static_cast(mx), - static_cast(my), - static_cast(dim_3)); + _goalsSet.clear(); + NodeVector goals; + CoordinateVector goals_coordinates; + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; + unsigned int dim_3_half_bin = 0; + switch (_goal_heading_mode) { + case GoalHeadingMode::DEFAULT: + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + break; + case GoalHeadingMode::BIDIRECTIONAL: + // Add two goals, one for each direction + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + // 180 degrees + dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins; + _goalsSet.insert(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3_half_bin))); + break; + case GoalHeadingMode::ALL_DIRECTION: + // Add all possible headings as goals + // set first goal to be the same as the input + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + for (unsigned int i = 0; i < num_bins; ++i) { + if (i == dim_3) { + continue; + } + goals.push_back(addToGraph(NodeT::getIndex(mx, my, i))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(i))); + } + break; + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + break; + } - if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + if (!_search_info.cache_obstacle_heuristic || goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -211,8 +274,11 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - _goal_coordinates = goal_coords; - _goal->setPose(_goal_coordinates); + _goals_coordinates = goals_coordinates; + for (unsigned int i = 0; i < goals.size(); i++) { + goals[i]->setPose(_goals_coordinates[i]); + _goalsSet.insert(goals[i]); + } } template @@ -224,15 +290,17 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || !_goal) { + if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { - throw nav2_core::GoalOccupied("Goal was in lethal cost"); + if (getToleranceHeuristic() < 0.001) { + // if any goal is valid, then all goals are valid + auto goal = *_goalsSet.begin(); + if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) { + throw nav2_core::GoalOccupied("Goal was in lethal cost"); + } } // Note: We do not check the if the start is valid because it is cleared @@ -245,6 +313,7 @@ template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, const float & tolerance, + std::function cancel_checker, std::vector> * expansions_log) { steady_clock::time_point start_time = steady_clock::now(); @@ -285,8 +354,11 @@ bool AStarAlgorithm::createPath( }; while (iterations < getMaxIterations() && !_queue.empty()) { - // Check for planning timeout only on every Nth iteration - if (iterations % _timing_interval == 0) { + // Check for planning timeout and cancel only on every Nth iteration + if (iterations % _terminal_checking_interval == 0) { + if (cancel_checker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } std::chrono::duration planning_duration = std::chrono::duration_cast>(steady_clock::now() - start_time); if (static_cast(planning_duration.count()) >= _max_planning_time) { @@ -316,7 +388,8 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); + current_node, getGoals(), + getInitialGoalCoordinate(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } @@ -366,7 +439,7 @@ bool AStarAlgorithm::createPath( template bool AStarAlgorithm::isGoal(NodePtr & node) { - return node == getGoal(); + return _goalsSet.find(node) != _goalsSet.end(); } template @@ -376,9 +449,9 @@ typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() } template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +typename AStarAlgorithm::NodeSet & AStarAlgorithm::getGoals() { - return _goal; + return _goalsSet; } template @@ -403,9 +476,9 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); - + // Heurittic cost for more than one goal is already cotemplated in the + // precomputation stage + float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -478,6 +551,13 @@ void AStarAlgorithm::clearStart() _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); } + +template +typename AStarAlgorithm::Coordinates AStarAlgorithm::getInitialGoalCoordinate() +{ + return _goals_coordinates[0]; +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 7c73fb91da..893f2ceb48 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,7 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & initial_goal_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -60,17 +60,20 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + + float current_best_score = std::numeric_limits::max(); + AnalyticExpansionNodes current_best_analytic_nodes = {}; + NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); - + static_cast(NodeT::getHeuristicCost(node_coords, initial_goal_coords))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); @@ -78,83 +81,99 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - // If we have a valid path, attempt to refine it - NodePtr node = current_node; - NodePtr test_node = current_node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - refined_analytic_nodes = - getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { + for (auto goal_node : goals_node) { + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, goal_node, getter, + current_node->motion_table.state_space); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && + test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + // print the goals pose + refined_analytic_nodes = + getAnalyticPath( + test_node, goal_node, getter, + test_node->motion_table.state_space); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { break; } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; } - } - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traveral cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traveral cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + if (score <= best_score) { + analytic_nodes = refined_analytic_nodes; + best_score = score; } - return score; - }; - - float best_score = scoringFn(analytic_nodes); - float score = std::numeric_limits::max(); - float min_turn_rad = node->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (node->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { - analytic_nodes = refined_analytic_nodes; - best_score = score; + if (best_score < current_best_score) { + current_best_score = best_score; + current_best_node = node; + current_best_goal = goal_node; + current_best_analytic_nodes = analytic_nodes; } } - - return setAnalyticPath(node, goal_node, analytic_nodes); } } - + if (!current_best_analytic_nodes.empty()) { + return setAnalyticPath( + current_best_node, current_best_goal, + current_best_analytic_nodes); + } analytic_iterations--; } @@ -179,16 +198,17 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.0f); + // If the length is too far, exit. This prevents unsafe shortcutting of paths // into higher cost areas far out from the goal itself, let search to the work of getting // close before the analytic expansion brings it home. This should never be smaller than // 4-5x the minimum turning radius being used, or planning times will begin to spike. - if (d > _search_info.analytic_expansion_max_length) { + if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) { return AnalyticExpansionNodes(); } - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = std::sqrt(2.0f); unsigned int num_intervals = static_cast(std::floor(d / sqrt_2)); AnalyticExpansionNodes possible_nodes; @@ -248,7 +268,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion max_cost) { + auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end()); + if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) { // If any element is above the comfortable cost limit, check edge cases: // (1) Check if goal is in greater than max_cost space requiring // entering it, but only entering it on final approach, not in-and-out @@ -354,7 +375,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & initial_goal_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 918a8a9b4e..2f23442097 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -51,9 +51,9 @@ GridCollisionChecker::GridCollisionChecker( void GridCollisionChecker::setFootprint( const nav2_costmap_2d::Footprint & footprint, const bool & radius, - const double & possible_inscribed_cost) + const double & possible_collision_cost) { - possible_inscribed_cost_ = static_cast(possible_inscribed_cost); + possible_collision_cost_ = static_cast(possible_collision_cost); footprint_is_radius_ = radius; // Use radius, no caching required @@ -113,8 +113,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5), static_cast(y + 0.5))); - if (footprint_cost_ < possible_inscribed_cost_) { - if (possible_inscribed_cost_ > 0) { + if (footprint_cost_ < possible_collision_cost_) { + if (possible_collision_cost_ > 0) { return false; } else { RCLCPP_ERROR_THROTTLE( diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 71158f32ff..5c597c8c75 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -46,6 +46,7 @@ std::shared_ptr NodeHybrid::inflation_layer = n CostmapDownsampler NodeHybrid::downsampler; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; +GoalHeadingMode NodeHybrid::goal_heading_mode; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -332,7 +333,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(theta / bin_size)); + return static_cast(floor(static_cast(theta) / bin_size)); } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) @@ -440,6 +441,9 @@ float NodeHybrid::getHeuristicCost( { const float obstacle_heuristic = getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + return obstacle_heuristic; + } const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, dist_heuristic); } @@ -780,6 +784,11 @@ void NodeHybrid::precomputeDistanceHeuristic( const unsigned int & dim_3_size, const SearchInfo & search_info) { + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + // For all direction goal heading, we only consider the obstacle heuristic + // it does not make sense to precompute the distance heuristic to save computation + return; + } // Dubin or Reeds-Shepp shortest distances if (motion_model == MotionModel::DUBIN) { motion_table.state_space = std::make_shared( @@ -816,6 +825,13 @@ void NodeHybrid::precomputeDistanceHeuristic( from[1] = y; from[2] = heading * angular_bin_size; motion_heuristic = motion_table.state_space->distance(from(), to()); + // we need to check for bidirectional goal heading and take the minimum + if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) { + unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int; + from[2] = heading_180_offset * angular_bin_size; + float motion_heuristic_180 = motion_table.state_space->distance(from(), to()); + motion_heuristic = std::min(motion_heuristic, motion_heuristic_180); + } dist_heuristic_lookup_table[index] = motion_heuristic; index++; } @@ -883,4 +899,8 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) return true; } +void NodeHybrid::setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode) +{ + goal_heading_mode = current_goal_heading_mode; +} } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c3303e0a11..90ef830146 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -38,6 +38,7 @@ namespace nav2_smac_planner LatticeMotionTable NodeLattice::motion_table; float NodeLattice::size_lookup = 25; LookupTable NodeLattice::dist_heuristic_lookup_table; +GoalHeadingMode NodeLattice::goal_heading_mode; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -332,6 +333,9 @@ float NodeLattice::getHeuristicCost( // get obstacle heuristic value const float obstacle_heuristic = getObstacleHeuristic( node_coords, goal_coords, motion_table.cost_penalty); + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + return obstacle_heuristic; + } const float distance_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, distance_heuristic); @@ -426,6 +430,11 @@ void NodeLattice::precomputeDistanceHeuristic( const unsigned int & dim_3_size, const SearchInfo & search_info) { + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + // For all direction goal heading, we only consider the obstacle heuristic + // it does not make sense to precompute the distance heuristic to save computation + return; + } // Dubin or Reeds-Shepp shortest distances if (!search_info.allow_reverse_expansion) { motion_table.state_space = std::make_shared( @@ -449,7 +458,7 @@ void NodeLattice::precomputeDistanceHeuristic( int dim_3_size_int = static_cast(dim_3_size); // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Deu to symmetries in the + // to help drive the search towards admissible approaches. Due to symmetries in the // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror // around the X axis any relative node lookup. This reduces memory overhead and increases // the size of a window a platform can store in memory. @@ -461,6 +470,13 @@ void NodeLattice::precomputeDistanceHeuristic( from[1] = y; from[2] = motion_table.getAngleFromBin(heading); motion_heuristic = motion_table.state_space->distance(from(), to()); + // we need to check for bidirectional goal heading and take the minimum + if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) { + unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int; + from[2] = motion_table.getAngleFromBin(heading_180_offset); + float motion_heuristic_180 = motion_table.state_space->distance(from(), to()); + motion_heuristic = std::min(motion_heuristic, motion_heuristic_180); + } dist_heuristic_lookup_table[index] = motion_heuristic; index++; } @@ -593,4 +609,9 @@ void NodeLattice::addNodeToPath( } } +void NodeLattice::setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode) +{ + goal_heading_mode = current_goal_heading_mode; +} + } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 70f8852f28..38fd12b9e1 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -83,6 +83,9 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation); @@ -120,6 +123,7 @@ void SmacPlanner2D::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); @@ -192,7 +196,8 @@ void SmacPlanner2D::cleanup() nav_msgs::msg::Path SmacPlanner2D::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -257,7 +262,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(costmap->getResolution()))) + _tolerance / static_cast(costmap->getResolution()), cancel_checker)) { // Note: If the start is blocked only one iteration will occur before failure if (num_iterations == 1) { @@ -377,6 +382,9 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } } } @@ -390,6 +398,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 727e4c08b2..16fa22b032 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -94,6 +94,9 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -164,6 +167,19 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } else { + _goal_heading_mode = goal_heading_mode; + } + _motion_model = fromString(_motion_model_for_search); if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( @@ -228,9 +244,11 @@ void SmacPlannerHybrid::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, - _angle_quantizations); + _angle_quantizations, + _goal_heading_mode); // Initialize path smoother if (smooth_path) { @@ -318,7 +336,8 @@ void SmacPlannerHybrid::cleanup() nav_msgs::msg::Path SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -394,7 +413,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(costmap->getResolution()), expansions.get())) + _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { geometry_msgs::msg::PoseArray msg; @@ -598,6 +617,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; @@ -616,6 +638,23 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } + } else if (name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + reinit_a_star = true; + _goal_heading_mode = goal_heading_mode; + } } } } @@ -653,9 +692,11 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, - _angle_quantizations); + _angle_quantizations, + _goal_heading_mode); } // Re-Initialize costmap downsampler diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 1666c7a9ce..3e1e0962ac 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -77,6 +77,9 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -139,6 +142,20 @@ void SmacPlannerLattice::configure( node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); node->get_parameter(name + ".debug_visualizations", _debug_visualizations); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + goal_heading_mode = fromStringToGH(goal_heading_type); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } else { + _goal_heading_mode = goal_heading_mode; + } + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -193,9 +210,11 @@ void SmacPlannerLattice::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, - _metadata.number_of_headings); + _metadata.number_of_headings, + _goal_heading_mode); // Initialize path smoother if (smooth_path) { @@ -261,7 +280,8 @@ void SmacPlannerLattice::cleanup() nav_msgs::msg::Path SmacPlannerLattice::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -316,7 +336,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(_costmap->getResolution()), expansions.get())) + _tolerance / static_cast(_costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { geometry_msgs::msg::PoseArray msg; @@ -504,15 +524,18 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } - } else if (name == _name + ".max_on_approach_iterations") { - reinit_a_star = true; - _max_on_approach_iterations = parameter.as_int(); - if (_max_on_approach_iterations <= 0) { - RCLCPP_INFO( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - _max_on_approach_iterations = std::numeric_limits::max(); + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { @@ -524,6 +547,23 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); + } else if (name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + reinit_a_star = true; + _goal_heading_mode = goal_heading_mode; + } } } } @@ -565,9 +605,11 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, - _metadata.number_of_headings); + _metadata.number_of_headings, + _goal_heading_mode); } } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 13c2c80298..ead3b7e685 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -47,10 +47,13 @@ TEST(AStarTest, test_a_star_2d) float tolerance = 0.0; float some_tolerance = 20.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 0.0, 1); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -67,6 +70,10 @@ TEST(AStarTest, test_a_star_2d) auto costmap = costmap_ros->getCostmap(); *costmap = *costmapA; + auto dummy_cancel_checker = []() { + return false; + }; + // functional case testing std::unique_ptr checker = std::make_unique(costmap_ros, 1, lnode); @@ -75,7 +82,7 @@ TEST(AStarTest, test_a_star_2d) a_star.setStart(20u, 20u, 0); a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); EXPECT_EQ(num_it, 2414); // check path is the right size and collision free @@ -92,28 +99,40 @@ TEST(AStarTest, test_a_star_2d) // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( nav2_smac_planner::MotionModel::TWOD, info); - a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1); + a_star_2.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 0, 1); num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); a_star_2.setCollisionChecker(checker.get()); num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); a_star_2.setStart(0, 0, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); num_it = 0; // invalid goal but liberal tolerance a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid - EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); + EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance, dummy_cancel_checker)); EXPECT_EQ(path.size(), 21u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_TRUE(!a_star_2.getGoals().empty()); + EXPECT_TRUE(a_star_2.getGoals().size() == 1); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); @@ -141,10 +160,13 @@ TEST(AStarTest, test_a_star_se2) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -173,7 +195,11 @@ TEST(AStarTest, test_a_star_se2) std::unique_ptr>> expansions = nullptr; expansions = std::make_unique>>(); - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, expansions.get())); + auto dummy_cancel_checker = []() { + return false; + }; + + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); // check path is the right size and collision free EXPECT_EQ(num_it, 3146); @@ -214,11 +240,14 @@ TEST(AStarTest, test_a_star_lattice) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; a_star.initialize( - false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0); @@ -239,12 +268,16 @@ TEST(AStarTest, test_a_star_lattice) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; + // functional case testing a_star.setCollisionChecker(checker.get()); a_star.setStart(5u, 5u, 0u); a_star.setGoal(40u, 40u, 1u); nav2_smac_planner::NodeLattice::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // check path is the right size and collision free EXPECT_EQ(num_it, 22); @@ -278,10 +311,13 @@ TEST(AStarTest, test_se2_single_pose_path) int max_iterations = 100; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -296,13 +332,16 @@ TEST(AStarTest, test_se2_single_pose_path) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; // functional case testing a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one // With the current implementation, this produces a longer path @@ -312,6 +351,80 @@ TEST(AStarTest, test_se2_single_pose_path) delete costmapA; } +TEST(AStarTest, test_goal_heading_mode) +{ + auto lnode = std::make_shared("test"); + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates + info.retrospective_penalty = 0.015; + info.analytic_expansion_max_length = 20.0; // in grid coordinates + info.analytic_expansion_ratio = 3.5; + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + int terminal_checking_interval = 5000; + double max_planning_time = 120.0; + int num_it = 0; + + // BIDIRECTIONAL goal heading mode + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + + std::unique_ptr checker = + std::make_unique(costmap_ros, size_theta, lnode); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + std::unique_ptr>> expansions = nullptr; + expansions = std::make_unique>>(); + + auto dummy_cancel_checker = []() { + return false; + }; + + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + + EXPECT_TRUE(a_star.getGoals().size() == 2); + EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + + + // ALL_DIRECTION goal heading mode + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + + unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + EXPECT_TRUE(a_star.getGoals().size() == num_bins); + // first goal should be the same the one set by the user + EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + +} + TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown @@ -323,6 +436,15 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); + nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); + gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); + gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); + gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); + EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -331,4 +453,16 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); + + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); } diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 7ed61c4ec4..1c127c4799 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -374,3 +374,33 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) // should be empty since totally invalid EXPECT_EQ(neighbors.size(), 0u); } + +TEST(NodeHybridTest, basic_get_closest_angular_bin_test) +{ + // Tests to check getClosestAngularBin behavior for different input types + nav2_smac_planner::HybridMotionTable motion_table; + + { + motion_table.bin_size = 3.1415926; + double test_theta = 3.1415926; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } + + { + motion_table.bin_size = M_PI; + double test_theta = M_PI; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } + + { + motion_table.bin_size = M_PI; + float test_theta = M_PI; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } +} diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 1ebe215b30..e2dff5a56f 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -57,6 +57,10 @@ TEST(SmacTest, test_smac_2d) { node2D->declare_parameter("test.downsampling_factor", 2); node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -69,7 +73,7 @@ TEST(SmacTest, test_smac_2d) { planner_2d->configure(node2D, "test", nullptr, costmap_ros); planner_2d->activate(); try { - planner_2d->createPlan(start, goal); + planner_2d->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -108,6 +112,7 @@ TEST(SmacTest, test_smac_2d_reconfigure) { rclcpp::Parameter("test.downsampling_factor", 2), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.terminal_checking_interval", 100), rclcpp::Parameter("test.use_final_approach_orientation", false)}); rclcpp::spin_until_future_complete( @@ -127,6 +132,9 @@ TEST(SmacTest, test_smac_2d_reconfigure) { EXPECT_EQ( node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); + EXPECT_EQ( + node2D->get_parameter("test.terminal_checking_interval").as_int(), + 100); results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true)}); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index aae4666edc..db24daa661 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -56,6 +56,10 @@ TEST(SmacTest, test_smac_se2) nodeSE2->declare_parameter("test.downsampling_factor", 2); nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -68,7 +72,7 @@ TEST(SmacTest, test_smac_se2) planner->activate(); try { - planner->createPlan(start, goal); + planner->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -119,7 +123,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), - rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); + rclcpp::Parameter("test.terminal_checking_interval", 42), + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")), + rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))}); rclcpp::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -144,7 +150,11 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42); + EXPECT_EQ(nodeSE2->get_parameter("test.terminal_checking_interval").as_int(), 42); EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); + EXPECT_EQ( + nodeSE2->get_parameter("test.goal_heading_mode").as_string(), + std::string("BIDIRECTIONAL")); } diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index b791f9961c..128db09fdc 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -60,6 +60,10 @@ TEST(SmacTest, test_smac_lattice) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -76,7 +80,7 @@ TEST(SmacTest, test_smac_lattice) planner->activate(); try { - planner->createPlan(start, goal); + planner->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -128,7 +132,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), - rclcpp::Parameter("test.allow_reverse_expansion", true)}); + rclcpp::Parameter("test.terminal_checking_interval", 42), + rclcpp::Parameter("test.allow_reverse_expansion", true), + rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))}); try { // All of these params will re-init A* which will involve loading the control set file diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 3844923498..616c674595 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -89,11 +89,14 @@ TEST(SmootherTest, test_full_smoother) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; a_star.initialize( - false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); // Convert raw costmap into a costmap ros object auto costmap_ros = std::make_shared(); @@ -105,12 +108,16 @@ TEST(SmootherTest, test_full_smoother) std::make_unique(costmap_ros, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; + // Create A* search to smooth a_star.setCollisionChecker(checker.get()); a_star.setStart(5u, 5u, 0u); a_star.setGoal(45u, 45u, 36u); nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Convert to world coordinates and get length to compare to smoothed length nav_msgs::msg::Path plan; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 2674b65e74..bfab0a79fe 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 51558d9ba1..0a4b320d8f 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 @@ -17,13 +17,14 @@ #include #include #include +#include #include #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" @@ -31,6 +32,8 @@ #include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -58,58 +61,9 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_goal_updated_controller_bt_node" - }; + std::vector plugin_libs; + boost::split(plugin_libs, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); } @@ -270,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); } @@ -321,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); } @@ -370,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); } @@ -428,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); } @@ -522,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); } @@ -586,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/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 706767b9ce..1e6d57e0f4 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -136,6 +136,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( std::this_thread::sleep_for(5s); if (make_fake_costmap_) { + sendFakeCostmap(target_yaw); sendFakeOdom(0.0); } @@ -211,7 +212,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(1)); + rclcpp::sleep_for(std::chrono::milliseconds(3)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); @@ -277,7 +278,7 @@ void SpinBehaviorTester::sendFakeCostmap(float angle) nav2_msgs::msg::Costmap fake_costmap; fake_costmap.header.frame_id = "odom"; - fake_costmap.header.stamp = stamp_; + fake_costmap.header.stamp = node_->now(); fake_costmap.metadata.layer = "master"; fake_costmap.metadata.resolution = .1; fake_costmap.metadata.size_x = 100; @@ -288,9 +289,9 @@ void SpinBehaviorTester::sendFakeCostmap(float angle) float costmap_val = 0; for (int ix = 0; ix < 100; ix++) { for (int iy = 0; iy < 100; iy++) { - if (abs(angle) > M_PI_2f32) { + if (fabs(angle) > M_PI_2f32) { // fake obstacles in the way so we get failure due to potential collision - costmap_val = 100; + costmap_val = 253; } fake_costmap.data.push_back(costmap_val); } @@ -302,7 +303,7 @@ void SpinBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; - pose.header.stamp = stamp_; + pose.header.stamp = node_->now(); pose.pose.pose.position.x = -2.0; pose.pose.pose.position.y = -0.5; pose.pose.pose.position.z = 0.0; @@ -325,7 +326,7 @@ void SpinBehaviorTester::sendFakeOdom(float angle) { geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = stamp_; + transformStamped.header.stamp = node_->now(); transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "base_link"; transformStamped.transform.translation.x = 0.0; @@ -342,7 +343,7 @@ void SpinBehaviorTester::sendFakeOdom(float angle) geometry_msgs::msg::PolygonStamped footprint; footprint.header.frame_id = "odom"; - footprint.header.stamp = stamp_; + footprint.header.stamp = node_->now(); footprint.polygon.points.resize(4); footprint.polygon.points[0].x = 0.22; footprint.polygon.points[0].y = 0.22; diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index c9df3d3243..8edbcf8fe8 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -60,6 +60,8 @@ def generate_launch_description(): 'gzserver', '-s', 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', '--minimal_comms', world, ], diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index f16db47851..6838e01221 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -118,12 +118,12 @@ INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, SpinBehaviorTestFixture, ::testing::Values( - std::make_tuple(-M_PIf32 / 6.0, 0.15), - std::make_tuple(M_PI_4f32, 0.15), - std::make_tuple(-M_PI_2f32, 0.15), - std::make_tuple(M_PIf32, 0.10), + std::make_tuple(-M_PIf32 / 6.0, 0.1), + // std::make_tuple(M_PI_4f32, 0.1), + // std::make_tuple(-M_PI_2f32, 0.1), + std::make_tuple(M_PIf32, 0.1), std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), - std::make_tuple(-2.0 * M_PIf32, 0.15), + std::make_tuple(-2.0 * M_PIf32, 0.1), std::make_tuple(4.0 * M_PIf32, 0.15)), testNameGenerator); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 5c948331f0..9f9bdd33de 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -47,56 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index c887ac3ce5..10040d8798 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -47,57 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 8bfe04488d..b50108a13f 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -47,57 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 388e6f2cdb..7886fe6686 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -154,7 +154,7 @@ planner_server: expected_planner_frequency: 20.0 planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map", "start_occupied", "goal_occupied", "timeout","no_valid_path", - "no_viapoints_given" ] + "no_viapoints_given", "cancelled" ] unknown: plugin: "nav2_system_tests::UnknownErrorPlanner" tf_error: @@ -173,6 +173,8 @@ planner_server: plugin: "nav2_system_tests::NoValidPathErrorPlanner" no_viapoints_given: plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" + cancelled: + plugin: "nav2_system_tests::CancelledPlanner" smoother_server: ros__parameters: diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp index 90b18c5b11..1c2eecf23a 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp @@ -24,3 +24,4 @@ PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::Gl PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::CancelledPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 2b6ae7971d..77b23aecce 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -51,7 +51,8 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerException("Unknown Error"); } @@ -61,7 +62,8 @@ class StartOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOccupied("Start Occupied"); } @@ -71,7 +73,8 @@ class GoalOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOccupied("Goal occupied"); } @@ -81,7 +84,8 @@ class StartOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); } @@ -91,7 +95,8 @@ class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); } @@ -101,7 +106,8 @@ class NoValidPathErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { return nav_msgs::msg::Path(); } @@ -112,7 +118,8 @@ class TimedOutErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTimedOut("Planner Timed Out"); } @@ -122,7 +129,8 @@ class TFErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTFError("TF Error"); } @@ -132,12 +140,33 @@ class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::NoViapointsGiven("No Via points given"); } }; +class CancelledPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &, + std::function cancel_checker) override + { + auto start_time = std::chrono::steady_clock::now(); + while (rclcpp::ok() && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(5)) + { + if (cancel_checker()) { + throw nav2_core::PlannerCancelled("Planner Cancelled"); + } + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + throw nav2_core::PlannerException("Cancel is not called in time."); + } +}; + } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/error_codes/planner_plugins.xml b/nav2_system_tests/src/error_codes/planner_plugins.xml index 9d6f7f0545..ab10ae800b 100644 --- a/nav2_system_tests/src/error_codes/planner_plugins.xml +++ b/nav2_system_tests/src/error_codes/planner_plugins.xml @@ -39,4 +39,8 @@ base_class_type="nav2_core::GlobalPlanner"> This global planner produces a no via points exception. + + This global planner produces a cancelled exception. + diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index f023bbb27c..a5f2b84d97 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -14,6 +14,7 @@ # limitations under the License. import sys +import threading import time from geometry_msgs.msg import PoseStamped @@ -23,7 +24,7 @@ FollowPath, SmoothPath, ) -from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from nav_msgs.msg import Path import rclpy @@ -111,6 +112,17 @@ def main(argv=sys.argv[1:]): result.error_code == error_code ), 'Compute path to pose error does not match' + def cancel_task(): + time.sleep(1) + navigator.goal_handle.cancel_goal_async() + + # Check compute path to pose cancel + threading.Thread(target=cancel_task).start() + result = navigator._getPathImpl(initial_pose, goal_pose, 'cancelled') + assert ( + navigator.getResult() == TaskResult.CANCELED + ), 'Compute path to pose cancel failed' + # Check compute path through error codes goal_pose1 = goal_pose goal_poses = [goal_pose, goal_pose1] @@ -133,6 +145,12 @@ def main(argv=sys.argv[1:]): assert ( result.error_code == error_code ), 'Compute path through pose error does not match' + # Check compute path to pose cancel + threading.Thread(target=cancel_task).start() + result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, 'cancelled') + assert ( + navigator.getResult() == TaskResult.CANCELED + ), 'Compute path through poses cancel failed' # Check compute path to pose error codes pose = PoseStamped() diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index d11ef6ab53..6e9f3f490b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -15,60 +15,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 7a5f8d4b7f..421a00f4b7 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -87,7 +87,8 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer return false; } try { - path = planners_["GridBased"]->createPlan(start, goal); + auto dummy_cancel_checker = []() {return false;}; + path = planners_["GridBased"]->createPlan(start, goal, dummy_cancel_checker); // The situation when createPlan() did not throw any exception // does not guarantee that plan was created correctly. // So it should be checked additionally that path is correct. diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 906e8f44d3..01d264006a 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -57,9 +57,11 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; + auto dummy_cancel_checker = []() {return false;}; + // Test without use_final_approach_orientation // expecting end path pose orientation to be equal to goal orientation - auto path = obj->getPlan(start, goal, "GridBased"); + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); // obj->onCleanup(state); @@ -93,7 +95,9 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; - auto path = obj->getPlan(start, goal, "GridBased"); + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); int path_size = path.poses.size(); @@ -114,6 +118,36 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) obj.reset(); } +void testCancel(std::string plugin) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->declare_parameter("GridBased.terminal_checking_interval", rclcpp::ParameterValue(1)); + obj->onConfigure(state); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = 0.6; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + + auto always_cancelled = []() {return true;}; + + EXPECT_THROW( + obj->getPlan(start, goal, "GridBased", always_cancelled), + nav2_core::PlannerCancelled); + // obj->onCleanup(state); + obj.reset(); +} + TEST(testPluginMap, Failures) { auto obj = std::make_shared(); @@ -127,11 +161,14 @@ TEST(testPluginMap, Failures) geometry_msgs::msg::PoseStamped goal; std::string plugin_fake = "fake"; std::string plugin_none = ""; - auto path = obj->getPlan(start, goal, plugin_none); + + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, plugin_none, dummy_cancel_checker); EXPECT_EQ(path.header.frame_id, std::string("map")); try { - path = obj->getPlan(start, goal, plugin_fake); + path = obj->getPlan(start, goal, plugin_fake, dummy_cancel_checker); FAIL() << "Failed to throw invalid planner id exception"; } catch (const nav2_core::InvalidPlanner & ex) { EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid")); @@ -140,6 +177,7 @@ TEST(testPluginMap, Failures) obj->onCleanup(state); } + TEST(testPluginMap, Smac2dEqualStartGoal) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); @@ -290,6 +328,32 @@ TEST(testPluginMap, ThetaStarAboveCostmapResolutionN) testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); } +TEST(testPluginMap, NavFnCancel) +{ + testCancel("nav2_navfn_planner/NavfnPlanner"); +} + +TEST(testPluginMap, ThetaStarCancel) +{ + testCancel("nav2_theta_star_planner/ThetaStarPlanner"); +} + +TEST(testPluginMap, Smac2dCancel) +{ + testCancel("nav2_smac_planner/SmacPlanner2D"); +} + +TEST(testPluginMap, SmacLatticeCancel) +{ + testCancel("nav2_smac_planner/SmacPlannerLattice"); +} + +TEST(testPluginMap, SmacHybridAStarCancel) +{ + testCancel("nav2_smac_planner/SmacPlannerHybrid"); +} + + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index a58a4f87ed..1c819a882e 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -76,6 +76,8 @@ class ThetaStar bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; + /// the interval at which the planner checks if it has been cancelled + int terminal_checking_interval_; ThetaStar(); @@ -87,7 +89,7 @@ class ThetaStar * @param raw_path is used to return the path obtained by executing the algorithm * @return true if a path is found, false if no path is found in between the start and goal pose */ - bool generatePath(std::vector & raw_path); + bool generatePath(std::vector & raw_path, std::function cancel_checker); /** * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index bcbff2ae7c..09f8714219 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -54,9 +54,17 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner void deactivate() override; + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled + * @return nav2_msgs::Path of the generated path + */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: std::shared_ptr tf_; @@ -76,9 +84,10 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner /** * @brief the function responsible for calling the algorithm and retrieving a path from it + * @param cancel_checker is a function to check if the action has been canceled * @return global_path is the planned path to be taken */ - void getPlan(nav_msgs::msg::Path & global_path); + void getPlan(nav_msgs::msg::Path & global_path, std::function cancel_checker); /** * @brief interpolates points between the consecutive waypoints of the path diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index f83b745b22..ba3629bcbf 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include "nav2_core/planner_exceptions.hpp" #include "nav2_theta_star_planner/theta_star.hpp" namespace theta_star @@ -26,6 +27,7 @@ ThetaStar::ThetaStar() allow_unknown_(true), size_x_(0), size_y_(0), + terminal_checking_interval_(5000), index_generated_(0) { exp_node = new tree_node; @@ -43,7 +45,7 @@ void ThetaStar::setStartAndGoal( dst_ = {static_cast(d[0]), static_cast(d[1])}; } -bool ThetaStar::generatePath(std::vector & raw_path) +bool ThetaStar::generatePath(std::vector & raw_path, std::function cancel_checker) { resetContainers(); addToNodesData(index_generated_); @@ -60,6 +62,11 @@ bool ThetaStar::generatePath(std::vector & raw_path) while (!queue_.empty()) { nodes_opened++; + if (nodes_opened % terminal_checking_interval_ == 0 && cancel_checker()) { + clearQueue(); + throw nav2_core::PlannerCancelled("Planner was canceled"); + } + if (isGoal(*curr_data)) { break; } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index ca9391111b..61064efbf4 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -59,6 +59,10 @@ void ThetaStarPlanner::configure( planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0; + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name_ + ".terminal_checking_interval", planner_->terminal_checking_interval_); + nav2_util::declare_parameter_if_not_declared( node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); @@ -86,7 +90,8 @@ void ThetaStarPlanner::deactivate() nav_msgs::msg::Path ThetaStarPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); @@ -140,7 +145,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); - getPlan(global_path); + getPlan(global_path, cancel_checker); // check if a plan is generated size_t plan_size = global_path.poses.size(); if (plan_size > 0) { @@ -173,13 +178,15 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } -void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +void ThetaStarPlanner::getPlan( + nav_msgs::msg::Path & global_path, + std::function cancel_checker) { std::vector path; if (planner_->isUnsafeToPlan()) { global_path.poses.clear(); throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); - } else if (planner_->generatePath(path)) { + } else if (planner_->generatePath(path, cancel_checker)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { global_path.poses.clear(); @@ -229,6 +236,9 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param if (name == name_ + ".how_many_corners") { planner_->how_many_corners_ = parameter.as_int(); } + if (name == name_ + ".terminal_checking_interval") { + planner_->terminal_checking_interval_ = parameter.as_int(); + } } else if (type == ParameterType::PARAMETER_DOUBLE) { if (name == name_ + ".w_euc_cost") { planner_->w_euc_cost_ = parameter.as_double(); diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 0476290f93..a8db0ddbfe 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -60,10 +60,12 @@ class test_theta_star : public theta_star::ThetaStar void uresetContainers() {nodes_data_.clear(); resetContainers();} - bool runAlgo(std::vector & path) + bool runAlgo( + std::vector & path, + std::function cancel_checker = [] () {return false;}) { if (!isUnsafeToPlan()) { - return generatePath(path); + return generatePath(path, cancel_checker); } return false; } @@ -162,7 +164,11 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { planner_2d->configure(life_node, "test", nullptr, costmap_ros); planner_2d->activate(); - nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + auto dummy_cancel_checker = []() { + return false; + }; + + nav_msgs::msg::Path path = planner_2d->createPlan(start, goal, dummy_cancel_checker); EXPECT_GT(static_cast(path.poses.size()), 0); // test if the goal is unsafe @@ -174,7 +180,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); + EXPECT_THROW(planner_2d->createPlan(start, goal, dummy_cancel_checker), nav2_core::GoalOccupied); planner_2d->deactivate(); planner_2d->cleanup(); @@ -212,7 +218,8 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), rclcpp::Parameter("test.use_final_approach_orientation", false), - rclcpp::Parameter("test.allow_unknown", false)}); + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.terminal_checking_interval", 100)}); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), @@ -225,6 +232,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.terminal_checking_interval").as_int(), 100); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 71828321f0..eec9c2dd9f 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -574,7 +574,7 @@ class SimpleActionServer * @param the Results object to terminate the action with */ void terminate( - std::shared_ptr> handle, + std::shared_ptr> & handle, typename std::shared_ptr result = std::make_shared()) { diff --git a/tools/bt2img.py b/tools/bt2img.py index b3c2b34eaf..2b12d777d8 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -27,7 +27,7 @@ 'ReactiveFallback', 'ReactiveSequence', 'Sequence', - 'SequenceStar', + 'SequenceWithMemory', 'BlackboardCheckInt', 'BlackboardCheckDouble', 'BlackboardCheckString',