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 68bd5d79dc2f1aac9ff6a8a156976554319c3cdc..6a847a7bc5429c1f6015c1420c69f5576adab4dd 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 @@ -141,7 +141,12 @@ TEST_F(BackUpActionTestFixture, test_tick) tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1); auto goal = action_server_->getCurrentGoal(); 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 d4a3a54f4d9c77cd6378fd7ec9c8658572df1cf7..d5d1cd6621ad625d9e8399d0371fb3b980ff98fb 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 @@ -128,17 +128,17 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); - EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); - // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + // check if returned path is correct nav_msgs::msg::Path path; config_->blackboard->get<nav_msgs::msg::Path>("path", path); @@ -153,13 +153,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) goal.pose.position.x = -2.5; config_->blackboard->set("goal", goal); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); + config_->blackboard->get<nav_msgs::msg::Path>("path", path); EXPECT_EQ(path.poses.size(), 1u); EXPECT_EQ(path.poses[0].pose.position.x, -2.5); 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 7ca9f65876625a4f25e14910880626816a17137b..9c3c67fe4e7468924f5508747ba220e3a03772a5 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 @@ -126,18 +126,17 @@ TEST_F(FollowPathActionTestFixture, test_tick) path.poses[0].pose.position.x = 1.0; config_->blackboard->set<nav_msgs::msg::Path>("path", path); - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(tree_->rootNode()->getInput<std::string>("controller_id"), std::string("FollowPath")); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); - EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); - // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + + // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput<std::string>("controller_id"), std::string("FollowPath")); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -147,13 +146,13 @@ TEST_F(FollowPathActionTestFixture, test_tick) path.poses[0].pose.position.x = -2.5; config_->blackboard->set<nav_msgs::msg::Path>("path", path); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); } int main(int argc, char ** argv) 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 b36eca9cb1e2320b02f56b976f8ba74c6c5f6b1c..7ea3e858f8dfedfe8f9a0050a46f03821ee12eb8 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 @@ -123,15 +123,14 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) geometry_msgs::msg::PoseStamped pose; - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); - // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + + // goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -142,13 +141,11 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) pose.pose.orientation.x = 1.0; config_->blackboard->set<geometry_msgs::msg::PoseStamped>("goal", pose); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); } 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 7bc1166e6734aeba5746d5e09e004217f6870a50..f3d0b3d913d6f267444a258bbc52e0b1de414e9a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -139,7 +139,12 @@ TEST_F(SpinActionTestFixture, test_tick) tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1); EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f); } 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 e6eea62a3040a69fb82a4578a96f9999a62f1c66..193395f940173bd37c8442dcf2c1331c27f93355 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -140,7 +140,12 @@ TEST_F(WaitActionTestFixture, test_tick) tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1); EXPECT_EQ(rclcpp::Duration(action_server_->getCurrentGoal()->time).seconds(), 5.0); }