diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index c7da5846b..7529c783d 100644 --- 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 new file mode 100644 index 000000000..e366f19c1 --- /dev/null +++ b/rcl/src/rcl/client_impl.h @@ -0,0 +1,36 @@ +// 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 "rcl/client.h" +#include "rcutils/stdatomic_helper.h" +#include "rmw/rmw.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 index 957bbf996..b174d3ae0 100644 --- 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 new file mode 100644 index 000000000..a4436a290 --- /dev/null +++ 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 index 26bbb43da..f7e87c046 100644 --- 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 index 4a2aec5ac..97816a54d 100644 --- 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 new file mode 100644 index 000000000..3390ce4b6 --- /dev/null +++ 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 index 3c05f51cc..9f812395b 100644 --- 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 index 8193421cf..7af04ed9b 100644 --- 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 index c27c45c8f..b0b98d789 100644 --- 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 new file mode 100644 index 000000000..2148740af --- /dev/null +++ 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 + +#include "rcl/timer.h" +#include "rcutils/stdatomic_helper.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 index 145c1eae9..54283ae55 100644 --- 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,30 @@ 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); +/* + * Ensure that the waitset lists do not contain duplicated entries. + * For example, considering `wait_set->clients`: + * - first we set `in_use_by_waitset` to false for each entry. + * - then we loop again setting `in_use_by_waitset` to true one by one + * - if we find an entry where `in_use_by_waitset` was already true, it + * means that it was present twice in the waitset and we return an error +*/ +#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 +553,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; diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 841a0fec1..71b7193d3 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -25,6 +25,7 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" #include "rcl/rcl.h" +#include "rcl/timer.h" #include "rcl/wait.h" #include "rcutils/logging_macros.h" @@ -909,3 +910,78 @@ TEST_F(WaitSetTestFixture, wait_set_test_maybe_init_fail) { } }); } + +TEST_F(WaitSetTestFixture, duplicated_guard_condition) { + const size_t kNumEntities = 2u; + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, kNumEntities, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_guard_condition_t guard_condition; + guard_condition = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_condition, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + for (size_t i = 0u; i < kNumEntities; ++i) { + ret = rcl_wait_set_add_guard_condition( + &wait_set, &guard_condition, nullptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(&guard_condition, wait_set.guard_conditions[i]); + } + ret = rcl_wait(&wait_set, 0); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID); + rcutils_reset_error(); +} + +TEST_F(WaitSetTestFixture, duplicated_timer) { + const size_t kNumEntities = 2u; + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, 0, kNumEntities, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ASSERT_EQ(RCL_RET_OK, rcl_clock_init(RCL_ROS_TIME, &clock, &allocator)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init2( + &timer, &clock, this->context_ptr, RCL_S_TO_NS(1), nullptr, rcl_get_default_allocator(), + true); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + }); + for (size_t i = 0u; i < kNumEntities; ++i) { + ret = rcl_wait_set_add_timer( + &wait_set, &timer, nullptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(&timer, wait_set.timers[i]); + } + ret = rcl_wait(&wait_set, 0); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID); + rcutils_reset_error(); +}