From 970f5b529d43d9940f75599f5f3c4fd90f7751de Mon Sep 17 00:00:00 2001
From: Sarthak Mittal <sarthakmittal2608@gmail.com>
Date: Tue, 29 Sep 2020 21:11:54 +0530
Subject: [PATCH] Fix flaky nav2_behavior_tree tests (#2001)
Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
---
.../plugins/action/test_back_up_action.cpp | 7 ++++++-
.../test_compute_path_to_pose_action.cpp | 18 ++++++++--------
.../action/test_follow_path_action.cpp | 21 +++++++++----------
.../action/test_navigate_to_pose_action.cpp | 11 ++++------
.../test/plugins/action/test_spin_action.cpp | 7 ++++++-
.../test/plugins/action/test_wait_action.cpp | 7 ++++++-
6 files changed, 41 insertions(+), 30 deletions(-)
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 68bd5d79..6a847a7b 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 d4a3a54f..d5d1cd66 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 7ca9f658..9c3c67fe 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 b36eca9c..7ea3e858 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 7bc1166e..f3d0b3d9 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 e6eea62a..193395f9 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);
}
--
GitLab