From 42dcd13fcede7fe8faf22b1b261a456fd5ad18af Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 19 Jan 2024 14:19:07 +0100 Subject: [PATCH] behavior_tree: support both ports and blackboard Signed-off-by: Davide Faconti --- .../behavior_tree_engine.hpp | 2 +- .../include/nav2_behavior_tree/bt_utils.hpp | 52 ++++++++++++++++--- .../globally_updated_goal_condition.hpp | 6 ++- .../condition/goal_updated_condition.hpp | 5 +- .../initial_pose_received_condition.hpp | 18 ++++++- .../decorator/goal_updated_controller.hpp | 5 +- .../plugins/decorator/speed_controller.hpp | 2 + .../action/remove_passed_goals_action.cpp | 2 +- .../condition/distance_traveled_condition.cpp | 4 +- .../globally_updated_goal_condition.cpp | 9 ++-- .../condition/goal_reached_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 9 ++-- .../initial_pose_received_condition.cpp | 16 ++++-- .../plugins/decorator/distance_controller.cpp | 4 +- .../decorator/goal_updated_controller.cpp | 10 ++-- .../plugins/decorator/speed_controller.cpp | 9 ++-- .../src/behavior_tree_engine.cpp | 2 +- ...test_compute_path_through_poses_action.cpp | 12 ++--- .../test_compute_path_to_pose_action.cpp | 12 ++--- .../action/test_controller_selector_node.cpp | 8 +-- .../action/test_follow_path_action.cpp | 2 +- .../test_goal_checker_selector_node.cpp | 8 +-- .../test_navigate_through_poses_action.cpp | 2 +- .../action/test_navigate_to_pose_action.cpp | 2 +- .../action/test_planner_selector_node.cpp | 8 +-- .../test_remove_passed_goals_action.cpp | 2 +- .../action/test_smooth_path_action.cpp | 2 +- .../action/test_smoother_selector_node.cpp | 8 +-- .../action/test_truncate_path_action.cpp | 2 +- .../test_truncate_path_local_action.cpp | 18 +++---- .../condition/test_initial_pose_received.cpp | 30 +++-------- .../decorator/test_goal_updater_node.cpp | 6 +-- nav2_behavior_tree/test/test_bt_utils.cpp | 4 +- 33 files changed, 169 insertions(+), 114 deletions(-) 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 daf609d067c..0acd765e28b 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 @@ -23,7 +23,7 @@ #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_v3/bt_factory.h" #include "rclcpp/rclcpp.hpp" 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 c0425d1711f..ddcec8f4df9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -135,17 +135,24 @@ inline std::set convertFromString(StringView key) * @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 + * @param behavior_tree_node the node + * @return */ -template -T1 deconflictPortAndParamFrame( +template +T deconflictPortAndParamFrame( rclcpp::Node::SharedPtr node, std::string param_name, - T2 * behavior_tree_node) + const BT::TreeNode * behavior_tree_node) { - T1 param_value; - if (!behavior_tree_node->getInput(param_name, param_value)) { + T param_value; + bool param_from_input = behavior_tree_node->getInput(param_name, param_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 +BT::Result getInputPortOrBlackboard( + const BT::TreeNode & bt_node, + const BT::Blackboard & blackboard, + const std::string & param_name, + T & value) +{ + if (auto res = bt_node.getInput(param_name, value)) { + return res; + } + if (auto res = blackboard.get(param_name, value)) { + return {}; + } + return nonstd::make_unexpected(StrCat(param_name, " not found")); +} + +// 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/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 64f77f1473d..e0f64e854f7 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 @@ -57,7 +57,11 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return + { + BT::InputPort>("goals"), + BT::InputPort("goal"), + }; } private: 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 1e8c9712c16..500c6ee9977 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 @@ -54,7 +54,10 @@ class GoalUpdatedCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>("goals"), + BT::InputPort("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 2bb7d995b80..f98d7664ecf 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 @@ -23,7 +23,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; +}; + +} // end namespace #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ 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 4ac0ab44eeb..ffbd7bcce0b 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 @@ -48,7 +48,10 @@ class GoalUpdatedController : public BT::DecoratorNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>("goals"), + BT::InputPort("goal"), + }; } private: 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 8150eb31098..9b1cb975d86 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 @@ -57,6 +57,8 @@ 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"), + BT::InputPort("goal"), }; } 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 bf8b95c84f6..fd255f78c54 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); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 64394ee3782..167caec938c 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; } 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 fee838c9379..61c45696f57 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; diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 2697680415b..8609ab55902 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); } diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 34930bb582e..f2f112c348e 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 { @@ -28,15 +29,15 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { if (status() == BT::NodeStatus::IDLE) { - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + 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; 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 41796834c63..0a74ce68c95 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -13,13 +13,21 @@ // 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; } @@ -28,7 +36,5 @@ BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node) #include "behaviortree_cpp_v3/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/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index b59fba77b45..3cef0ea978c 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -43,9 +43,9 @@ 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); } diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index f8a2d8cefc2..049bc61f7ec 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -19,7 +19,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" - +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -37,8 +37,8 @@ BT::NodeStatus GoalUpdatedController::tick() // 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; diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index f47dfed38a8..8d160aa76e1 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 { @@ -61,17 +62,17 @@ inline BT::NodeStatus SpeedController::tick() if (status() == BT::NodeStatus::IDLE) { // 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 diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 429368ea96a..33100d76bb3 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -48,7 +48,7 @@ BehaviorTreeEngine::run( try { while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { - tree->rootNode()->halt(); + tree->haltTree(); return BtStatus::CANCELED; } 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 ec619b01205..60adb8065ac 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 @@ -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); @@ -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 d726186cd31..9003e928d73 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 @@ -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); @@ -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_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 73a1510a1e2..0634a89e949 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 @@ -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); } @@ -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_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index c4fcc575193..878007357e4 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 @@ -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 e797dbd4cfd..5341fc89c6a 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 @@ -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); } @@ -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 5fa2c9aa036..4346c821be2 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 @@ -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 264b775a680..8b8b4c28b21 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 @@ -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 8c76e1440db..f0228e09029 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 @@ -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); } @@ -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_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 9e1aa32f8ea..2efa65d8f20 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 @@ -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 c2e43890212..7ec01269591 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 @@ -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 d3516aba199..5de5abea5dd 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 @@ -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); } @@ -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_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 5540b7b645c..b0c43368bf5 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 @@ -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 00564d4e8fb..0559d6effe3 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 @@ -138,7 +138,7 @@ TEST_F(TruncatePathLocalTestFixture, 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_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); @@ -233,7 +233,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) tree_->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); @@ -276,7 +276,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) tree_->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(); @@ -317,7 +317,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) tree_->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(); @@ -362,7 +362,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); @@ -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/condition/test_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp index 1f9aa41bb84..7b8a67529ed 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/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index cc6db706f24..26d585a8d2b 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 @@ -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) @@ -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); @@ -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/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 076b9c3b7c4..238dee27dcd 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -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);