Skip to content

Commit

Permalink
nav2_system_tests behavior_tree result handling improvement (#4341)
Browse files Browse the repository at this point in the history 
NOTE: This stuff does not really test the error_msg/error_code
handling as its all dummy servers.

Signed-off-by: Mike Wake <[email protected]>
  • Loading branch information
@ewak
ewak committed Dec 21, 2024
1 parent d0afe88 commit f86dca5
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 55 deletions.
20 changes: 11 additions & 9 deletions nav2_system_tests/src/behavior_tree/dummy_action_server.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class DummyActionServer
const rclcpp::Node::SharedPtr & node,
std::string action_name)
: action_name_(action_name),
goal_count_(0)
goal_count_(0),
result_(std::make_shared<typename ActionT::Result>())
{
action_server_ = rclcpp_action::create_server<ActionT>(
node->get_node_base_interface(),
Expand Down Expand Up @@ -71,19 +72,20 @@ class DummyActionServer
failure_ranges_.clear();
running_ranges_.clear();
goal_count_ = 0;
updateResultForSuccess(result_);
}

int getGoalCount() const
{
return goal_count_;
}

protected:
virtual std::shared_ptr<typename ActionT::Result> fillResult()
std::shared_ptr<typename ActionT::Result> getResult(void)
{
return std::make_shared<typename ActionT::Result>();
return result_;
}

protected:
virtual void updateResultForFailure(std::shared_ptr<typename ActionT::Result> & result)
{
result->error_code = ActionT::Result::UNKNOWN;
Expand Down Expand Up @@ -113,7 +115,6 @@ class DummyActionServer
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)
{
goal_count_++;
auto result = fillResult();

// if current goal index exists in running range, the thread sleeps for 1 second
// to simulate a long running action
Expand All @@ -127,15 +128,15 @@ class DummyActionServer
// if current goal index exists in failure range, the goal will be aborted
for (auto & index : failure_ranges_) {
if (goal_count_ >= index.first && goal_count_ <= index.second) {
updateResultForFailure(result);
goal_handle->abort(result);
updateResultForFailure(result_);
goal_handle->abort(result_);
return;
}
}

// goal succeeds for all other indices
updateResultForSuccess(result);
goal_handle->succeed(result);
updateResultForSuccess(result_);
goal_handle->succeed(result_);
}

void handle_accepted(
Expand All @@ -157,6 +158,7 @@ class DummyActionServer
Ranges running_ranges_;

unsigned int goal_count_;
std::shared_ptr<typename ActionT::Result> result_;
};
} // namespace nav2_system_tests

Expand Down
28 changes: 6 additions & 22 deletions nav2_system_tests/src/behavior_tree/server_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,26 +49,24 @@ class DummyComputePathToPoseActionServer
explicit DummyComputePathToPoseActionServer(const rclcpp::Node::SharedPtr & node)
: DummyActionServer(node, "compute_path_to_pose")
{
result_ = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
geometry_msgs::msg::PoseStamped pose;
pose.header = result_->path.header;
pose.header.stamp = node->get_clock()->now();
pose.header.frame_id = "map";
pose.pose.position.x = 0.0;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;

result_->path.header.stamp = node->now();
result_->path.header.frame_id = pose.header.frame_id;
for (int i = 0; i < 6; ++i) {
result_->path.poses.push_back(pose);
}
}

std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> fillResult() override
{
return result_;
}

protected:
void updateResultForFailure(
std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result>
Expand All @@ -77,24 +75,13 @@ class DummyComputePathToPoseActionServer
result->error_code = nav2_msgs::action::ComputePathToPose::Result::TIMEOUT;
result->error_msg = "Timeout";
}

private:
std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> result_;
};

class DummyFollowPathActionServer : public DummyActionServer<nav2_msgs::action::FollowPath>
{
public:
explicit DummyFollowPathActionServer(const rclcpp::Node::SharedPtr & node)
: DummyActionServer(node, "follow_path")
{
result_ = std::make_shared<nav2_msgs::action::FollowPath::Result>();
}

std::shared_ptr<nav2_msgs::action::FollowPath::Result> fillResult() override
{
return result_;
}
: DummyActionServer(node, "follow_path") {}

protected:
void updateResultForFailure(
Expand All @@ -104,9 +91,6 @@ class DummyFollowPathActionServer : public DummyActionServer<nav2_msgs::action::
result->error_code = nav2_msgs::action::FollowPath::Result::NO_VALID_CONTROL;
result->error_msg = "No valid control";
}

private:
std::shared_ptr<nav2_msgs::action::FollowPath::Result> result_;
};


Expand Down
48 changes: 24 additions & 24 deletions nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -234,12 +234,12 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess)

// Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 0);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "");
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "");

EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 1);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "");

// Goal count should be 0 since no goal is sent to all other servers
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
Expand Down Expand Up @@ -290,13 +290,13 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure)

// Goal count should be 2 since only two goals are sent to ComputePathToPose
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 14);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 207);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "Timeout");
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 207);
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "Timeout");

// Goal count should be 0 since no goal is sent to FollowPath action server
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 0);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "");

EXPECT_EQ(server_handler->spin_server->getGoalCount(), 5);
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 5);
Expand Down Expand Up @@ -343,12 +343,12 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries)

// Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 2);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 207);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "Timeout");
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "");

EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 2);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 106);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "No valid control");
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "");

// Navigate subtree recovery services are called once each
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 1);
Expand Down Expand Up @@ -406,13 +406,13 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple)

// FollowPath is called 4 times
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 106);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "No valid control");
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "");

// ComputePathToPose is called 3 times
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 207);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "Timeout");
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "");

// Local costmap is cleared 3 times
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3);
Expand Down Expand Up @@ -504,13 +504,13 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)

// ComputePathToPose is called 12 times
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 7);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 207);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "Timeout");
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "");

// FollowPath is called 4 times
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 14);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 106);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "No valid control");
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 106);
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "No valid control");

// Local costmap is cleared 5 times
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 9);
Expand Down Expand Up @@ -587,13 +587,13 @@ TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated)

// ComputePathToPose is called 4 times
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 207);
EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "Timeout");
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->compute_path_to_pose_server->getResult()->error_msg, "");

// FollowPath is called 3 times
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 5);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 106);
EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "No valid control");
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_code, 0);
EXPECT_EQ(server_handler->follow_path_server->getResult()->error_msg, "");

// Local costmap is cleared 2 times
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3);
Expand Down

0 comments on commit f86dca5

Please sign in to comment.