From 870a98abbab42192396ec4ad787eca29b5bcad68 Mon Sep 17 00:00:00 2001 From: mini-1235 <58433591+mini-1235@users.noreply.github.com> Date: Tue, 4 Feb 2025 04:53:12 +0800 Subject: [PATCH] Fix unstable test in nav2 util (#4894) * Fix unstable test in nav2 util Signed-off-by: mini-1235 * Fix linting Signed-off-by: mini-1235 * Change 5s to 1s Signed-off-by: mini-1235 --------- Signed-off-by: mini-1235 --- nav2_util/include/nav2_util/simple_action_server.hpp | 3 +++ nav2_util/test/test_actions.cpp | 6 ++++++ 2 files changed, 9 insertions(+) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 8de024ab05f..b4b1ef73dd6 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -150,6 +150,9 @@ class SimpleActionServer std::lock_guard lock(update_mutex_); if (!server_active_) { + RCLCPP_INFO( + node_logging_interface_->get_logger(), + "Action server is inactive. Rejecting the goal."); return rclcpp_action::GoalResponse::REJECT; } diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 5ecd95d60b9..0ae2ffe0c0e 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -258,6 +258,7 @@ class ActionTest : public ::testing::Test TEST_F(ActionTest, test_simple_action) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); node_->activate_server(); // The goal for this invocation @@ -272,6 +273,7 @@ TEST_F(ActionTest, test_simple_action) future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -320,6 +322,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -364,6 +367,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) node_->deactivate_server(); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -430,6 +434,7 @@ TEST_F(ActionTest, test_simple_action_preemption) future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result auto future_result = node_->action_client_->async_get_result(goal_handle); @@ -478,6 +483,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) // Get the results auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); // Wait for the result of initial goal auto future_result = node_->action_client_->async_get_result(goal_handle);