Fix unstable test in nav2 util (#4894)

* Fix unstable test in nav2 util

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Fix linting

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Change 5s to 1s

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Author: mini-1235

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -150,6 +150,9 @@ class SimpleActionServer
     std::lock_guard<std::recursive_mutex> 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;
     }
 

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);