diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index e9014b6..ce32052 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -61,6 +62,7 @@ typedef struct rcl_allocator_t * This function is lock-free. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_allocator_t rcl_get_default_allocator(); @@ -69,6 +71,7 @@ rcl_get_default_allocator(); * function pointer fields. */ RCL_PUBLIC +RCL_WARN_UNUSED void * rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator); diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 79b4d1d..c1f6c1a 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -41,6 +42,7 @@ typedef struct rcl_guard_condition_options_t /// Return a rcl_guard_condition_t struct with members set to NULL. RCL_PUBLIC +RCL_WARN_UNUSED rcl_guard_condition_t rcl_get_zero_initialized_guard_condition(); @@ -84,6 +86,7 @@ rcl_get_zero_initialized_guard_condition(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_guard_condition_init( rcl_guard_condition_t * guard_condition, @@ -106,6 +109,7 @@ rcl_guard_condition_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); @@ -115,6 +119,7 @@ rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * n * This function is lock-free. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_guard_condition_options_t rcl_guard_condition_get_default_options(); @@ -136,6 +141,7 @@ rcl_guard_condition_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); @@ -157,6 +163,7 @@ rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); * \return rmw guard_condition handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_guard_condition_t * rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition); diff --git a/rcl/include/rcl/macros.h b/rcl/include/rcl/macros.h new file mode 100644 index 0000000..2399307 --- /dev/null +++ b/rcl/include/rcl/macros.h @@ -0,0 +1,33 @@ +// Copyright 2015 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__MACROS_H_ +#define RCL__MACROS_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#ifndef _WIN32 +# define RCL_WARN_UNUSED __attribute__((warn_unused_result)) +#else +# define RCL_WARN_UNUSED _Check_return_ +#endif + +#if __cplusplus +} +#endif + +#endif // RCL__MACROS_H_ diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 6f6c54a..9850810 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -23,6 +23,7 @@ extern "C" #include #include "rcl/allocator.h" +#include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -59,6 +60,7 @@ typedef struct rcl_node_options_t /// Return a rcl_node_t struct with members initialized to NULL. RCL_PUBLIC +RCL_WARN_UNUSED rcl_node_t rcl_get_zero_initialized_node(); @@ -106,6 +108,7 @@ rcl_get_zero_initialized_node(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options); @@ -124,6 +127,7 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_node_fini(rcl_node_t * node); @@ -146,6 +150,7 @@ rcl_node_get_default_options(); * \return name string if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const char * rcl_node_get_name(const rcl_node_t * node); @@ -163,6 +168,7 @@ rcl_node_get_name(const rcl_node_t * node); * \return options struct if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node); @@ -185,6 +191,7 @@ rcl_node_get_options(const rcl_node_t * node); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); @@ -207,6 +214,7 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); * \return rmw node handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_node_t * rcl_node_get_rmw_handle(const rcl_node_t * node); @@ -226,6 +234,7 @@ rcl_node_get_rmw_handle(const rcl_node_t * node); * \return rcl instance id captured at node creation or 0 if there was an error */ RCL_PUBLIC +RCL_WARN_UNUSED uint64_t rcl_node_get_rcl_instance_id(const rcl_node_t * node); diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 2cc4583..6cc5d8b 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -22,6 +22,7 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -51,6 +52,7 @@ typedef struct rcl_publisher_options_t * being allocated on the heap. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_publisher_t rcl_get_zero_initialized_publisher(); @@ -126,6 +128,7 @@ rcl_get_zero_initialized_publisher(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_publisher_init( rcl_publisher_t * publisher, @@ -150,11 +153,13 @@ rcl_publisher_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); /// Return the default publisher options in a rcl_publisher_options_t. RCL_PUBLIC +RCL_WARN_UNUSED rcl_publisher_options_t rcl_publisher_get_default_options(); @@ -203,6 +208,7 @@ rcl_publisher_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); @@ -222,6 +228,7 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); * \return name string if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const char * rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); @@ -241,6 +248,7 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); * \return options struct if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const rcl_publisher_options_t * rcl_publisher_get_options(const rcl_publisher_t * publisher); @@ -262,6 +270,7 @@ rcl_publisher_get_options(const rcl_publisher_t * publisher); * \return rmw publisher handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_publisher_t * rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher); diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 21d5f04..0257473 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/publisher.h" #include "rcl/subscription.h" @@ -60,6 +61,7 @@ extern "C" * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator); @@ -88,6 +90,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * RCL_RET_ERROR if an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_shutdown(); @@ -102,6 +105,7 @@ rcl_shutdown(); * \return a unique id specific to this rcl instance, or 0 if not initialized. */ RCL_PUBLIC +RCL_WARN_UNUSED uint64_t rcl_get_instance_id(); @@ -112,6 +116,7 @@ rcl_get_instance_id(); * atomic_is_lock_free() returns true for atomic_uint_least64_t. */ RCL_PUBLIC +RCL_WARN_UNUSED bool rcl_ok(); diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 1c376dc..a417248 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -22,6 +22,7 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -53,6 +54,7 @@ typedef struct rcl_subscription_options_t * is being allocated on the heap. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_subscription_t rcl_get_zero_initialized_subscription(); @@ -127,6 +129,7 @@ rcl_get_zero_initialized_subscription(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_subscription_init( rcl_subscription_t * subscription, @@ -153,11 +156,13 @@ rcl_subscription_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); /// Return the default subscription options in a rcl_subscription_options_t. RCL_PUBLIC +RCL_WARN_UNUSED rcl_subscription_options_t rcl_subscription_get_default_options(); @@ -204,6 +209,7 @@ rcl_subscription_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_take( const rcl_subscription_t * subscription, @@ -227,6 +233,7 @@ rcl_take( * \return name string if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const char * rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); @@ -246,6 +253,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); * \return options struct if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const rcl_subscription_options_t * rcl_subscription_get_options(const rcl_subscription_t * subscription); @@ -269,6 +277,7 @@ rcl_subscription_get_options(const rcl_subscription_t * subscription); * \return rmw subscription handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_subscription_t * rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription); diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index c80c63e..60ccf5d 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -93,6 +94,7 @@ typedef struct rcl_duration_t * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_system_time_point_now(rcl_system_time_point_t * now); @@ -117,6 +119,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now); diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index a8b72a0..8753356 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -23,6 +23,7 @@ extern "C" #include #include "rcl/allocator.h" +#include "rcl/macros.h" #include "rcl/time.h" #include "rcl/types.h" #include "rmw/rmw.h" @@ -51,6 +52,7 @@ typedef void (* rcl_timer_callback_t)(rcl_timer_t *, uint64_t); /// Return a zero initialized timer. RCL_PUBLIC +RCL_WARN_UNUSED rcl_timer_t rcl_get_zero_initialized_timer(); @@ -112,6 +114,7 @@ rcl_get_zero_initialized_timer(); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_init( rcl_timer_t * timer, @@ -136,6 +139,7 @@ rcl_timer_init( * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_fini(rcl_timer_t * timer); @@ -169,6 +173,7 @@ rcl_timer_fini(rcl_timer_t * timer); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_call(rcl_timer_t * timer); @@ -193,6 +198,7 @@ rcl_timer_call(rcl_timer_t * timer); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); @@ -222,6 +228,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); @@ -248,6 +255,7 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call); @@ -268,6 +276,7 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_si * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); @@ -292,6 +301,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period); @@ -308,6 +318,7 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64 * \return function pointer to the callback, or NULL if an error occurred */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_timer_callback_t rcl_timer_get_callback(const rcl_timer_t * timer); @@ -326,6 +337,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer); * \return function pointer to the old callback, or NULL if an error occurred */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_timer_callback_t rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback); @@ -347,6 +359,7 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_ * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_cancel(rcl_timer_t * timer); @@ -368,6 +381,7 @@ rcl_timer_cancel(rcl_timer_t * timer); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); @@ -387,6 +401,7 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_reset(rcl_timer_t * timer); diff --git a/rcl/include/rcl/visibility_control.h b/rcl/include/rcl/visibility_control.h index 24b3a25..85e3b16 100644 --- a/rcl/include/rcl/visibility_control.h +++ b/rcl/include/rcl/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index ca67d19..c36300d 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -23,8 +23,9 @@ extern "C" #include #include -#include "rcl/subscription.h" #include "rcl/guard_condition.h" +#include "rcl/macros.h" +#include "rcl/subscription.h" #include "rcl/timer.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -49,6 +50,7 @@ typedef struct rcl_wait_set_t /// Return a rcl_wait_set_t struct with members set to NULL. RCL_PUBLIC +RCL_WARN_UNUSED rcl_wait_set_t rcl_get_zero_initialized_wait_set(); @@ -93,6 +95,7 @@ rcl_get_zero_initialized_wait_set(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_init( rcl_wait_set_t * wait_set, @@ -121,6 +124,7 @@ rcl_wait_set_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); @@ -138,6 +142,7 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator); @@ -157,6 +162,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, @@ -178,6 +184,7 @@ rcl_wait_set_add_subscription( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); @@ -207,6 +214,7 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); @@ -215,6 +223,7 @@ rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); * \see rcl_wait_set_add_subscription */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, @@ -225,6 +234,7 @@ rcl_wait_set_add_guard_condition( * \see rcl_wait_set_clear_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); @@ -233,6 +243,7 @@ rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); * \see rcl_wait_set_resize_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); @@ -241,6 +252,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * \see rcl_wait_set_add_subscription */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, @@ -251,6 +263,7 @@ rcl_wait_set_add_timer( * \see rcl_wait_set_clear_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); @@ -259,6 +272,7 @@ rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); * \see rcl_wait_set_resize_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); @@ -352,6 +366,7 @@ rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout); diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a214673..0323963 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -55,14 +55,18 @@ __wait_set_is_valid(const rcl_wait_set_t * wait_set) static void __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) { + rcl_ret_t ret; if (wait_set->subscriptions) { - rcl_wait_set_resize_subscriptions(wait_set, 0); + ret = rcl_wait_set_resize_subscriptions(wait_set, 0); + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } if (wait_set->guard_conditions) { - rcl_wait_set_resize_guard_conditions(wait_set, 0); + ret = rcl_wait_set_resize_guard_conditions(wait_set, 0); + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } if (wait_set->timers) { - rcl_wait_set_resize_timers(wait_set, 0); + ret = rcl_wait_set_resize_timers(wait_set, 0); + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } if (wait_set->impl) { allocator.deallocate(wait_set->impl, allocator.state); @@ -416,8 +420,11 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Check for timeout. if (ret == RMW_RET_TIMEOUT) { // Assume none were set (because timeout was reached first), and clear all. - rcl_wait_set_clear_subscriptions(wait_set); - rcl_wait_set_clear_guard_conditions(wait_set); + rcl_ret_t rcl_ret; + rcl_ret = rcl_wait_set_clear_subscriptions(wait_set); + assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. + rcl_ret = rcl_wait_set_clear_guard_conditions(wait_set); + assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. return RCL_RET_TIMEOUT; } // Set corresponding rcl subscription handles NULL.