Increase rclcpp_action test coverage (#1153)

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -59,9 +59,7 @@ create_client(
         return;
       }
       auto shared_node = weak_node.lock();
-      if (!shared_node) {
+      if (shared_node) {
-        return;
-      }
         // API expects a shared pointer, give it one with a deleter that does nothing.
         std::shared_ptr<Client<ActionT>> fake_shared_ptr(ptr, [](Client<ActionT> *) {});
 
@@ -69,12 +67,13 @@ create_client(
           // Was added to default group
           shared_node->remove_waitable(fake_shared_ptr, nullptr);
         } else {
-        // Was added to a specfic group
+          // Was added to a specific group
           auto shared_group = weak_group.lock();
           if (shared_group) {
             shared_node->remove_waitable(fake_shared_ptr, shared_group);
           }
         }
+      }
       delete ptr;
     };
 

rclcpp_action/include/rclcpp_action/create_server.hpp

@@ -78,9 +78,7 @@ create_server(
         return;
       }
       auto shared_node = weak_node.lock();
-      if (!shared_node) {
+      if (shared_node) {
-        return;
-      }
         // API expects a shared pointer, give it one with a deleter that does nothing.
         std::shared_ptr<Server<ActionT>> fake_shared_ptr(ptr, [](Server<ActionT> *) {});
 
@@ -88,12 +86,13 @@ create_server(
           // Was added to default group
           shared_node->remove_waitable(fake_shared_ptr, nullptr);
         } else {
-        // Was added to a specfic group
+          // Was added to a specific group
           auto shared_group = weak_group.lock();
           if (shared_group) {
             shared_node->remove_waitable(fake_shared_ptr, shared_group);
           }
         }
+      }
       delete ptr;
     };
 

rclcpp_action/test/test_client.cpp

@@ -32,6 +32,7 @@
 
 #include <test_msgs/action/fibonacci.hpp>
 
+#include <future>
 #include <map>
 #include <memory>
 #include <string>
@@ -73,7 +74,7 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
-  void SetUp()
+  void SetUpServer()
   {
     rcl_allocator_t allocator = rcl_get_default_allocator();
 
@@ -211,7 +212,10 @@ protected:
     ASSERT_TRUE(status_publisher != nullptr);
     allocator.deallocate(status_topic_name, allocator.state);
     server_executor.add_node(server_node);
+  }
 
+  void SetUp() override
+  {
     client_node = std::make_shared<rclcpp::Node>(client_node_name, namespace_name);
     client_executor.add_node(client_node);
 
@@ -219,7 +223,12 @@ protected:
     ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1)));
   }
 
-  void TearDown()
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void TearDownServer()
   {
     status_publisher.reset();
     feedback_publisher.reset();
@@ -227,6 +236,10 @@ protected:
     result_service.reset();
     goal_service.reset();
     server_node.reset();
+  }
+
+  void TearDown() override
+  {
     client_node.reset();
   }
 
@@ -265,11 +278,37 @@ protected:
   typename rclcpp::Publisher<ActionStatusMessage>::SharedPtr status_publisher;
 };
 
+class TestClientAgainstServer : public TestClient
+{
+protected:
+  void SetUp() override
+  {
+    SetUpServer();
+    TestClient::SetUp();
+  }
+
+  void TearDown() override
+  {
+    TestClient::TearDown();
+    TearDownServer();
+  }
+};
+
+
 TEST_F(TestClient, construction_and_destruction)
 {
   ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
 }
 
+TEST_F(TestClient, construction_and_destruction_after_node)
+{
+  ASSERT_NO_THROW(
+  {
+    auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+    client_node.reset();
+  });
+}
+
 TEST_F(TestClient, construction_and_destruction_callback_group)
 {
   auto group = client_node->create_callback_group(
@@ -285,7 +324,24 @@ TEST_F(TestClient, construction_and_destruction_callback_group)
     ).reset());
 }
 
-TEST_F(TestClient, async_send_goal_no_callbacks)
+TEST_F(TestClient, wait_for_action_server)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  EXPECT_FALSE(action_client->wait_for_action_server(0ms));
+  EXPECT_FALSE(action_client->wait_for_action_server(100ms));
+  auto future = std::async(
+    std::launch::async, [&action_client]() {
+      return action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT);
+    });
+  SetUpServer();
+  EXPECT_TRUE(future.get());
+  TearDownServer();
+
+  client_node.reset();  // Drop node before action client
+  EXPECT_THROW(action_client->wait_for_action_server(0ms), rclcpp::exceptions::InvalidNodeError);
+}
+
+TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -306,7 +362,24 @@ TEST_F(TestClient, async_send_goal_no_callbacks)
   EXPECT_FALSE(goal_handle->is_result_aware());
 }
 
-TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result)
+TEST_F(TestClientAgainstServer, bad_goal_handles)
+{
+  auto action_client0 = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client0->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 0;
+  auto future_goal_handle = action_client0->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+
+  auto action_client1 = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  using rclcpp_action::exceptions::UnknownGoalHandleError;
+  EXPECT_THROW(action_client1->async_get_result(goal_handle), UnknownGoalHandleError);
+  EXPECT_THROW(action_client1->async_cancel_goal(goal_handle), UnknownGoalHandleError);
+}
+
+TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -329,13 +402,33 @@ TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result)
   EXPECT_EQ(5, wrapped_result.result->sequence[5]);
 }
 
-TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result)
+TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal goal;
-  goal.order = 4;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  ASSERT_NE(nullptr, goal_handle);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  auto future_result = action_client->async_get_result(goal_handle);
+  EXPECT_TRUE(goal_handle->is_result_aware());
+
+  action_client.reset();  // Ensure goal handle is invalidated once client goes out of scope
+
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status());
+  using rclcpp_action::exceptions::UnawareGoalHandleError;
+  EXPECT_THROW(future_result.get(), UnawareGoalHandleError);
+}
+
+TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
   bool goal_response_received = false;
   auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
   send_goal_ops.goal_response_callback =
@@ -347,6 +440,20 @@ TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result)
         goal_response_received = true;
       }
     };
+
+  {
+    ActionGoal bad_goal;
+    bad_goal.order = -1;
+    auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_FALSE(goal_response_received);
+    EXPECT_EQ(nullptr, goal_handle);
+  }
+
+  {
+    ActionGoal goal;
+    goal.order = 4;
     auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
     dual_spin_until_future_complete(future_goal_handle);
     auto goal_handle = future_goal_handle.get();
@@ -358,12 +465,12 @@ TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result)
     EXPECT_TRUE(goal_handle->is_result_aware());
     dual_spin_until_future_complete(future_result);
     auto wrapped_result = future_result.get();
-
     ASSERT_EQ(5u, wrapped_result.result->sequence.size());
     EXPECT_EQ(3, wrapped_result.result->sequence.back());
   }
+}
 
-TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result)
+TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -397,7 +504,7 @@ TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result)
   EXPECT_EQ(5, feedback_count);
 }
 
-TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result)
+TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -432,7 +539,7 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result)
   EXPECT_EQ(3, wrapped_result.result->sequence.back());
 }
 
-TEST_F(TestClient, async_get_result_with_callback)
+TEST_F(TestClientAgainstServer, async_get_result_with_callback)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -467,7 +574,7 @@ TEST_F(TestClient, async_get_result_with_callback)
   EXPECT_EQ(3, wrapped_result.result->sequence.back());
 }
 
-TEST_F(TestClient, async_cancel_one_goal)
+TEST_F(TestClientAgainstServer, async_cancel_one_goal)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -485,7 +592,7 @@ TEST_F(TestClient, async_cancel_one_goal)
   EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
 }
 
-TEST_F(TestClient, async_cancel_one_goal_with_callback)
+TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -519,7 +626,7 @@ TEST_F(TestClient, async_cancel_one_goal_with_callback)
   EXPECT_TRUE(cancel_response_received);
 }
 
-TEST_F(TestClient, async_cancel_all_goals)
+TEST_F(TestClientAgainstServer, async_cancel_all_goals)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -555,7 +662,7 @@ TEST_F(TestClient, async_cancel_all_goals)
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
 }
 
-TEST_F(TestClient, async_cancel_all_goals_with_callback)
+TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -605,7 +712,7 @@ TEST_F(TestClient, async_cancel_all_goals_with_callback)
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
 }
 
-TEST_F(TestClient, async_cancel_some_goals)
+TEST_F(TestClientAgainstServer, async_cancel_some_goals)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@@ -636,7 +743,7 @@ TEST_F(TestClient, async_cancel_some_goals)
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
 }
 
-TEST_F(TestClient, async_cancel_some_goals_with_callback)
+TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));

rclcpp_action/test/test_server.cpp

@@ -82,6 +82,8 @@ TEST_F(TestServer, construction_and_destruction)
 {
   auto node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
 
+  ASSERT_NO_THROW(
+  {
     using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
     auto as = rclcpp_action::create_server<Fibonacci>(
       node, "fibonacci",
@@ -93,6 +95,29 @@ TEST_F(TestServer, construction_and_destruction)
       },
       [](std::shared_ptr<GoalHandle>) {});
     (void)as;
+  });
+}
+
+TEST_F(TestServer, construction_and_destruction_after_node)
+{
+  auto node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
+
+  ASSERT_NO_THROW(
+  {
+    using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+    auto as = rclcpp_action::create_server<Fibonacci>(
+      node, "fibonacci",
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    (void)as;
+
+    node.reset();
+  });
 }
 
 TEST_F(TestServer, construction_and_destruction_callback_group)
@@ -733,6 +758,84 @@ TEST_F(TestServer, get_result)
 
   using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
 
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  const std::chrono::milliseconds result_timeout{50};
+
+  rcl_action_server_options_t options = rcl_action_server_get_default_options();
+  options.result_timeout.nanoseconds = RCL_MS_TO_NS(result_timeout.count());
+  auto as = rclcpp_action::create_server<Fibonacci>(
+    node, "fibonacci",
+    handle_goal,
+    handle_cancel,
+    handle_accepted,
+    options);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  // Send result request
+  auto result_client = node->create_client<Fibonacci::Impl::GetResultService>(
+    "fibonacci/_action/get_result");
+  if (!result_client->wait_for_service(std::chrono::seconds(20))) {
+    throw std::runtime_error("get result service didn't become available");
+  }
+  auto request = std::make_shared<Fibonacci::Impl::GetResultService::Request>();
+  request->goal_id.uuid = uuid;
+  auto future = result_client->async_send_request(request);
+
+  // Send a result
+  auto result = std::make_shared<Fibonacci::Result>();
+  result->sequence = {5, 8, 13, 21};
+  received_handle->succeed(result);
+
+  // Wait for the result request to be received
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future));
+
+  auto response = future.get();
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
+  EXPECT_EQ(result->sequence, response->result.sequence);
+
+  // Wait for goal expiration
+  rclcpp::sleep_for(2 * result_timeout);
+
+  // Allow for expiration to take place
+  rclcpp::spin_some(node);
+
+  // Send and wait for another result request
+  future = result_client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future));
+
+  response = future.get();
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_UNKNOWN, response->status);
+}
+
+TEST_F(TestServer, get_result_deferred)
+{
+  auto node = std::make_shared<rclcpp::Node>("get_result", "/rclcpp_action/get_result");
+  const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
   auto handle_cancel = [](std::shared_ptr<GoalHandle>)
     {
       return rclcpp_action::CancelResponse::REJECT;
@@ -763,6 +866,10 @@ TEST_F(TestServer, get_result)
   request->goal_id.uuid = uuid;
   auto future = result_client->async_send_request(request);
 
+  // Process request first
+  rclcpp::sleep_for(std::chrono::milliseconds(10));  // Give a chance for the request to be served
+  rclcpp::spin_some(node);
+
   // Send a result
   auto result = std::make_shared<Fibonacci::Result>();
   result->sequence = {5, 8, 13, 21};