Increase test coverage of rcl_action (#663)

* Add action_server validity check, and remove dead code

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Increase test coverage of rcl_action

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Reorganizing impl structs into private header

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Addressing PR Feedback

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Addressing memory error and PR feedback

Signed-off-by: Stephen Brawner <brawner@gmail.com>

Co-authored-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

rcl_action/CMakeLists.txt

@@ -85,6 +85,7 @@ if(BUILD_TESTING)
   if(TARGET test_action_client)
     target_include_directories(test_action_client PUBLIC
       include
+      src
     )
     target_link_libraries(test_action_client
       ${PROJECT_NAME}
@@ -146,6 +147,7 @@ if(BUILD_TESTING)
   if(TARGET test_action_server)
     target_include_directories(test_action_server PUBLIC
       include
+      src
     )
     target_link_libraries(test_action_server
       ${PROJECT_NAME}
@@ -166,7 +168,7 @@ if(BUILD_TESTING)
     target_link_libraries(test_goal_handle
       ${PROJECT_NAME}
     )
-    ament_target_dependencies(test_action_server "rcl")
+    ament_target_dependencies(test_goal_handle "rcl")
   endif()
   ament_add_gtest(test_goal_state_machine
     test/rcl_action/test_goal_state_machine.cpp
@@ -175,7 +177,7 @@ if(BUILD_TESTING)
     target_link_libraries(test_goal_state_machine
       ${PROJECT_NAME}
     )
-    ament_target_dependencies(test_action_server "rcl")
+    ament_target_dependencies(test_goal_state_machine  "osrf_testing_tools_cpp" "rcl")
   endif()
   ament_add_gtest(test_types
     test/rcl_action/test_types.cpp
@@ -187,7 +189,7 @@ if(BUILD_TESTING)
     target_link_libraries(test_types
       ${PROJECT_NAME}
     )
-    ament_target_dependencies(test_action_server "rcl")
+    ament_target_dependencies(test_types "rcl")
   endif()
   ament_add_gtest(test_names
     test/rcl_action/test_names.cpp
@@ -196,7 +198,23 @@ if(BUILD_TESTING)
     target_link_libraries(test_names
       ${PROJECT_NAME}
     )
-    ament_target_dependencies(test_action_server "rcl")
+    ament_target_dependencies(test_names "rcl")
+  endif()
+  ament_add_gtest(test_wait
+    test/rcl_action/test_wait.cpp
+  )
+  if(TARGET test_wait)
+    target_link_libraries(test_wait
+      ${PROJECT_NAME}
+    )
+    target_include_directories(test_wait PUBLIC
+      src
+    )
+    ament_target_dependencies(test_wait
+      "osrf_testing_tools_cpp"
+      "rcl"
+      "test_msgs"
+    )
   endif()
 endif()
 

rcl_action/src/rcl_action/action_client.c

@@ -18,6 +18,7 @@ extern "C"
 #endif
 
 #include "rcl_action/action_client.h"
+#include "./action_client_impl.h"
 
 #include "rcl_action/default_qos.h"
 #include "rcl_action/names.h"
@@ -38,23 +39,6 @@ extern "C"
 #include "rmw/types.h"
 
 
-typedef struct rcl_action_client_impl_t
-{
-  rcl_client_t goal_client;
-  rcl_client_t cancel_client;
-  rcl_client_t result_client;
-  rcl_subscription_t feedback_subscription;
-  rcl_subscription_t status_subscription;
-  rcl_action_client_options_t options;
-  char * action_name;
-  // Wait set records
-  size_t wait_set_goal_client_index;
-  size_t wait_set_cancel_client_index;
-  size_t wait_set_result_client_index;
-  size_t wait_set_feedback_subscription_index;
-  size_t wait_set_status_subscription_index;
-} rcl_action_client_impl_t;
-
 rcl_action_client_t
 rcl_action_get_zero_initialized_client(void)
 {

rcl_action/src/rcl_action/action_client_impl.h

@@ -0,0 +1,39 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCL_ACTION__ACTION_CLIENT_IMPL_H_
+#define RCL_ACTION__ACTION_CLIENT_IMPL_H_
+
+#include "rcl_action/types.h"
+#include "rcl/rcl.h"
+
+typedef struct rcl_action_client_impl_t
+{
+  rcl_client_t goal_client;
+  rcl_client_t cancel_client;
+  rcl_client_t result_client;
+  rcl_subscription_t feedback_subscription;
+  rcl_subscription_t status_subscription;
+  rcl_action_client_options_t options;
+  char * action_name;
+  // Wait set records
+  size_t wait_set_goal_client_index;
+  size_t wait_set_cancel_client_index;
+  size_t wait_set_result_client_index;
+  size_t wait_set_feedback_subscription_index;
+  size_t wait_set_status_subscription_index;
+} rcl_action_client_impl_t;
+
+
+#endif  // RCL_ACTION__ACTION_CLIENT_IMPL_H_

rcl_action/src/rcl_action/action_server.c

@@ -18,6 +18,7 @@ extern "C"
 #endif
 
 #include "rcl_action/action_server.h"
+#include "./action_server_impl.h"
 
 #include "rcl_action/default_qos.h"
 #include "rcl_action/goal_handle.h"
@@ -34,28 +35,6 @@ extern "C"
 
 #include "rmw/rmw.h"
 
-/// Internal rcl_action implementation struct.
-typedef struct rcl_action_server_impl_t
-{
-  rcl_service_t goal_service;
-  rcl_service_t cancel_service;
-  rcl_service_t result_service;
-  rcl_publisher_t feedback_publisher;
-  rcl_publisher_t status_publisher;
-  rcl_timer_t expire_timer;
-  char * action_name;
-  rcl_action_server_options_t options;
-  // Array of goal handles
-  rcl_action_goal_handle_t ** goal_handles;
-  size_t num_goal_handles;
-  // Clock
-  rcl_clock_t clock;
-  // Wait set records
-  size_t wait_set_goal_service_index;
-  size_t wait_set_cancel_service_index;
-  size_t wait_set_result_service_index;
-  size_t wait_set_expire_timer_index;
-} rcl_action_server_impl_t;
 
 rcl_action_server_t
 rcl_action_get_zero_initialized_server(void)
@@ -693,6 +672,9 @@ rcl_ret_t
 rcl_action_notify_goal_done(
   const rcl_action_server_t * action_server)
 {
+  if (!rcl_action_server_is_valid(action_server)) {
+    return RCL_RET_ACTION_SERVER_INVALID;
+  }
   return _recalculate_expire_timer(
     &action_server->impl->expire_timer,
     action_server->impl->options.result_timeout.nanoseconds,

rcl_action/src/rcl_action/action_server_impl.h

@@ -0,0 +1,45 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCL_ACTION__ACTION_SERVER_IMPL_H_
+#define RCL_ACTION__ACTION_SERVER_IMPL_H_
+
+#include "rcl_action/types.h"
+#include "rcl/rcl.h"
+
+/// Internal rcl_action implementation struct.
+typedef struct rcl_action_server_impl_t
+{
+  rcl_service_t goal_service;
+  rcl_service_t cancel_service;
+  rcl_service_t result_service;
+  rcl_publisher_t feedback_publisher;
+  rcl_publisher_t status_publisher;
+  rcl_timer_t expire_timer;
+  char * action_name;
+  rcl_action_server_options_t options;
+  // Array of goal handles
+  rcl_action_goal_handle_t ** goal_handles;
+  size_t num_goal_handles;
+  // Clock
+  rcl_clock_t clock;
+  // Wait set records
+  size_t wait_set_goal_service_index;
+  size_t wait_set_cancel_service_index;
+  size_t wait_set_result_service_index;
+  size_t wait_set_expire_timer_index;
+} rcl_action_server_impl_t;
+
+
+#endif  // RCL_ACTION__ACTION_SERVER_IMPL_H_

rcl_action/src/rcl_action/goal_state_machine.c

@@ -26,51 +26,40 @@ typedef rcl_action_goal_state_t
 rcl_action_goal_state_t
 _execute_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
 {
-  if (GOAL_STATE_ACCEPTED != state || GOAL_EVENT_EXECUTE != event) {
+  assert(GOAL_STATE_ACCEPTED == state);
-    return GOAL_STATE_UNKNOWN;
+  assert(GOAL_EVENT_EXECUTE == event);
-  }
   return GOAL_STATE_EXECUTING;
 }
 
 rcl_action_goal_state_t
 _cancel_goal_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
 {
-  if ((GOAL_STATE_ACCEPTED != state && GOAL_STATE_EXECUTING != state) ||
+  assert(GOAL_STATE_ACCEPTED == state || GOAL_STATE_EXECUTING == state);
-    GOAL_EVENT_CANCEL_GOAL != event)
+  assert(GOAL_EVENT_CANCEL_GOAL == event);
-  {
-    return GOAL_STATE_UNKNOWN;
-  }
   return GOAL_STATE_CANCELING;
 }
 
 rcl_action_goal_state_t
 _succeed_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
 {
-  if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) ||
+  assert(GOAL_STATE_EXECUTING == state || GOAL_STATE_CANCELING == state);
-    GOAL_EVENT_SUCCEED != event)
+  assert(GOAL_EVENT_SUCCEED == event);
-  {
-    return GOAL_STATE_UNKNOWN;
-  }
   return GOAL_STATE_SUCCEEDED;
 }
 
 rcl_action_goal_state_t
 _abort_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
 {
-  if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) ||
+  assert(GOAL_STATE_EXECUTING == state || GOAL_STATE_CANCELING == state);
-    GOAL_EVENT_ABORT != event)
+  assert(GOAL_EVENT_ABORT == event);
-  {
-    return GOAL_STATE_UNKNOWN;
-  }
   return GOAL_STATE_ABORTED;
 }
 
 rcl_action_goal_state_t
 _canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
 {
-  if (GOAL_STATE_CANCELING != state || GOAL_EVENT_CANCELED != event) {
+  assert(GOAL_STATE_CANCELING == state);
-    return GOAL_STATE_UNKNOWN;
+  assert(GOAL_EVENT_CANCELED == event);
-  }
   return GOAL_STATE_CANCELED;
 }
 

rcl_action/test/rcl_action/test_action_client.cpp

@@ -15,6 +15,7 @@
 #include <gtest/gtest.h>
 
 #include "rcl_action/action_client.h"
+#include "rcl_action/action_client_impl.h"
 
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
@@ -22,6 +23,24 @@
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 #include "test_msgs/action/fibonacci.h"
 
+struct __time_bomb_allocator_state
+{
+  int malloc_count_until_failure;
+};
+
+void * time_bomb_malloc(size_t size, void * state)
+{
+  __time_bomb_allocator_state * time_bomb_state =
+    reinterpret_cast<__time_bomb_allocator_state *>(state);
+  if (time_bomb_state->malloc_count_until_failure >= 0 &&
+    time_bomb_state->malloc_count_until_failure-- == 0)
+  {
+    printf("Malloc time bomb countdown reached 0, returning nullptr\n");
+    return nullptr;
+  }
+  return rcutils_get_default_allocator().allocate(size, rcutils_get_default_allocator().state);
+}
+
 class TestActionClientBaseFixture : public ::testing::Test
 {
 protected:
@@ -122,6 +141,33 @@ TEST_F(TestActionClientBaseFixture, test_action_client_init_fini) {
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
   rcl_reset_error();
 
+  // Fail allocating for implement struct
+  invalid_action_client_options.allocator =
+    rcl_get_default_allocator();
+  __time_bomb_allocator_state time_bomb_state = {0};
+  invalid_action_client_options.allocator.state =
+    reinterpret_cast<void *>(&time_bomb_state);
+  invalid_action_client_options.allocator.allocate = time_bomb_malloc;
+  ret = rcl_action_client_init(
+    &action_client, &this->node, action_typesupport,
+    action_name, &invalid_action_client_options);
+  EXPECT_EQ(ret, RCL_RET_BAD_ALLOC) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Fail copying action_name string, this will also fail to cleanup and will return generic error
+  time_bomb_state.malloc_count_until_failure = 1;
+  invalid_action_client_options.allocator.state = &time_bomb_state;
+  ret = rcl_action_client_init(
+    &action_client, &this->node, action_typesupport,
+    action_name, &invalid_action_client_options);
+  EXPECT_EQ(ret, RCL_RET_ERROR) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Manually cleanup to setup for next test
+  action_client_options.allocator.deallocate(
+    action_client.impl, action_client_options.allocator.state);
+  action_client.impl = NULL;
+
   ret = rcl_action_client_init(
     &action_client, &this->node, action_typesupport,
     action_name, &action_client_options);
@@ -185,6 +231,28 @@ protected:
   rcl_action_client_t action_client;
 };
 
+TEST_F(TestActionClientFixture, test_action_server_is_available) {
+  bool is_available = false;
+  rcl_ret_t ret = rcl_action_server_is_available(nullptr, &this->action_client, &is_available);
+  EXPECT_EQ(ret, RCL_RET_NODE_INVALID);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  ret = rcl_action_server_is_available(&this->node, nullptr, &is_available);
+  EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  ret = rcl_action_server_is_available(&this->node, &this->action_client, nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  ret = rcl_action_server_is_available(&this->node, &this->action_client, &is_available);
+  EXPECT_EQ(ret, RCL_RET_OK);
+  EXPECT_FALSE(is_available);
+}
+
 TEST_F(TestActionClientFixture, test_action_client_is_valid) {
   bool is_valid = rcl_action_client_is_valid(nullptr);
   EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
@@ -194,6 +262,47 @@ TEST_F(TestActionClientFixture, test_action_client_is_valid) {
   EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
   rcl_reset_error();
 
+  rcl_client_impl_t * tmp_client = this->action_client.impl->goal_client.impl;
+  this->action_client.impl->goal_client.impl = nullptr;
+  is_valid = rcl_action_client_is_valid(&this->action_client);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_client.impl->goal_client.impl = tmp_client;
+
+  tmp_client = this->action_client.impl->cancel_client.impl;
+  this->action_client.impl->cancel_client.impl = nullptr;
+  is_valid = rcl_action_client_is_valid(&this->action_client);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_client.impl->cancel_client.impl = tmp_client;
+
+  tmp_client = this->action_client.impl->result_client.impl;
+  this->action_client.impl->result_client.impl = nullptr;
+  is_valid = rcl_action_client_is_valid(&this->action_client);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_client.impl->result_client.impl = tmp_client;
+
+  rcl_subscription_impl_t * tmp_subscription =
+    this->action_client.impl->feedback_subscription.impl;
+  this->action_client.impl->feedback_subscription.impl = nullptr;
+  is_valid = rcl_action_client_is_valid(&this->action_client);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_client.impl->feedback_subscription.impl = tmp_subscription;
+
+  tmp_subscription = this->action_client.impl->status_subscription.impl;
+  this->action_client.impl->status_subscription.impl = nullptr;
+  is_valid = rcl_action_client_is_valid(&this->action_client);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_client.impl->status_subscription.impl = tmp_subscription;
+
   is_valid = rcl_action_client_is_valid(&this->action_client);
   EXPECT_TRUE(is_valid) << rcl_get_error_string().str;
   rcl_reset_error();

rcl_action/test/rcl_action/test_action_server.cpp

@@ -22,12 +22,29 @@
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 
 #include "rcl_action/action_server.h"
+#include "rcl_action/action_server_impl.h"
 
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
 
 #include "test_msgs/action/fibonacci.h"
 
+void * bad_malloc(size_t, void *)
+{
+  return nullptr;
+}
+
+void * bad_realloc(void *, size_t, void *)
+{
+  return nullptr;
+}
+
+void * bad_calloc(size_t, size_t, void *)
+{
+  printf("Returning null ptr\n");
+  return nullptr;
+}
+
 TEST(TestActionServerInitFini, test_action_server_init_fini)
 {
   rcl_allocator_t allocator = rcl_get_default_allocator();
@@ -54,6 +71,7 @@ TEST(TestActionServerInitFini, test_action_server_init_fini)
   // Initialize with a null action server
   ret = rcl_action_server_init(nullptr, &node, &clock, ts, action_name, &options);
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   // Initialize with a null node
@@ -106,6 +124,13 @@ TEST(TestActionServerInitFini, test_action_server_init_fini)
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
   rcl_reset_error();
 
+  // Initialize with an invalid timeout
+  rcl_action_server_options_t bad_options = rcl_action_server_get_default_options();
+  bad_options.result_timeout.nanoseconds = -1;
+  ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, &bad_options);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+  rcl_reset_error();
+
   // Initialize with valid arguments
   ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, &options);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@@ -240,6 +265,104 @@ TEST_F(TestActionServer, test_action_server_is_valid)
   // Check valid action server
   is_valid = rcl_action_server_is_valid(&this->action_server);
   EXPECT_TRUE(is_valid) << rcl_get_error_string().str;
+
+  rcl_service_impl_t * tmp_service = this->action_server.impl->goal_service.impl;
+  this->action_server.impl->goal_service.impl = nullptr;
+  is_valid = rcl_action_server_is_valid(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->goal_service.impl = tmp_service;
+
+  tmp_service = this->action_server.impl->cancel_service.impl;
+  this->action_server.impl->cancel_service.impl = nullptr;
+  is_valid = rcl_action_server_is_valid(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->cancel_service.impl = tmp_service;
+
+  tmp_service = this->action_server.impl->result_service.impl;
+  this->action_server.impl->result_service.impl = nullptr;
+  is_valid = rcl_action_server_is_valid(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->result_service.impl = tmp_service;
+
+  rcl_publisher_impl_t * tmp_publisher = this->action_server.impl->feedback_publisher.impl;
+  this->action_server.impl->feedback_publisher.impl = nullptr;
+  is_valid = rcl_action_server_is_valid(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->feedback_publisher.impl = tmp_publisher;
+
+  tmp_publisher = this->action_server.impl->status_publisher.impl;
+  this->action_server.impl->status_publisher.impl = nullptr;
+  is_valid = rcl_action_server_is_valid(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->status_publisher.impl = tmp_publisher;
+}
+
+TEST_F(TestActionServer, test_action_server_is_valid_except_context)
+{
+  // Check with null pointer
+  bool is_valid = rcl_action_server_is_valid_except_context(nullptr);
+  EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Check with uninitialized action server
+  rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
+  is_valid = rcl_action_server_is_valid_except_context(&invalid_action_server);
+  EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  // Check valid action server
+  is_valid = rcl_action_server_is_valid_except_context(&this->action_server);
+  EXPECT_TRUE(is_valid) << rcl_get_error_string().str;
+
+  rcl_service_impl_t * tmp_service = this->action_server.impl->goal_service.impl;
+  this->action_server.impl->goal_service.impl = nullptr;
+  is_valid = rcl_action_server_is_valid_except_context(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->goal_service.impl = tmp_service;
+
+  tmp_service = this->action_server.impl->cancel_service.impl;
+  this->action_server.impl->cancel_service.impl = nullptr;
+  is_valid = rcl_action_server_is_valid_except_context(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->cancel_service.impl = tmp_service;
+
+  tmp_service = this->action_server.impl->result_service.impl;
+  this->action_server.impl->result_service.impl = nullptr;
+  is_valid = rcl_action_server_is_valid_except_context(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->result_service.impl = tmp_service;
+
+  rcl_publisher_impl_t * tmp_publisher = this->action_server.impl->feedback_publisher.impl;
+  this->action_server.impl->feedback_publisher.impl = nullptr;
+  is_valid = rcl_action_server_is_valid_except_context(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->feedback_publisher.impl = tmp_publisher;
+
+  tmp_publisher = this->action_server.impl->status_publisher.impl;
+  this->action_server.impl->status_publisher.impl = nullptr;
+  is_valid = rcl_action_server_is_valid_except_context(&this->action_server);
+  EXPECT_FALSE(is_valid);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  this->action_server.impl->status_publisher.impl = tmp_publisher;
 }
 
 TEST_F(TestActionServer, test_action_accept_new_goal)
@@ -251,17 +374,38 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
   // Accept goal with a null action server
   rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal(nullptr, &goal_info_in);
   EXPECT_EQ(goal_handle, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   // Accept goal with null goal info
   goal_handle = rcl_action_accept_new_goal(&this->action_server, nullptr);
   EXPECT_EQ(goal_handle, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   // Accept goal with invalid action server
   rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
   goal_handle = rcl_action_accept_new_goal(&invalid_action_server, &goal_info_in);
   EXPECT_EQ(goal_handle, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check failing allocation of goal_handle
+  this->action_server.impl->options.allocator.allocate = bad_malloc;
+  goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
+  EXPECT_EQ(goal_handle, nullptr);
+  this->action_server.impl->options.allocator.allocate =
+    rcl_get_default_allocator().allocate;
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check failing reallocate of goal_handles
+  this->action_server.impl->options.allocator.reallocate = bad_realloc;
+  goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
+  EXPECT_EQ(goal_handle, nullptr);
+  this->action_server.impl->options.allocator.reallocate =
+    rcl_get_default_allocator().reallocate;
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   std::vector<rcl_action_goal_handle_t> handles;
@@ -276,6 +420,13 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
   EXPECT_TRUE(uuidcmp(goal_info_out.goal_id.uuid, goal_info_in.goal_id.uuid));
   size_t num_goals = 0u;
   rcl_action_goal_handle_t ** goal_handle_array = {nullptr};
+
+  // Check invalid action server
+  ret = rcl_action_server_get_goal_handles(nullptr, &goal_handle_array, &num_goals);
+  EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
   ret = rcl_action_server_get_goal_handles(&this->action_server, &goal_handle_array, &num_goals);
   ASSERT_EQ(ret, RCL_RET_OK);
   EXPECT_EQ(num_goals, 1u);
@@ -285,6 +436,7 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
   // Accept with the same goal ID
   goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
   EXPECT_EQ(goal_handle, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   // Accept a different goal
@@ -305,14 +457,84 @@ TEST_F(TestActionServer, test_action_accept_new_goal)
 
   for (auto & handle : handles) {
     EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&handle));
+    EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str;
   }
 }
 
+TEST_F(TestActionServer, test_action_server_goal_exists) {
+  rcl_action_goal_info_t goal_info_out = rcl_action_get_zero_initialized_goal_info();
+  EXPECT_FALSE(rcl_action_server_goal_exists(nullptr, &goal_info_out));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  EXPECT_FALSE(rcl_action_server_goal_exists(&this->action_server, nullptr));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Initialize a goal info
+  rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
+  init_test_uuid0(goal_info_in.goal_id.uuid);
+
+  // Add new goal
+  rcl_action_goal_handle_t * goal_handle =
+    rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
+  EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
+
+  // Check exists
+  EXPECT_TRUE(rcl_action_server_goal_exists(&this->action_server, &goal_info_in));
+
+  rcl_action_goal_info_t different_goal = rcl_action_get_zero_initialized_goal_info();
+  init_test_uuid1(goal_info_in.goal_id.uuid);
+
+  // Check doesn't exist
+  EXPECT_FALSE(rcl_action_server_goal_exists(&this->action_server, &different_goal));
+  EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str;
+
+  // Check corrupted goal_handles
+  rcl_get_default_allocator().deallocate(goal_handle, rcl_get_default_allocator().state);
+  this->action_server.impl->goal_handles[this->action_server.impl->num_goal_handles - 1] = nullptr;
+  EXPECT_FALSE(rcl_action_server_goal_exists(&this->action_server, &different_goal));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Reset for teardown
+  this->action_server.impl->num_goal_handles--;
+}
+
+TEST_F(TestActionServer, test_action_server_notify_goal_done) {
+  // Invalid action server
+  EXPECT_EQ(RCL_RET_ACTION_SERVER_INVALID, rcl_action_notify_goal_done(nullptr));
+  rcl_reset_error();
+
+  // No goals yet, should be ok
+  EXPECT_EQ(RCL_RET_OK, rcl_action_notify_goal_done(&action_server));
+
+  rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
+  init_test_uuid0(goal_info_in.goal_id.uuid);
+
+  // Add new goal
+  rcl_action_goal_handle_t * goal_handle =
+    rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
+  EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
+
+  // One goal, should be able to notify
+  EXPECT_EQ(RCL_RET_OK, rcl_action_notify_goal_done(&action_server));
+
+  // Invalid goal handle
+  rcl_get_default_allocator().deallocate(goal_handle, rcl_get_default_allocator().state);
+  this->action_server.impl->goal_handles[this->action_server.impl->num_goal_handles - 1] = nullptr;
+  EXPECT_EQ(RCL_RET_ERROR, rcl_action_notify_goal_done(&action_server));
+  rcl_reset_error();
+
+  // Reset for teardown
+  this->action_server.impl->num_goal_handles--;
+}
+
 TEST_F(TestActionServer, test_action_clear_expired_goals)
 {
   const size_t capacity = 1u;
   rcl_action_goal_info_t expired_goals[1u];
-  size_t num_expired = 1u;
+  size_t num_expired = 42u;
   // Clear expired goals with null action server
   rcl_ret_t ret = rcl_action_expire_goals(nullptr, expired_goals, capacity, &num_expired);
   EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
@@ -324,6 +546,24 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
   EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
   rcl_reset_error();
 
+  // Clear with invalid arguments
+  ret = rcl_action_expire_goals(&this->action_server, nullptr, capacity, &num_expired);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+  EXPECT_EQ(num_expired, 42u);
+  rcl_reset_error();
+
+  // Clear with invalid arguments
+  ret = rcl_action_expire_goals(&this->action_server, expired_goals, 0u, &num_expired);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+  EXPECT_EQ(num_expired, 42u);
+  rcl_reset_error();
+
+  // Clear with invalid arguments
+  ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+  EXPECT_EQ(num_expired, 42u);
+  rcl_reset_error();
+
   // Clear with valid arguments
   ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@@ -339,6 +579,7 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
   // Set ROS time
   ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock));
   ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(1)));
+
   // Accept a goal to create a new handle
   rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
   init_test_uuid1(goal_info_in.goal_id.uuid);
@@ -346,11 +587,14 @@ TEST_F(TestActionServer, test_action_clear_expired_goals)
     rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
   ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
   handles.push_back(*goal_handle);
+
   // Transition executing to aborted
   ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_EXECUTE));
   ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_ABORT));
+
   // Set time to something far in the future
   ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(99999)));
+
   // Clear with valid arguments
   ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@@ -382,6 +626,13 @@ TEST_F(TestActionServer, test_action_process_cancel_request)
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
   rcl_reset_error();
 
+  this->action_server.impl->options.allocator.allocate = bad_malloc;
+  // Process cancel request with bad allocator
+  ret = rcl_action_process_cancel_request(&this->action_server, &cancel_request, &cancel_response);
+  EXPECT_EQ(ret, RCL_RET_BAD_ALLOC);
+  rcl_reset_error();
+  this->action_server.impl->options.allocator = rcl_get_default_allocator();
+
   // Process cancel request with invalid action server
   rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
   ret = rcl_action_process_cancel_request(
@@ -397,6 +648,24 @@ TEST_F(TestActionServer, test_action_process_cancel_request)
   EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u);
   // A zero request means "cancel all goals", which succeeds if there's nothing to cancel
   EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE);
+
+  // Number of goals is not 0, but goal handle is null, for case with request_nanosec == 0
+  size_t num_goal_handles = 1u;
+  rcl_allocator_t allocator = this->action_server.impl->options.allocator;
+  this->action_server.impl->num_goal_handles = num_goal_handles;
+  this->action_server.impl->goal_handles = reinterpret_cast<rcl_action_goal_handle_t **>(
+    allocator.zero_allocate(num_goal_handles, sizeof(rcl_action_goal_handle_t *), allocator.state));
+  ret = rcl_action_process_cancel_request(
+    &this->action_server, &cancel_request, &cancel_response);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+
+  // Number of goals is not 0, but goal handle is null, for case with request_nanosec > 0
+  cancel_request.goal_info.stamp.nanosec = 1;
+  ret = rcl_action_process_cancel_request(
+    &this->action_server, &cancel_request, &cancel_response);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
 }
 
 TEST_F(TestActionServer, test_action_server_get_goal_status_array)
@@ -419,6 +688,23 @@ TEST_F(TestActionServer, test_action_server_get_goal_status_array)
   EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
   rcl_reset_error();
 
+  // Check goal_status_array_init fails
+  this->action_server.impl->num_goal_handles = 1u;
+  this->action_server.impl->options.allocator.zero_allocate = bad_calloc;
+  ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
+  EXPECT_EQ(ret, RCL_RET_BAD_ALLOC);
+  rcl_reset_error();
+  this->action_server.impl->options.allocator = rcl_get_default_allocator();
+
+  // Check status_message is already inited
+  this->action_server.impl->num_goal_handles = 1u;
+  status_array.msg.status_list.size = 1;
+  ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+  status_array.msg.status_list.size = 0;
+  this->action_server.impl->num_goal_handles = 0u;
+
   // Get with valid arguments (but not goals being tracked)
   ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;

rcl_action/test/rcl_action/test_goal_handle.cpp

@@ -23,6 +23,11 @@
 
 #include "rcl/error_handling.h"
 
+void * bad_malloc(size_t, void *)
+{
+  return nullptr;
+}
+
 TEST(TestGoalHandle, test_goal_handle_init_fini)
 {
   rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
@@ -45,6 +50,13 @@ TEST(TestGoalHandle, test_goal_handle_init_fini)
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
   rcl_reset_error();
 
+  // Initialize with a failing allocator
+  rcl_allocator_t bad_allocator = rcl_get_default_allocator();
+  bad_allocator.allocate = bad_malloc;
+  ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, bad_allocator);
+  EXPECT_EQ(RCL_RET_BAD_ALLOC, ret);
+  rcl_reset_error();
+
   // Initialize with valid goal handle and allocator
   ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator());
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
@@ -154,6 +166,7 @@ TEST(TestGoalHandle, test_goal_handle_update_state_invalid)
   ret = rcl_action_update_goal_state(&goal_handle, GOAL_EVENT_NUM_EVENTS);
   EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID) << rcl_get_error_string().str;
   rcl_reset_error();
+
   EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle));
 }
 
@@ -216,6 +229,28 @@ TEST_P(TestGoalHandleStateTransitionSequence, test_goal_handle_state_transitions
   // Goal handle starts in state ACCEPTED
   expect_state_eq(GOAL_STATE_ACCEPTED);
 
+  // Test bad goal handles
+  rcl_action_goal_state_t status;
+  EXPECT_EQ(
+    RCL_RET_ACTION_GOAL_HANDLE_INVALID,
+    rcl_action_goal_handle_get_status(nullptr, &status));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  EXPECT_EQ(
+    RCL_RET_INVALID_ARGUMENT,
+    rcl_action_goal_handle_get_status(&this->goal_handle, nullptr));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  EXPECT_FALSE(rcl_action_goal_handle_is_active(nullptr));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  EXPECT_FALSE(rcl_action_goal_handle_is_cancelable(nullptr));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
   // Walk through state transitions
   rcl_ret_t ret;
   for (const EventStateActiveCancelableTuple & event_state : this->test_sequence) {
@@ -228,6 +263,8 @@ TEST_P(TestGoalHandleStateTransitionSequence, test_goal_handle_state_transitions
     ret = rcl_action_update_goal_state(&this->goal_handle, goal_event);
     if (GOAL_STATE_UNKNOWN == expected_goal_state) {
       EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID);
+      EXPECT_TRUE(rcl_error_is_set());
+      rcl_reset_error();
       continue;
     }
     EXPECT_EQ(ret, RCL_RET_OK);

rcl_action/test/rcl_action/test_graph.cpp

@@ -36,6 +36,11 @@
 # define CLASSNAME(NAME, SUFFIX) NAME
 #endif
 
+void * bad_malloc(size_t, void *)
+{
+  return NULL;
+}
+
 class CLASSNAME (TestActionGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test
 {
 public:
@@ -144,6 +149,15 @@ TEST_F(
     &this->node, &this->allocator, this->test_graph_node_name, "", nullptr);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
   rcl_reset_error();
+
+  // Failing allocator
+  rcl_allocator_t bad_allocator = rcl_get_default_allocator();
+  bad_allocator.allocate = bad_malloc;
+  ret = rcl_action_get_client_names_and_types_by_node(
+    &this->node, &bad_allocator, this->test_graph_node_name, "", nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+
   // Valid call
   ret = rcl_action_get_client_names_and_types_by_node(
     &this->node, &this->allocator, this->test_graph_node_name, "", &nat);

rcl_action/test/rcl_action/test_names.cpp

@@ -30,6 +30,11 @@ struct ActionDerivedNameTestSubject
   const char * subject_name;
 };
 
+void * bad_malloc(size_t, void *)
+{
+  return NULL;
+}
+
 std::ostream & operator<<(std::ostream & os, const ActionDerivedNameTestSubject & test_subject)
 {
   return os << test_subject.subject_name;
@@ -57,6 +62,8 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
     null_action_name, default_allocator,
     &action_derived_name);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   action_derived_name = NULL;
   const char * const invalid_action_name = "";
@@ -64,6 +71,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
     invalid_action_name, default_allocator,
     &action_derived_name);
   EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret) << rcl_get_error_string().str;
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   action_derived_name = NULL;
@@ -73,6 +81,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
     test_subject.action_name, invalid_allocator,
     &action_derived_name);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   action_derived_name = NULL;
@@ -81,6 +90,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
     test_subject.action_name, default_allocator,
     invalid_ptr_to_action_derived_name);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   char dummy_char = '\0';
@@ -89,6 +99,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
     test_subject.action_name, default_allocator,
     &action_derived_name);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  EXPECT_TRUE(rcl_error_is_set());
   rcl_reset_error();
 
   action_derived_name = NULL;
@@ -98,6 +109,16 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   EXPECT_STREQ(test_subject.expected_action_derived_name, action_derived_name);
   default_allocator.deallocate(action_derived_name, default_allocator.state);
+
+  rcl_allocator_t failing_allocator = rcl_get_default_allocator();
+  failing_allocator.allocate = bad_malloc;
+  action_derived_name = NULL;
+  ret = test_subject.get_action_derived_name(
+    test_subject.action_name, failing_allocator,
+    &action_derived_name);
+  EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str;
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 }
 
 const ActionDerivedNameTestSubject action_service_and_topic_subjects[] = {

rcl_action/test/rcl_action/test_types.cpp

@@ -13,8 +13,19 @@
 // limitations under the License.
 #include <gtest/gtest.h>
 
+#include "rcl/error_handling.h"
 #include "rcl_action/types.h"
 
+void * bad_realloc(void *, size_t, void *)
+{
+  return nullptr;
+}
+
+void * bad_calloc(size_t, size_t, void *)
+{
+  return nullptr;
+}
+
 TEST(TestActionTypes, test_get_zero_inititalized_goal_info)
 {
   rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
@@ -79,6 +90,8 @@ TEST(TestActionTypes, test_init_fini_goal_status_array)
     num_status,
     rcl_get_default_allocator());
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Initialize with invalid allocator
   rcl_allocator_t invalid_allocator = rcl_get_default_allocator();
@@ -90,6 +103,22 @@ TEST(TestActionTypes, test_init_fini_goal_status_array)
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
   EXPECT_EQ(status_array.msg.status_list.size, 0u);
   EXPECT_EQ(status_array.msg.status_list.data, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Initialize with a failing allocator
+  rcl_allocator_t failing_allocator = rcl_get_default_allocator();
+  failing_allocator.zero_allocate = bad_calloc;
+  status_array = rcl_action_get_zero_initialized_goal_status_array();
+  ASSERT_EQ(status_array.msg.status_list.size, 0u);
+  ret = rcl_action_goal_status_array_init(&status_array, num_status, failing_allocator);
+  EXPECT_EQ(ret, RCL_RET_BAD_ALLOC);
+  EXPECT_EQ(status_array.msg.status_list.size, 0u);
+  EXPECT_EQ(status_array.msg.status_list.data, nullptr);
+  // Doesn't set error
+  EXPECT_FALSE(rcl_error_is_set());
+  rcl_reset_error();
+
   // Initialize with zero size
   status_array = rcl_action_get_zero_initialized_goal_status_array();
   ASSERT_EQ(status_array.msg.status_list.size, 0u);
@@ -97,18 +126,33 @@ TEST(TestActionTypes, test_init_fini_goal_status_array)
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
   EXPECT_EQ(status_array.msg.status_list.size, 0u);
   EXPECT_EQ(status_array.msg.status_list.data, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Initialize with non-zero size
+  status_array = rcl_action_get_zero_initialized_goal_status_array();
+  status_array.msg.status_list.size = 1;
+  ret = rcl_action_goal_status_array_init(&status_array, num_status, rcl_get_default_allocator());
+  EXPECT_EQ(ret, RCL_RET_ALREADY_INIT);
+  // Expect array to be unchanged
+  EXPECT_EQ(status_array.msg.status_list.size, 1u);
+  EXPECT_EQ(status_array.msg.status_list.data, nullptr);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Initialize with valid arguments
   status_array = rcl_action_get_zero_initialized_goal_status_array();
   ASSERT_EQ(status_array.msg.status_list.size, 0u);
   ret = rcl_action_goal_status_array_init(&status_array, num_status, rcl_get_default_allocator());
-  EXPECT_EQ(ret, RCL_RET_OK);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   EXPECT_EQ(status_array.msg.status_list.size, num_status);
   EXPECT_NE(status_array.msg.status_list.data, nullptr);
 
   // Finalize with invalid status array
   ret = rcl_action_goal_status_array_fini(nullptr);
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Finalize with valid arguments
   ret = rcl_action_goal_status_array_fini(&status_array);
@@ -124,6 +168,8 @@ TEST(TestActionTypes, test_init_fini_cancel_response)
     num_goals_canceling,
     rcl_get_default_allocator());
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Initialize with invalid allocator
   rcl_allocator_t invalid_allocator = rcl_get_default_allocator();
@@ -135,6 +181,22 @@ TEST(TestActionTypes, test_init_fini_cancel_response)
   EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u);
   EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr);
   EXPECT_EQ(cancel_response.msg.return_code, 0);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Initialize with failing allocator
+  rcl_allocator_t failing_allocator = rcl_get_default_allocator();
+  failing_allocator.zero_allocate = bad_calloc;
+  cancel_response = rcl_action_get_zero_initialized_cancel_response();
+  ASSERT_EQ(cancel_response.msg.goals_canceling.size, 0u);
+  ret = rcl_action_cancel_response_init(&cancel_response, num_goals_canceling, failing_allocator);
+  EXPECT_EQ(ret, RCL_RET_BAD_ALLOC);
+  EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u);
+  EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr);
+  EXPECT_EQ(cancel_response.msg.return_code, 0);
+  // Bad allocation doesn't set error message
+  EXPECT_FALSE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Initialize with zero size
   cancel_response = rcl_action_get_zero_initialized_cancel_response();
@@ -144,6 +206,33 @@ TEST(TestActionTypes, test_init_fini_cancel_response)
   EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u);
   EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr);
   EXPECT_EQ(cancel_response.msg.return_code, 0);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Initialize with non-zero size
+  cancel_response = rcl_action_get_zero_initialized_cancel_response();
+  cancel_response.msg.goals_canceling.size = 1;
+  ret = rcl_action_cancel_response_init(&cancel_response, 1, rcl_get_default_allocator());
+  EXPECT_EQ(ret, RCL_RET_ALREADY_INIT);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check unchanged
+  EXPECT_EQ(cancel_response.msg.goals_canceling.size, 1u);
+  EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr);
+  EXPECT_EQ(cancel_response.msg.return_code, 0);
+
+  // Initialize with non-zero return code
+  cancel_response = rcl_action_get_zero_initialized_cancel_response();
+  cancel_response.msg.return_code = 1;
+  ret = rcl_action_cancel_response_init(&cancel_response, 0, rcl_get_default_allocator());
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  // Check unchanged
+  EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u);
+  EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr);
+  EXPECT_EQ(cancel_response.msg.return_code, 1);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Initialize with valid arguments
   cancel_response = rcl_action_get_zero_initialized_cancel_response();
@@ -152,7 +241,7 @@ TEST(TestActionTypes, test_init_fini_cancel_response)
     &cancel_response,
     num_goals_canceling,
     rcl_get_default_allocator());
-  EXPECT_EQ(ret, RCL_RET_OK);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   EXPECT_EQ(cancel_response.msg.goals_canceling.size, num_goals_canceling);
   EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr);
   EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE);
@@ -160,8 +249,10 @@ TEST(TestActionTypes, test_init_fini_cancel_response)
   // Finalize with invalid cancel response
   ret = rcl_action_cancel_response_fini(nullptr);
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
 
   // Finalize with valid arguments
   ret = rcl_action_cancel_response_fini(&cancel_response);
-  EXPECT_EQ(ret, RCL_RET_OK);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
 }

rcl_action/test/rcl_action/test_wait.cpp

@@ -0,0 +1,813 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include <gtest/gtest.h>
+
+#include "rcl_action/action_client.h"
+#include "rcl_action/action_server.h"
+#include "rcl_action/wait.h"
+#include "rcl_action/action_client_impl.h"
+#include "rcl_action/action_server_impl.h"
+
+#include "rcl/error_handling.h"
+#include "rcl/rcl.h"
+
+#include "osrf_testing_tools_cpp/scope_exit.hpp"
+#include "test_msgs/action/fibonacci.h"
+
+class TestActionClientWait : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rcl_ret_t ret;
+    {
+      rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+      ret = rcl_init_options_init(&init_options, rcl_get_default_allocator());
+      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+      OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+      {
+        EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str;
+      });
+      this->context = rcl_get_zero_initialized_context();
+      ret = rcl_init(0, nullptr, &init_options, &this->context);
+      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
+    this->node = rcl_get_zero_initialized_node();
+    rcl_node_options_t node_options = rcl_node_get_default_options();
+    const char * node_name = "test_action_client_node";
+    ret = rcl_node_init(&this->node, node_name, "", &this->context, &node_options);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str;
+
+    const char * action_name = "test_action_client_name";
+    const rosidl_action_type_support_t * action_typesupport =
+      ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
+    const rcl_action_client_options_t action_client_options =
+      rcl_action_client_get_default_options();
+
+    action_client = rcl_action_get_zero_initialized_client();
+    ret = rcl_action_client_init(
+      &this->action_client, &this->node, action_typesupport,
+      action_name, &action_client_options);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str;
+  }
+
+  void TearDown() override
+  {
+    rcl_ret_t fini_ret = rcl_action_client_fini(&action_client, &this->node);
+    EXPECT_EQ(RCL_RET_OK, fini_ret) << rcl_get_error_string().str;
+    rcl_ret_t ret = rcl_node_fini(&this->node);
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    ret = rcl_shutdown(&this->context);
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    ret = rcl_context_fini(&this->context);
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  }
+
+  rcl_context_t context;
+  rcl_node_t node;
+  rcl_action_client_t action_client;
+};
+
+class TestActionServerWait : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+    rcl_ret_t ret;
+    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+    ret = rcl_init_options_init(&init_options, allocator);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+    {
+      EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str;
+    });
+    context = rcl_get_zero_initialized_context();
+    ret = rcl_init(0, nullptr, &init_options, &context);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    this->node = rcl_get_zero_initialized_node();
+    rcl_node_options_t node_options = rcl_node_get_default_options();
+    ret = rcl_node_init(&this->node, "test_action_server_node", "", &context, &node_options);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    ret = rcl_clock_init(RCL_ROS_TIME, &this->clock, &allocator);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
+      test_msgs, Fibonacci);
+    const rcl_action_server_options_t options = rcl_action_server_get_default_options();
+    const char * action_name = "test_action_server_name";
+    this->action_server = rcl_action_get_zero_initialized_server();
+    ret = rcl_action_server_init(
+      &this->action_server, &this->node, &this->clock, ts, action_name, &options);
+    ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str;
+  }
+
+  void TearDown() override
+  {
+    // Finalize
+    rcl_ret_t ret = rcl_action_server_fini(&this->action_server, &this->node);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    ret = rcl_clock_fini(&this->clock);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    ret = rcl_node_fini(&this->node);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    ret = rcl_shutdown(&context);
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    ret = rcl_context_fini(&this->context);
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  }
+
+  void init_test_uuid0(uint8_t * uuid)
+  {
+    for (uint8_t i = 0; i < UUID_SIZE; ++i) {
+      uuid[i] = i;
+    }
+  }
+
+  void init_test_uuid1(uint8_t * uuid)
+  {
+    for (uint8_t i = 0; i < UUID_SIZE; ++i) {
+      uuid[i] = 15 - i;
+    }
+  }
+
+  rcl_action_server_t action_server;
+  rcl_context_t context;
+  rcl_node_t node;
+  rcl_clock_t clock;
+};  // class TestActionServer
+
+TEST_F(TestActionClientWait, test_wait_set_add_action_client) {
+  // Check wait_set is null
+  size_t client_index = 42;
+  size_t subscription_index = 42;
+  rcl_ret_t ret = rcl_action_wait_set_add_action_client(
+    nullptr, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_WAIT_SET_INVALID, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  rcl_reset_error();
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  });
+
+  // Check action client is null
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, nullptr, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_ACTION_CLIENT_INVALID, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  rcl_reset_error();
+
+  // Failed to add goal client
+  wait_set = rcl_get_zero_initialized_wait_set();
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator());
+  EXPECT_EQ(RCL_RET_OK, ret);
+
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Failed to add cancel client
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 1, 0, 0, &this->context, rcl_get_default_allocator());
+  EXPECT_EQ(RCL_RET_OK, ret);
+
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+
+  // Failed to add result client
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 2, 0, 0, &this->context, rcl_get_default_allocator());
+  EXPECT_EQ(RCL_RET_OK, ret);
+
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Failed to add feedback subscription
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator());
+  EXPECT_EQ(RCL_RET_OK, ret);
+
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Failed to add status subscription
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 1, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator());
+  EXPECT_EQ(RCL_RET_OK, ret);
+
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret);
+  EXPECT_EQ(42u, client_index);
+  EXPECT_EQ(42u, subscription_index);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Typical case
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 2, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  ret = rcl_action_wait_set_add_action_client(
+    &wait_set, &action_client, &client_index, &subscription_index);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(0u, client_index);
+  EXPECT_EQ(0u, subscription_index);
+
+  // Should work fine, but doesn't increment counts
+  // wait_set = rcl_get_zero_initialized_wait_set();
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 2, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  ret = rcl_action_wait_set_add_action_client(&wait_set, &action_client, nullptr, nullptr);
+  EXPECT_EQ(RCL_RET_OK, ret);
+}
+
+TEST_F(TestActionServerWait, test_wait_set_add_action_server) {
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  });
+  rcl_ret_t ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  size_t service_index = 42;
+  ret = rcl_action_wait_set_add_action_server(nullptr, &this->action_server, &service_index);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID);
+  EXPECT_EQ(service_index, 42u);
+  rcl_reset_error();
+
+  ret = rcl_action_wait_set_add_action_server(&wait_set, nullptr, &service_index);
+  EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
+  EXPECT_EQ(service_index, 42u);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check adding goal service fails
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator());
+
+  ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str;
+  EXPECT_EQ(service_index, 42u);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check adding cancel service fails
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 0, 1, 0, &this->context, rcl_get_default_allocator());
+
+  ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str;
+  EXPECT_EQ(service_index, 42u);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check adding result service fails
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 0, 2, 0, &this->context, rcl_get_default_allocator());
+
+  ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str;
+  EXPECT_EQ(service_index, 42u);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check adding expire timer fails
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 0, 0, 3, 0, &this->context, rcl_get_default_allocator());
+
+  ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str;
+  EXPECT_EQ(service_index, 42u);
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+
+  // Check everything is good
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator());
+
+  ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  EXPECT_EQ(service_index, 0u);
+
+  // Everything should be ok without a valid service index.
+  EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator());
+  ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, nullptr);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str;
+}
+
+
+TEST_F(TestActionClientWait, test_client_wait_set_get_num_entities) {
+  const char * action_name = "test_action_client_name";
+  const rosidl_action_type_support_t * action_typesupport =
+    ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
+  const rcl_action_client_options_t action_client_options =
+    rcl_action_client_get_default_options();
+
+  rcl_action_client_t action_client = rcl_action_get_zero_initialized_client();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    rcl_ret_t fini_ret = rcl_action_client_fini(&action_client, &this->node);
+    EXPECT_EQ(RCL_RET_OK, fini_ret) << rcl_get_error_string().str;
+  });
+
+  rcl_ret_t ret = rcl_action_client_init(
+    &action_client, &this->node, action_typesupport,
+    action_name, &action_client_options);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  size_t num_subscriptions = 0;
+  size_t num_guard_conditions = 0;
+  size_t num_timers = 0;
+  size_t num_clients = 0;
+  size_t num_services = 0;
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    nullptr, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services);
+  EXPECT_EQ(RCL_RET_ACTION_CLIENT_INVALID, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    &action_client, nullptr, &num_guard_conditions, &num_timers, &num_clients, &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    &action_client, &num_subscriptions, nullptr, &num_timers, &num_clients, &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    &action_client,
+    &num_subscriptions,
+    &num_guard_conditions,
+    nullptr,
+    &num_clients,
+    &num_services);
+
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    &action_client,
+    &num_subscriptions,
+    &num_guard_conditions,
+    &num_clients,
+    nullptr,
+    &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    &action_client,
+    &num_subscriptions,
+    &num_guard_conditions,
+    &num_clients,
+    &num_clients,
+    nullptr);
+
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_num_entities(
+    &action_client,
+    &num_subscriptions,
+    &num_guard_conditions,
+    &num_clients,
+    &num_clients,
+    &num_services);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(num_subscriptions, 2u);
+  EXPECT_EQ(num_guard_conditions, 0u);
+  EXPECT_EQ(num_timers, 0u);
+  EXPECT_EQ(num_clients, 3u);
+  EXPECT_EQ(num_services, 0u);
+}
+
+TEST_F(TestActionServerWait, test_server_wait_set_get_num_entities) {
+  size_t num_subscriptions = 0;
+  size_t num_guard_conditions = 0;
+  size_t num_timers = 0;
+  size_t num_clients = 0;
+  size_t num_services = 0;
+
+  rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities(
+    nullptr, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services);
+  EXPECT_EQ(RCL_RET_ACTION_SERVER_INVALID, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    &this->action_server,
+    nullptr,
+    &num_guard_conditions,
+    &num_timers,
+    &num_clients,
+    &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    &this->action_server,
+    &num_subscriptions,
+    nullptr,
+    &num_timers,
+    &num_clients,
+    &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    &this->action_server,
+    &num_subscriptions,
+    &num_guard_conditions,
+    nullptr,
+    &num_clients,
+    &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    &this->action_server,
+    &num_subscriptions,
+    &num_guard_conditions,
+    &num_timers,
+    nullptr,
+    &num_services);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    &this->action_server,
+    &num_subscriptions,
+    &num_guard_conditions,
+    &num_timers,
+    &num_clients,
+    nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    &this->action_server,
+    &num_subscriptions,
+    &num_guard_conditions,
+    &num_timers,
+    &num_clients,
+    &num_services);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  EXPECT_EQ(num_subscriptions, 0u);
+  EXPECT_EQ(num_guard_conditions, 0u);
+  EXPECT_EQ(num_timers, 1u);
+  EXPECT_EQ(num_clients, 0u);
+  EXPECT_EQ(num_services, 3u);
+}
+
+TEST_F(TestActionClientWait, test_client_wait_set_get_entities_ready) {
+  const char * action_name = "test_action_client_name";
+  const rosidl_action_type_support_t * action_typesupport =
+    ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
+  const rcl_action_client_options_t action_client_options =
+    rcl_action_client_get_default_options();
+
+  rcl_action_client_t action_client = rcl_action_get_zero_initialized_client();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    rcl_ret_t fini_ret = rcl_action_client_fini(&action_client, &this->node);
+    EXPECT_EQ(RCL_RET_OK, fini_ret) << rcl_get_error_string().str;
+  });
+
+  rcl_ret_t ret = rcl_action_client_init(
+    &action_client, &this->node, action_typesupport,
+    action_name, &action_client_options);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  });
+  ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 1, &this->context, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  bool is_feedback_ready = false;
+  bool is_status_ready = false;
+  bool is_goal_response_ready = false;
+  bool is_cancel_response_ready = false;
+  bool is_result_response_ready = false;
+
+  // Check valid arguments
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    nullptr,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    nullptr,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    nullptr,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    nullptr,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    nullptr,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    nullptr,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  // Even though they should be different subscriptions and clients, we can mock the correct
+  // behavior by assigning all three clients to the same index, and both subscriptions as well
+  wait_set.size_of_subscriptions = 1;
+  wait_set.size_of_clients = 1;
+
+  // Check wait indices are out of bounds
+  action_client.impl->wait_set_feedback_subscription_index = 10;
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+  action_client.impl->wait_set_feedback_subscription_index = 0;
+
+  action_client.impl->wait_set_status_subscription_index = 10;
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+  action_client.impl->wait_set_status_subscription_index = 0;
+
+  action_client.impl->wait_set_goal_client_index = 10;
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+  action_client.impl->wait_set_goal_client_index = 0;
+
+  action_client.impl->wait_set_cancel_client_index = 10;
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+  action_client.impl->wait_set_cancel_client_index = 0;
+
+  action_client.impl->wait_set_result_client_index = 10;
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcl_reset_error();
+  action_client.impl->wait_set_result_client_index = 0;
+
+  ret = rcl_action_client_wait_set_get_entities_ready(
+    &wait_set,
+    &action_client,
+    &is_feedback_ready,
+    &is_status_ready,
+    &is_goal_response_ready,
+    &is_cancel_response_ready,
+    &is_result_response_ready);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  EXPECT_FALSE(is_feedback_ready);
+  EXPECT_FALSE(is_status_ready);
+  EXPECT_FALSE(is_goal_response_ready);
+  EXPECT_FALSE(is_cancel_response_ready);
+  EXPECT_FALSE(is_result_response_ready);
+}
+
+TEST_F(TestActionServerWait, test_server_wait_set_get_entities_ready) {
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  });
+
+  bool is_goal_request_ready = false;
+  bool is_cancel_request_ready = false;
+  bool is_result_request_ready = false;
+  bool is_goal_expired = false;
+
+  rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
+    nullptr,
+    &this->action_server,
+    &is_goal_request_ready,
+    &is_cancel_request_ready,
+    &is_result_request_ready,
+    &is_goal_expired);
+  EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_entities_ready(
+    &wait_set,
+    nullptr,
+    &is_goal_request_ready,
+    &is_cancel_request_ready,
+    &is_result_request_ready,
+    &is_goal_expired);
+  EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_entities_ready(
+    &wait_set,
+    &this->action_server,
+    nullptr,
+    &is_cancel_request_ready,
+    &is_result_request_ready,
+    &is_goal_expired);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_entities_ready(
+    &wait_set,
+    &this->action_server,
+    &is_goal_request_ready,
+    nullptr,
+    &is_result_request_ready,
+    &is_goal_expired);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+  ret = rcl_action_server_wait_set_get_entities_ready(
+    &wait_set,
+    &this->action_server,
+    &is_goal_request_ready,
+    &is_cancel_request_ready,
+    nullptr,
+    &is_goal_expired);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+  rcl_reset_error();
+
+
+  ret = rcl_action_server_wait_set_get_entities_ready(
+    &wait_set,
+    &this->action_server,
+    &is_goal_request_ready,
+    &is_cancel_request_ready,
+    &is_result_request_ready,
+    nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
+
+  ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator());
+  wait_set.services[0] = &this->action_server.impl->goal_service;
+  this->action_server.impl->wait_set_goal_service_index = 0;
+  wait_set.services[1] = &this->action_server.impl->cancel_service;
+  this->action_server.impl->wait_set_cancel_service_index = 1;
+  wait_set.services[2] = &this->action_server.impl->result_service;
+  this->action_server.impl->wait_set_result_service_index = 2;
+  wait_set.timers[0] = &this->action_server.impl->expire_timer;
+  this->action_server.impl->wait_set_expire_timer_index = 0;
+  ret = rcl_action_server_wait_set_get_entities_ready(
+    &wait_set,
+    &this->action_server,
+    &is_goal_request_ready,
+    &is_cancel_request_ready,
+    &is_result_request_ready,
+    &is_goal_expired);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  EXPECT_TRUE(is_goal_request_ready);
+  EXPECT_TRUE(is_cancel_request_ready);
+  EXPECT_TRUE(is_result_request_ready);
+  EXPECT_TRUE(is_goal_expired);
+}