feat: Added check for double usage of entities in rcl_waitset

Janosch Machowinski · Janosch Machowinski · commit 53a0d401b1eb · 2025-02-17T10:17:20.000+01:00
Adding entities twice to a waitset is not allowed, and will introduce
subtle bugs. Therefore the rcl_wait function will not explicitly check
for duplicated entries in the waitset.

Signed-off-by: Janosch Machowinski &lt;J.Machowinski@cellumation.com&gt;
diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c
@@ -38,20 +38,9 @@ extern "C"
 #include "rosidl_runtime_c/service_type_support_struct.h"
 
 #include "./common.h"
+#include "./client_impl.h"
 #include "./service_event_publisher.h"
 
-struct rcl_client_impl_s
-{
-  rcl_client_options_t options;
-  rmw_qos_profile_t actual_request_publisher_qos;
-  rmw_qos_profile_t actual_response_subscription_qos;
-  rmw_client_t * rmw_handle;
-  atomic_int_least64_t sequence_number;
-  rcl_service_event_publisher_t * service_event_publisher;
-  char * remapped_service_name;
-  rosidl_type_hash_t type_hash;
-};
-
 rcl_client_t
 rcl_get_zero_initialized_client(void)
 {
@@ -178,6 +167,7 @@ rcl_client_init(
   // options
   client->impl->options = *options;
   atomic_init(&client->impl->sequence_number, 0);
+  client->impl->in_use_by_waitset = false;
 
   const rosidl_type_hash_t * hash = type_support->get_type_hash_func(type_support);
   if (hash == NULL) {
diff --git a/rcl/src/rcl/client_impl.h b/rcl/src/rcl/client_impl.h
@@ -0,0 +1,37 @@
+// Copyright 2025 cellumation GmbH
+//
+// 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__CLIENT_IMPL_H_
+#define RCL__CLIENT_IMPL_H_
+
+#include <stdatomic.h>
+
+#include "rmw/rmw.h"
+#include "rcl/client.h"
+#include "./service_event_publisher.h"
+
+struct rcl_client_impl_s
+{
+  rcl_client_options_t options;
+  rmw_qos_profile_t actual_request_publisher_qos;
+  rmw_qos_profile_t actual_response_subscription_qos;
+  rmw_client_t * rmw_handle;
+  atomic_int_least64_t sequence_number;
+  rcl_service_event_publisher_t * service_event_publisher;
+  char * remapped_service_name;
+  rosidl_type_hash_t type_hash;
+  bool in_use_by_waitset;
+};
+
+#endif  // RCL__CLIENT_IMPL_H_
diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c
@@ -25,13 +25,8 @@ extern "C"
 #include "rmw/rmw.h"
 
 #include "./context_impl.h"
+#include "./guard_condition_impl.h"
 
-struct rcl_guard_condition_impl_s
-{
-  rmw_guard_condition_t * rmw_handle;
-  bool allocated_rmw_guard_condition;
-  rcl_guard_condition_options_t options;
-};
 
 rcl_guard_condition_t
 rcl_get_zero_initialized_guard_condition(void)
@@ -74,6 +69,7 @@ __rcl_guard_condition_init_from_rmw_impl(
     RCL_SET_ERROR_MSG("allocating memory failed");
     return RCL_RET_BAD_ALLOC;
   }
+  guard_condition->impl->in_use_by_waitset = false;
   // Create the rmw guard condition.
   if (rmw_guard_condition) {
     // If given, just assign (cast away const).
diff --git a/rcl/src/rcl/guard_condition_impl.h b/rcl/src/rcl/guard_condition_impl.h
@@ -0,0 +1,28 @@
+// Copyright 2025 cellumation GmbH
+//
+// 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__GUARD_CONDITION_IMPL_H_
+#define RCL__GUARD_CONDITION_IMPL_H_
+
+#include "rcl/guard_condition.h"
+
+struct rcl_guard_condition_impl_s
+{
+  rmw_guard_condition_t * rmw_handle;
+  bool allocated_rmw_guard_condition;
+  rcl_guard_condition_options_t options;
+  bool in_use_by_waitset;
+};
+
+#endif  // RCL__GUARD_CONDITION_IMPL_H_
diff --git a/rcl/src/rcl/publisher_impl.h b/rcl/src/rcl/publisher_impl.h
@@ -16,7 +16,6 @@
 #define RCL__PUBLISHER_IMPL_H_
 
 #include "rmw/rmw.h"
-
 #include "rcl/publisher.h"
 
 struct rcl_publisher_impl_s
diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c
@@ -39,17 +39,7 @@ extern "C"
 
 #include "./common.h"
 #include "./service_event_publisher.h"
-
-struct rcl_service_impl_s
-{
-  rcl_service_options_t options;
-  rmw_qos_profile_t actual_request_subscription_qos;
-  rmw_qos_profile_t actual_response_publisher_qos;
-  rmw_service_t * rmw_handle;
-  rcl_service_event_publisher_t * service_event_publisher;
-  char * remapped_service_name;
-  rosidl_type_hash_t type_hash;
-};
+#include "./service_impl.h"
 
 rcl_service_t
 rcl_get_zero_initialized_service(void)
@@ -189,6 +179,7 @@ rcl_service_init(
 
   // options
   service->impl->options = *options;
+  service->impl->in_use_by_waitset = false;
 
   if (RCL_RET_OK != rcl_node_type_cache_register_type(
       node, type_support->get_type_hash_func(type_support),
diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h
@@ -0,0 +1,34 @@
+// Copyright 2025 cellumation GmbH
+//
+// 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__SERVICE_IMPL_H_
+#define RCL__SERVICE_IMPL_H_
+
+#include "rmw/rmw.h"
+#include "rcl/service.h"
+#include "./service_event_publisher.h"
+
+struct rcl_service_impl_s
+{
+  rcl_service_options_t options;
+  rmw_qos_profile_t actual_request_subscription_qos;
+  rmw_qos_profile_t actual_response_publisher_qos;
+  rmw_service_t * rmw_handle;
+  rcl_service_event_publisher_t * service_event_publisher;
+  char * remapped_service_name;
+  rosidl_type_hash_t type_hash;
+  bool in_use_by_waitset;
+};
+
+#endif  // RCL__SERVICE_IMPL_H_
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
@@ -103,6 +103,7 @@ rcl_subscription_init(
 
   // options
   subscription->impl->options = *options;
+  subscription->impl->in_use_by_waitset = false;
 
   // Fill out the implemenation struct.
   // TODO(wjwwood): pass allocator once supported in rmw api.
diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h
@@ -25,6 +25,7 @@ struct rcl_subscription_impl_s
   rmw_qos_profile_t actual_qos;
   rmw_subscription_t * rmw_handle;
   rosidl_type_hash_t type_hash;
+  bool in_use_by_waitset;
 };
 
 #endif  // RCL__SUBSCRIPTION_IMPL_H_
diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c
@@ -26,34 +26,8 @@ extern "C"
 #include "rcutils/stdatomic_helper.h"
 #include "rcutils/time.h"
 #include "tracetools/tracetools.h"
+#include "./timer_impl.h"
 
-struct rcl_timer_impl_s
-{
-  // The clock providing time.
-  rcl_clock_t * clock;
-  // The associated context.
-  rcl_context_t * context;
-  // A guard condition used to wake the associated wait set, either when
-  // ROSTime causes the timer to expire or when the timer is reset.
-  rcl_guard_condition_t guard_condition;
-  // The user supplied callback.
-  atomic_uintptr_t callback;
-  // This is a duration in nanoseconds, which is initialized as int64_t
-  // to be used for internal time calculation.
-  atomic_int_least64_t period;
-  // This is a time in nanoseconds since an unspecified time.
-  atomic_int_least64_t last_call_time;
-  // This is a time in nanoseconds since an unspecified time.
-  atomic_int_least64_t next_call_time;
-  // Credit for time elapsed before ROS time is activated or deactivated.
-  atomic_int_least64_t time_credit;
-  // A flag which indicates if the timer is canceled.
-  atomic_bool canceled;
-  // The user supplied allocator.
-  rcl_allocator_t allocator;
-  // The user supplied on reset callback data.
-  rcl_timer_on_reset_callback_data_t callback_data;
-};
 
 rcl_timer_t
 rcl_get_zero_initialized_timer(void)
@@ -177,6 +151,7 @@ rcl_timer_init2(
   impl.callback_data.on_reset_callback = NULL;
   impl.callback_data.user_data = NULL;
   impl.callback_data.reset_counter = 0;
+  impl.in_use_by_waitset = false;
 
   timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
   if (NULL == timer->impl) {
diff --git a/rcl/src/rcl/timer_impl.h b/rcl/src/rcl/timer_impl.h
@@ -0,0 +1,51 @@
+// Copyright 2025 cellumation GmbH
+//
+// 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__TIMER_IMPL_H_
+#define RCL__TIMER_IMPL_H_
+
+#include <inttypes.h>
+#include <stdatomic.h>
+
+#include "rcl/timer.h"
+
+struct rcl_timer_impl_s
+{
+  // The clock providing time.
+  rcl_clock_t * clock;
+  // The associated context.
+  rcl_context_t * context;
+  // A guard condition used to wake the associated wait set, either when
+  // ROSTime causes the timer to expire or when the timer is reset.
+  rcl_guard_condition_t guard_condition;
+  // The user supplied callback.
+  atomic_uintptr_t callback;
+  // This is a duration in nanoseconds, which is initialized as int64_t
+  // to be used for internal time calculation.
+  atomic_int_least64_t period;
+  // This is a time in nanoseconds since an unspecified time.
+  atomic_int_least64_t last_call_time;
+  // This is a time in nanoseconds since an unspecified time.
+  atomic_int_least64_t next_call_time;
+  // Credit for time elapsed before ROS time is activated or deactivated.
+  atomic_int_least64_t time_credit;
+  // A flag which indicates if the timer is canceled.
+  atomic_bool canceled;
+  // The user supplied allocator.
+  rcl_allocator_t allocator;
+  // The user supplied on reset callback data.
+  rcl_timer_on_reset_callback_data_t callback_data;
+  bool in_use_by_waitset;
+};
+#endif  // RCL__TIMER_IMPL_H_
diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c
@@ -32,6 +32,11 @@ extern "C"
 #include "rmw/event.h"
 
 #include "./context_impl.h"
+#include "./client_impl.h"
+#include "./guard_condition_impl.h"
+#include "./service_impl.h"
+#include "./subscription_impl.h"
+#include "./timer_impl.h"
 
 struct rcl_wait_set_impl_s
 {
@@ -296,6 +301,22 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
   } \
   memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size);
 
+#define CHECK_DOUBLE_USAGE(Type) \
+  for (size_t idx = 0; idx < wait_set->size_of_ ## Type ## s; idx++) { \
+    if (wait_set->Type ## s[idx]) { \
+      wait_set->Type ## s[idx]->impl->in_use_by_waitset = false; \
+    } \
+  } \
+  for (size_t idx = 0; idx < wait_set->size_of_ ## Type ## s; idx++) { \
+    if (wait_set->Type ## s[idx]) { \
+      if(wait_set->Type ## s[idx]->impl->in_use_by_waitset) { \
+        RCL_SET_ERROR_MSG("Entitiy of type " #Type " added multiple times to waitset."); \
+        return RCL_RET_WAIT_SET_INVALID; \
+      } \
+      wait_set->Type ## s[idx]->impl->in_use_by_waitset = true; \
+    } \
+  }
+
 /* Implementation-specific notes:
  *
  * Add the rmw representation to the underlying rmw array and increment
@@ -524,6 +545,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     RCL_SET_ERROR_MSG("wait set is empty");
     return RCL_RET_WAIT_SET_EMPTY;
   }
+
+  CHECK_DOUBLE_USAGE(client);
+  CHECK_DOUBLE_USAGE(guard_condition);
+  CHECK_DOUBLE_USAGE(service);
+  CHECK_DOUBLE_USAGE(subscription);
+  CHECK_DOUBLE_USAGE(timer);
+
   // Calculate the timeout argument.
   // By default, set the timer to block indefinitely if none of the below conditions are met.
   rmw_time_t * timeout_argument = NULL;
