From c1dfda97e128255ea50ea63d5e6a8b0ed59259a9 Mon Sep 17 00:00:00 2001
From: Shane Loretz <shane.loretz@gmail.com>
Date: Sat, 1 Dec 2018 08:55:07 -0800
Subject: [PATCH] add rcl_action_server_is_valid_except_context (#348)

---
 rcl_action/include/rcl_action/action_server.h | 24 +++++++++++++
 rcl_action/src/rcl_action/action_server.c     | 35 +++++++++++++++++--
 2 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h
index 86a103e..a51f90e 100644
--- a/rcl_action/include/rcl_action/action_server.h
+++ b/rcl_action/include/rcl_action/action_server.h
@@ -905,6 +905,30 @@ RCL_WARN_UNUSED
 bool
 rcl_action_server_is_valid(const rcl_action_server_t * action_server);
 
+/// Check if an action server is valid without erroring if the library is shutting down.
+/**
+ * In the case where `false` is returned (ie. the action server is invalid),
+ * an error message is set.
+ *
+ * This function cannot fail.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] action_server handle to the action server
+ * \return `true` if `action_server` is valid, or
+ * \return `false` otherwise.
+ */
+RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
+bool
+rcl_action_server_is_valid_except_context(const rcl_action_server_t * action_server);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index 2461edb..e4e558a 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -921,6 +921,35 @@ rcl_action_server_is_valid(const rcl_action_server_t * action_server)
   return true;
 }
 
+bool
+rcl_action_server_is_valid_except_context(const rcl_action_server_t * action_server)
+{
+  RCL_CHECK_FOR_NULL_WITH_MSG(action_server, "action server pointer is invalid", return false);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    action_server->impl, "action server implementation is invalid", return false);
+  if (!rcl_service_is_valid(&action_server->impl->goal_service)) {
+    RCL_SET_ERROR_MSG("goal service is invalid");
+    return false;
+  }
+  if (!rcl_service_is_valid(&action_server->impl->cancel_service)) {
+    RCL_SET_ERROR_MSG("cancel service is invalid");
+    return false;
+  }
+  if (!rcl_service_is_valid(&action_server->impl->result_service)) {
+    RCL_SET_ERROR_MSG("result service is invalid");
+    return false;
+  }
+  if (!rcl_publisher_is_valid_except_context(&action_server->impl->feedback_publisher)) {
+    RCL_SET_ERROR_MSG("feedback publisher is invalid");
+    return false;
+  }
+  if (!rcl_publisher_is_valid_except_context(&action_server->impl->status_publisher)) {
+    RCL_SET_ERROR_MSG("status publisher is invalid");
+    return false;
+  }
+  return true;
+}
+
 rcl_ret_t
 rcl_action_wait_set_add_action_server(
   rcl_wait_set_t * wait_set,
@@ -928,7 +957,7 @@ rcl_action_wait_set_add_action_server(
   size_t * service_index)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID);
-  if (!rcl_action_server_is_valid(action_server)) {
+  if (!rcl_action_server_is_valid_except_context(action_server)) {
     return RCL_RET_ACTION_SERVER_INVALID;  // error already set
   }
 
@@ -977,7 +1006,7 @@ rcl_action_server_wait_set_get_num_entities(
   size_t * num_clients,
   size_t * num_services)
 {
-  if (!rcl_action_server_is_valid(action_server)) {
+  if (!rcl_action_server_is_valid_except_context(action_server)) {
     return RCL_RET_ACTION_SERVER_INVALID;  // error already set
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(num_subscriptions, RCL_RET_INVALID_ARGUMENT);
@@ -1003,7 +1032,7 @@ rcl_action_server_wait_set_get_entities_ready(
   bool * is_goal_expired)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID);
-  if (!rcl_action_server_is_valid(action_server)) {
+  if (!rcl_action_server_is_valid_except_context(action_server)) {
     return RCL_RET_ACTION_SERVER_INVALID;  // error already set
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_request_ready, RCL_RET_INVALID_ARGUMENT);