From 5040c6049f2320fb34fb803ac97c4a458212102c Mon Sep 17 00:00:00 2001 From: iRobot ROS <49500531+irobot-ros@users.noreply.github.com> Date: Thu, 24 Feb 2022 08:32:11 +0000 Subject: [PATCH 1/2] Add Events Executor (#839) * Add RMW listener APIs Add set_guard_condition_callback api Move apis - Add constness Use or discard previous events: Guard conditions Rename to set_events_executor_callback Rename Event_callback -> ExecutorEventCallback update name Add events support void return on set_events_executor_callback Revert "void return on set_events_executor_callback" Rename ExecutorEventCallback -> EventsExecutorCallback Rename set_events_executor_callback->set_listener_callback Use data types when setting callbacks Move rcutils/executor_event_types.h to rmw/ rename event types Rename executor_context->callback_context Rename callback_context->user_data Reorder APIs arguments rename rmw_listener_cb_t->rmw_listener_callback_t use void * to pass executor ptr Rework executor callback data Use RMW renamed file Signed-off-by: Alberto Soragna * Remove use_previous_event Signed-off-by: Mauro Passerino * Remove guard condition listener Signed-off-by: Mauro Passerino * refactor to remove listener term and document Signed-off-by: William Woodall * allow rmw event callback to be NULL to unset them Signed-off-by: Alberto Soragna * Add APIs to support actions on EventsExecutor Signed-off-by: Mauro Passerino * fix messed up merge Signed-off-by: William Woodall Co-authored-by: Mauro Co-authored-by: William Woodall Co-authored-by: Alberto Soragna --- rcl/include/rcl/client.h | 32 ++++++++ rcl/include/rcl/event.h | 32 ++++++++ rcl/include/rcl/event_callback.h | 31 +++++++ rcl/include/rcl/service.h | 32 ++++++++ rcl/include/rcl/subscription.h | 33 ++++++++ rcl/src/rcl/client.c | 18 +++++ rcl/src/rcl/event.c | 17 ++++ rcl/src/rcl/service.c | 17 ++++ rcl/src/rcl/subscription.c | 17 ++++ rcl_action/include/rcl_action/action_client.h | 41 ++++++++++ rcl_action/include/rcl_action/action_server.h | 25 ++++++ rcl_action/src/rcl_action/action_client.c | 80 +++++++++++++++++++ rcl_action/src/rcl_action/action_server.c | 54 +++++++++++++ 13 files changed, 429 insertions(+) create mode 100644 rcl/include/rcl/event_callback.h diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index 94290982a..224f74b8c 100644 --- a/rcl/include/rcl/client.h +++ b/rcl/include/rcl/client.h @@ -24,6 +24,7 @@ extern "C" #include "rosidl_runtime_c/service_type_support_struct.h" +#include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -461,6 +462,37 @@ RCL_WARN_UNUSED const rmw_qos_profile_t * rcl_client_response_subscription_get_actual_qos(const rcl_client_t * client); +/// Set the on new response callback function for the client. +/** + * This API sets the callback function to be called whenever the + * client is notified about a new response. + * + * \sa rmw_client_set_on_new_response_callback for details about this function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] client The client on which to set the callback + * \param[in] callback The callback to be called when new responses arrive, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if callback was set to the listener, or + * \return `RCL_RET_INVALID_ARGUMENT` if `client` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_set_on_new_response_callback( + const rcl_client_t * client, + rcl_event_callback_t callback, + const void * user_data); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/event.h b/rcl/include/rcl/event.h index abf4e6386..fea87d782 100644 --- a/rcl/include/rcl/event.h +++ b/rcl/include/rcl/event.h @@ -25,6 +25,7 @@ extern "C" #include #include "rcl/client.h" +#include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/publisher.h" #include "rcl/service.h" @@ -198,6 +199,37 @@ RCL_PUBLIC bool rcl_event_is_valid(const rcl_event_t * event); +/// Set the callback function for the event. +/** + * This API sets the callback function to be called whenever the + * event is notified about a new instance of the event. + * + * \sa rmw_event_set_callback for more details about this function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] event The event on which to set the callback + * \param[in] callback The callback to be called when new events occur, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if callback was set to the listener, or + * \return `RCL_RET_INVALID_ARGUMENT` if `event` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_event_set_callback( + const rcl_event_t * event, + rcl_event_callback_t callback, + const void * user_data); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/event_callback.h b/rcl/include/rcl/event_callback.h new file mode 100644 index 000000000..9124907ab --- /dev/null +++ b/rcl/include/rcl/event_callback.h @@ -0,0 +1,31 @@ +// Copyright 2021 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__EVENT_CALLBACK_H_ +#define RCL__EVENT_CALLBACK_H_ + +#include "rmw/event_callback_type.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef rmw_event_callback_t rcl_event_callback_t; + +#ifdef __cplusplus +} +#endif + +#endif // RCL__EVENT_CALLBACK_H_ diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index b102d14c9..2461bd551 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -24,6 +24,7 @@ extern "C" #include "rosidl_runtime_c/service_type_support_struct.h" +#include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -492,6 +493,37 @@ RCL_WARN_UNUSED const rmw_qos_profile_t * rcl_service_response_publisher_get_actual_qos(const rcl_service_t * service); +/// Set the on new request callback function for the service. +/** + * This API sets the callback function to be called whenever the + * service is notified about a new request. + * + * \sa rmw_service_set_on_new_request_callback for details about this function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] service The service on which to set the callback + * \param[in] callback The callback to be called when new requests arrive, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if callback was set to the listener, or + * \return `RCL_RET_INVALID_ARGUMENT` if `service` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_set_on_new_request_callback( + const rcl_service_t * service, + rcl_event_callback_t callback, + const void * user_data); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 09d6e6bc4..121949999 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -24,6 +24,7 @@ extern "C" #include "rosidl_runtime_c/message_type_support_struct.h" +#include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -611,6 +612,38 @@ RCL_PUBLIC bool rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription); +/// Set the on new message callback function for the subscription. +/** + * This API sets the callback function to be called whenever the + * subscription is notified about a new message. + * + * \sa rmw_subscription_set_on_new_message_callback for details about this + * function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] subscription The subscription on which to set the callback + * \param[in] callback The callback to be called when new messages arrive, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_set_on_new_message_callback( + const rcl_subscription_t * subscription, + rcl_event_callback_t callback, + const void * user_data); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index bd38efa7a..432948d59 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -327,6 +327,24 @@ rcl_client_response_subscription_get_actual_qos(const rcl_client_t * client) } return &client->impl->actual_response_subscription_qos; } + +rcl_ret_t +rcl_client_set_on_new_response_callback( + const rcl_client_t * client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_client_is_valid(client)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_client_set_on_new_response_callback( + client->impl->rmw_handle, + callback, + user_data); +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/event.c b/rcl/src/rcl/event.c index c79c60ce6..6196442d1 100644 --- a/rcl/src/rcl/event.c +++ b/rcl/src/rcl/event.c @@ -218,6 +218,23 @@ rcl_event_is_valid(const rcl_event_t * event) return true; } +rcl_ret_t +rcl_event_set_callback( + const rcl_event_t * event, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_event_is_valid(event)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_event_set_callback( + &event->impl->rmw_handle, + callback, + user_data); +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 42222b996..e1bdb0174 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -346,6 +346,23 @@ rcl_service_response_publisher_get_actual_qos(const rcl_service_t * service) return &service->impl->actual_response_publisher_qos; } +rcl_ret_t +rcl_service_set_on_new_request_callback( + const rcl_service_t * service, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_service_is_valid(service)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_service_set_on_new_request_callback( + service->impl->rmw_handle, + callback, + user_data); +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index d92fe21a4..32c3ca767 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -445,6 +445,23 @@ rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription) return subscription->impl->rmw_handle->can_loan_messages; } +rcl_ret_t +rcl_subscription_set_on_new_message_callback( + const rcl_subscription_t * subscription, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_subscription_is_valid(subscription)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_subscription_set_on_new_message_callback( + subscription->impl->rmw_handle, + callback, + user_data); +} + #ifdef __cplusplus } #endif diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index 7fdf6df78..b686b65f1 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -22,6 +22,7 @@ extern "C" #include "rcl_action/types.h" #include "rcl_action/visibility_control.h" +#include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" @@ -741,6 +742,46 @@ bool rcl_action_client_is_valid( const rcl_action_client_t * action_client); +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_client_set_goal_client_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data); + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_client_set_cancel_client_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data); + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_client_set_result_client_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data); + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_client_set_feedback_subscription_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data); + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_client_set_status_subscription_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data); + #ifdef __cplusplus } #endif diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h index d0d9a8e3a..eb4f95ce9 100644 --- a/rcl_action/include/rcl_action/action_server.h +++ b/rcl_action/include/rcl_action/action_server.h @@ -23,6 +23,7 @@ extern "C" #include "rcl_action/goal_handle.h" #include "rcl_action/types.h" #include "rcl_action/visibility_control.h" +#include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" #include "rcl/time.h" @@ -930,6 +931,30 @@ RCL_WARN_UNUSED bool rcl_action_server_is_valid_except_context(const rcl_action_server_t * action_server); +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_set_goal_service_callback( + const rcl_action_server_t * action_server, + rcl_event_callback_t callback, + const void * user_data); + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_set_cancel_service_callback( + const rcl_action_server_t * action_server, + rcl_event_callback_t callback, + const void * user_data); + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_set_result_service_callback( + const rcl_action_server_t * action_server, + rcl_event_callback_t callback, + const void * user_data); + #ifdef __cplusplus } #endif diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index a03a61ec3..d999f1e01 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -649,6 +649,86 @@ rcl_action_client_wait_set_get_entities_ready( return RCL_RET_OK; } +rcl_ret_t +rcl_action_client_set_goal_client_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; + } + + return rcl_client_set_on_new_response_callback( + &action_client->impl->goal_client, + callback, + user_data); +} + +rcl_ret_t +rcl_action_client_set_cancel_client_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; + } + + return rcl_client_set_on_new_response_callback( + &action_client->impl->cancel_client, + callback, + user_data); +} + +rcl_ret_t +rcl_action_client_set_result_client_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; + } + + return rcl_client_set_on_new_response_callback( + &action_client->impl->result_client, + callback, + user_data); +} + +rcl_ret_t +rcl_action_client_set_feedback_subscription_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; + } + + return rcl_subscription_set_on_new_message_callback( + &action_client->impl->feedback_subscription, + callback, + user_data); +} + +rcl_ret_t +rcl_action_client_set_status_subscription_callback( + const rcl_action_client_t * action_client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; + } + + return rcl_subscription_set_on_new_message_callback( + &action_client->impl->status_subscription, + callback, + user_data); +} + #ifdef __cplusplus } #endif diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index a5b6dbde4..63580c40f 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -1054,6 +1054,60 @@ rcl_action_server_wait_set_get_entities_ready( return RCL_RET_OK; } +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_set_goal_service_callback( + const rcl_action_server_t * action_server, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_server_is_valid_except_context(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; + } + + return rcl_service_set_on_new_request_callback( + &action_server->impl->goal_service, + callback, + user_data); +} + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_set_result_service_callback( + const rcl_action_server_t * action_server, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_server_is_valid_except_context(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; + } + + return rcl_service_set_on_new_request_callback( + &action_server->impl->result_service, + callback, + user_data); +} + +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_set_cancel_service_callback( + const rcl_action_server_t * action_server, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_action_server_is_valid_except_context(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; + } + + return rcl_service_set_on_new_request_callback( + &action_server->impl->cancel_service, + callback, + user_data); +} + #ifdef __cplusplus } #endif From f5b51093c270f9d7f83a9cfea746d8311971968c Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 28 Feb 2022 11:10:40 +0000 Subject: [PATCH 2/2] Add rcl_timer_get_next_call_time timer API Signed-off-by: Mauro Passerino --- rcl/include/rcl/timer.h | 27 +++++++++++++++++++++++++++ rcl/src/rcl/timer.c | 10 ++++++++++ 2 files changed, 37 insertions(+) diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 1bf9212d7..2e4157041 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -325,6 +325,33 @@ RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); +/// Retrieve the time point value for the next timer call. +/** + * This function will populate the data of the time_point_value object with the + * value corresponding to when the next timer call should happen. + * + * The `time_point_value` argument must point to an allocated rcl_time_point_value_t, as + * the time point is copied into that instance. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer that is being queried + * \param[out] time_point_value the output variable for the result + * \return #RCL_RET_OK if the timer until next call was successfully calculated, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_get_next_call_time(const rcl_timer_t * timer, rcl_time_point_value_t * time_point_value); + /// Retrieve the time since the previous call to rcl_timer_call() occurred. /** * This function calculates the time since the last call and copies it into diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index b672e5156..99a7ee829 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -319,6 +319,16 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt return RCL_RET_OK; } +rcl_ret_t +rcl_timer_get_next_call_time(const rcl_timer_t * timer, rcl_time_point_value_t * time_point_value) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(time_point_value, RCL_RET_INVALID_ARGUMENT); + + *time_point_value = rcutils_atomic_load_int64_t(&timer->impl->next_call_time); + return RCL_RET_OK; +} + rcl_ret_t rcl_timer_get_time_since_last_call( const rcl_timer_t * timer,