diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 8256aa8..482d2e0 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -18,11 +18,13 @@ endif() set(${PROJECT_NAME}_sources src/rcl/allocator.c + src/rcl/client.c src/rcl/common.c src/rcl/guard_condition.c src/rcl/node.c src/rcl/publisher.c src/rcl/rcl.c + src/rcl/service.c src/rcl/subscription.c src/rcl/wait.c src/rcl/time.c diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h new file mode 100644 index 0000000..a8694e6 --- /dev/null +++ b/rcl/include/rcl/client.h @@ -0,0 +1,301 @@ +// Copyright 2016 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__CLIENT_H_ +#define RCL__CLIENT_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rosidl_generator_c/service_type_support.h" + +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" + +/// Internal rcl client implementation struct. +struct rcl_client_impl_t; + +/// Handle for a rcl client. +typedef struct rcl_client_t +{ + struct rcl_client_impl_t * impl; +} rcl_client_t; + +/// Options available for a rcl client. +typedef struct rcl_client_options_t +{ + /// Middleware quality of service settings for the client. + rmw_qos_profile_t qos; + /// Custom allocator for the client, used for incidental allocations. + /* For default behavior (malloc/free), use: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_client_options_t; + +/// Return a rcl_client_t struct with members set to NULL. +/* Should be called to get a null rcl_client_t before passing to + * rcl_initalize_client(). + * It's also possible to use calloc() instead of this if the rcl_client is + * being allocated on the heap. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_client_t +rcl_get_zero_initialized_client(void); + + +/// Initialize a rcl client. +/* After calling this function on a rcl_client_t, it can be used to send requests of the given + * type by calling rcl_send_request(). + * If the request is received by a (possibly remote) service and if the service sends a response, + * the client can access the response through rcl_take_response once the response is available to + * the client. + * + * The given rcl_node_t must be valid and the resulting rcl_client_t is only + * valid as long as the given rcl_node_t remains valid. + * + * The rosidl_service_type_support_t is obtained on a per .srv type basis. + * When the user defines a ROS service, code is generated which provides the + * required rosidl_service_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \TODO(wjwwood) probably should talk about this once and link to it instead + * For C this macro can be used (using example_interfaces/AddTwoInts as an example): + * + * #include + * #include + * rosidl_service_type_support_t * ts = + * ROSIDL_GET_SERVICE_TYPE_SUPPORT(example_interfaces, AddTwoInts); + * + * For C++ a template function is used: + * + * #include + * #include + * rosidl_service_type_support_t * ts = rosidl_generator_cpp::get_service_type_support_handle< + * example_interfaces::srv::AddTwoInts>(); + * + * The rosidl_service_type_support_t object contains service type specific + * information used to send or take requests and responses. + * + * \TODO(wjwwood) update this once we've come up with an official scheme. + * The service name must be a non-empty string which follows the topic/service naming + * format. + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when initializing/finalizing the + * client to allocate space for incidentals, e.g. the service name string. + * + * Expected usage (for C services): + * + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops); + * // ... error handling + * rosidl_service_type_support_t * ts = ROSIDL_GET_SERVICE_TYPE_SUPPORT( + * example_interfaces, AddTwoInts); + * rcl_client_t client = rcl_get_zero_initialized_client(); + * rcl_client_options_t client_ops = rcl_client_get_default_options(); + * ret = rcl_client_init(&client, &node, ts, "add_two_ints", &client_ops); + * // ... error handling, and on shutdown do finalization: + * ret = rcl_client_fini(&client, &node); + * // ... error handling for rcl_client_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * + * This function is not thread-safe. + * + * \param[inout] client preallocated client structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the service's type + * \param[in] service_name the name of the service to request + * \param[in] options client options, including quality of service settings + * \return RCL_RET_OK if the client was initialized successfully, or + * RCL_RET_NODE_INVALID if the node is invalid, or + * RCL_RET_ALREADY_INIT if the client is already initialized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory fails, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_init( + rcl_client_t * client, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_client_options_t * options); + +/// Finalize a rcl_client_t. +/* + * After calling, calls to rcl_send_request and rcl_take_response will fail when using this client. + * However, the given node handle is still valid. + * + * This function is not thread-safe. + * + * \param[inout] client handle to the client to be finalized + * \param[in] node handle to the node used to create the client + * \return RCL_RET_OK if client was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_fini(rcl_client_t * client, rcl_node_t * node); + +/// Return the default client options in a rcl_client_options_t. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_client_options_t +rcl_client_get_default_options(void); + +/// Send a ROS request using a client. +/* It is the job of the caller to ensure that the type of the ros_request + * parameter and the type associate with the client (via the type support) + * match. + * Passing a different type to send_request produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * send_request is an non-blocking call. + * + * The ROS request message given by the ros_request void pointer is always owned by the + * calling code, but should remain constant during send_request. + * + * This function is thread safe so long as access to both the client and the + * ros_request is synchronized. + * That means that calling rcl_send_request from multiple threads is allowed, but + * calling rcl_send_request at the same time as non-thread safe client functions + * is not, e.g. calling rcl_send_request and rcl_client_fini concurrently + * is not allowed. + * Before calling rcl_send_request the message can change and after calling + * rcl_send_request the message can change, but it cannot be changed during the + * send_request call. + * The same ros_request, however, can be passed to multiple calls of + * rcl_send_request simultaneously, even if the clients differ. + * The ros_request is unmodified by rcl_send_request. + * + * \param[in] client handle to the client which will make the response + * \param[in] ros_request type-erased pointer to the ROS request message + * \param[out] sequence_number the sequence number + * \return RCL_RET_OK if the request was sent successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_CLIENT_INVALID if the client is invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number); + + +// Take a ROS response using a client +/* It is the job of the caller to ensure that the type of the ros_response + * parameter and the type associate with the client (via the type support) + * match. + * Passing a different type to take_response produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * The request_header is an rmw struct for meta-information about the request sent + * (e.g. the sequence number). + * ros_response should point to an already allocated ROS response message struct of the + * correct type, into which the response from the service will be copied. + * + * \param[in] client handle to the client which will take the response + * \param[inout] request_header pointer to the request header + * \param[inout] ros_response type-erased pointer to the ROS response message + * \return RCL_RET_OK if the response was taken successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_CLIENT_INVALID if the client is invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_response( + const rcl_client_t * client, + rmw_request_id_t * request_header, + void * ros_response); + +/// Get the name of the service that this client will request a response from. +/* This function returns the client's internal service name string. + * This function can fail, and therefore return NULL, if the: + * - client is NULL + * - client is invalid (never called init, called fini, or invalid node) + * + * The returned string is only valid as long as the rcl_client_t is valid. + * The value of the string may change if the service name changes, and therefore + * copying the string is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * + * \param[in] client pointer to the client + * \return name string if successful, otherwise NULL + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_client_get_service_name(const rcl_client_t * client); + +/// Return the rcl client options. +/* This function returns the client's internal options struct. + * This function can fail, and therefore return NULL, if the: + * - client is NULL + * - client is invalid (never called init, called fini, or invalid node) + * + * The returned struct is only valid as long as the rcl_client_t is valid. + * The values in the struct may change if the options of the client change, + * and therefore copying the struct is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * + * \param[in] client pointer to the client + * \return options struct if successful, otherwise NULL + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_client_options_t * +rcl_client_get_options(const rcl_client_t * client); + +/// Return the rmw client handle. +/* The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return NULL, if the: + * - client is NULL + * - client is invalid (never called init, called fini, or invalid node) + * + * The returned handle is made invalid if the client is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * client as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the client using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + * \param[in] client pointer to the rcl client + * \return rmw client handle if successful, otherwise NULL + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_client_t * +rcl_client_get_rmw_handle(const rcl_client_t * client); + +#if __cplusplus +} +#endif + +#endif // RCL__CLIENT_H_ diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index d52cfad..d1347a4 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -68,7 +68,7 @@ rcl_get_zero_initialized_guard_condition(void); * ret = rcl_guard_condition_fini(&guard_condition, &node); * // ... error handling for rcl_guard_condition_fini() * ret = rcl_node_fini(&node); - * // ... error handling for rcl_deinitialize_node() + * // ... error handling for rcl_node_fini() * * This function does allocate heap memory. * This function is not thread-safe. diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h new file mode 100644 index 0000000..b20b4bf --- /dev/null +++ b/rcl/include/rcl/service.h @@ -0,0 +1,317 @@ +// Copyright 2016 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__SERVICE_H_ +#define RCL__SERVICE_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rosidl_generator_c/service_type_support.h" + +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" + +/// Internal rcl implementation struct. +struct rcl_service_impl_t; + +/// Handle for a rcl service. +typedef struct rcl_service_t +{ + struct rcl_service_impl_t * impl; +} rcl_service_t; + +/// Options available for a rcl service. +typedef struct rcl_service_options_t +{ + /// Middleware quality of service settings for the service. + rmw_qos_profile_t qos; + /// Custom allocator for the service, used for incidental allocations. + /* For default behavior (malloc/free), see: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_service_options_t; + +/// Return a rcl_service_t struct with members set to NULL. +/* Should be called to get a null rcl_service_t before passing to + * rcl_initalize_service(). + * It's also possible to use calloc() instead of this if the rcl_service_t + * is being allocated on the heap. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_t +rcl_get_zero_initialized_service(void); + +/// Initialize a rcl service. +/* After calling this function on a rcl_service_t, it can be used to take + * requests of the given type to the given topic using rcl_take_request(). + * It can also send a response to a request using rcl_send_response(). + * + * The given rcl_node_t must be valid and the resulting rcl_service_t is + * only valid as long as the given rcl_node_t remains valid. + * + * The rosidl_service_type_support_t is obtained on a per .srv type basis. + * When the user defines a ROS service, code is generated which provides the + * required rosidl_service_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \TODO(wjwwood) probably should talk about this once and link to it instead + * \TODO(jacquelinekay) reworded this for services with substitutions, should it refer to messages? + * For C this macro can be used (using example_interfaces/AddTwoInts as an example): + * + * #include + * #include + * rosidl_service_type_support_t * ts = + * ROSIDL_GET_SERVICE_TYPE_SUPPORT(example_interfaces, AddTwoInts); + * + * For C++ a template function is used: + * + * #include + * #include + * rosidl_service_type_support_t * ts = rosidl_generator_cpp::get_service_type_support_handle< + * example_interfaces::srv::AddTwoInts>(); + * + * The rosidl_service_type_support_t object contains service type specific + * information used to send or take requests and responses. + * + * \TODO(wjwwood) update this once we've come up with an official scheme. + * The service name must be a non-empty string which follows the service/topic naming + * format. + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when initializing/finalizing the + * client to allocate space for incidentals, e.g. the service name string. + * + * Expected usage (for C services): + * + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops); + * // ... error handling + * rosidl_service_type_support_t * ts = ROSIDL_GET_SERVICE_TYPE_SUPPORT( + * example_interfaces, AddTwoInts); + * rcl_service_t service = rcl_get_zero_initialized_service(); + * rcl_service_options_t service_ops = rcl_service_get_default_options(); + * ret = rcl_service_init(&service, &node, ts, "add_two_ints", &service_ops); + * // ... error handling, and on shutdown do finalization: + * ret = rcl_service_fini(&service, &node); + * // ... error handling for rcl_service_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * + * \param[out] service preallocated service structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the service's type + * \param[in] service_name the name of the service + * \param[in] options service options, including quality of service settings + * \return RCL_RET_OK if service was initialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_init( + rcl_service_t * service, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_service_options_t * options); + +/// Finalize a rcl_service_t. +/* After calling, the node will no longer listen for requests for this service. + * (assuming this is the only service of this type in this node). + * + * After calling, calls to rcl_wait, rcl_take_request, and rcl_send_response will fail when using + * this service. + * Additionally rcl_wait will be interrupted if currently blocking. + * However, the given node handle is still valid. + * + * This function is not thread-safe. + * + * \param[inout] service handle to the service to be deinitialized + * \param[in] node handle to the node used to create the service + * \return RCL_RET_OK if service was deinitialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_fini(rcl_service_t * service, rcl_node_t * node); + +/// Return the default service options in a rcl_service_options_t. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_options_t +rcl_service_get_default_options(void); + +/// Take a pending ROS request using a rcl service. +/* It is the job of the caller to ensure that the type of the ros_request + * argument and the type associate with the service, via the type + * support, match. + * Passing a different type to rcl_take produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * TODO(jacquelinekay) blocking of take? + * TODO(jacquelinekay) pre-, during-, and post-conditions for message ownership? + * TODO(jacquelinekay) is rcl_take_request thread-safe? + * TODO(jacquelinekay) Should there be an rcl_request_id_t? + * + * The ros_request pointer should point to an already allocated ROS request message + * struct of the correct type, into which the taken ROS request will be copied + * if one is available. + * If taken is false after calling, then the ROS request will be unmodified. + * + * If allocation is required when taking the request, e.g. if space needs to + * be allocated for a dynamically sized array in the target message, then the + * allocator given in the service options is used. + * + * request_header is a pointer to pre-allocated a rmw struct containing + * meta-information about the request (e.g. the sequence number). + * + * \param[in] service the handle to the service from which to take + * \param[inout] request_header ptr to the struct holding metadata about the request ID + * \param[inout] ros_request type-erased ptr to an allocated ROS request message + * \return RCL_RET_OK if the request was taken, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_SERVICE_INVALID if the service is invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_request( + const rcl_service_t * service, + rmw_request_id_t * request_header, + void * ros_request); + +/// Send a ROS response to a client using a service. +/* It is the job of the caller to ensure that the type of the ros_response + * parameter and the type associate with the service (via the type support) + * match. + * Passing a different type to send_response produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * send_response is an non-blocking call. + * + * The ROS response message given by the ros_response void pointer is always owned by the + * calling code, but should remain constant during send_response. + * + e This function is thread safe so long as access to both the service and the + * ros_response is synchronized. + * That means that calling rcl_send_response from multiple threads is allowed, but + * calling rcl_send_response at the same time as non-thread safe service functions + * is not, e.g. calling rcl_send_response and rcl_service_fini concurrently + * is not allowed. + * Before calling rcl_send_response the message can change and after calling + * rcl_send_response the message can change, but it cannot be changed during the + * send_response call. + * The same ros_response, however, can be passed to multiple calls of + * rcl_send_response simultaneously, even if the services differ. + * The ros_response is unmodified by rcl_send_response. + * + * \param[in] service handle to the service which will make the response + * \param[inout] response_header ptr to the struct holding metadata about the request ID + * \param[in] ros_response type-erased pointer to the ROS response message + * \return RCL_RET_OK if the response was sent successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_CLIENT_INVALID if the service is invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_send_response( + const rcl_service_t * service, + rmw_request_id_t * response_header, + void * ros_response); + +/// Get the topic name for the service. +/* This function returns the service's internal topic name string. + * This function can fail, and therefore return NULL, if the: + * - service is NULL + * - service is invalid (never called init, called fini, or invalid) + * + * The returned string is only valid as long as the service is valid. + * The value of the string may change if the topic name changes, and therefore + * copying the string is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * + * \param[in] service the pointer to the service + * \return name string if successful, otherwise NULL + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_service_get_service_name(const rcl_service_t * service); + +/// Return the rcl service options. +/* This function returns the service's internal options struct. + * This function can fail, and therefore return NULL, if the: + * - service is NULL + * - service is invalid (never called init, called fini, or invalid) + * + * The returned struct is only valid as long as the service is valid. + * The values in the struct may change if the service's options change, + * and therefore copying the struct is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * + * \param[in] service pointer to the service + * \return options struct if successful, otherwise NULL + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_service_options_t * +rcl_service_get_options(const rcl_service_t * service); + +/// Return the rmw service handle. +/* The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return NULL, if the: + * - service is NULL + * - service is invalid (never called init, called fini, or invalid) + * + * The returned handle is made invalid if the service is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * service as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the service using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + * This function is not thread-safe. + * + * \param[in] service pointer to the rcl service + * \return rmw service handle if successful, otherwise NULL + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_service_t * +rcl_service_get_rmw_handle(const rcl_service_t * service); + +#if __cplusplus +} +#endif + +#endif // RCL__SERVICE_H_ diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index b3482ce..8d660b5 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -114,7 +114,7 @@ rcl_get_zero_initialized_subscription(void); * ret = rcl_subscription_fini(&subscription, &node); * // ... error handling for rcl_subscription_fini() * ret = rcl_node_fini(&node); - * // ... error handling for rcl_deinitialize_node() + * // ... error handling for rcl_node_fini() * * This function is not thread-safe. * @@ -138,7 +138,7 @@ rcl_subscription_init( const char * topic_name, const rcl_subscription_options_t * options); -/// Deinitialize a rcl_subscription_t. +/// Finalize a rcl_subscription_t. /* After calling, the node will no longer be subscribed on this topic * (assuming this is the only subscription on this topic in this node). * @@ -206,6 +206,8 @@ rcl_subscription_get_default_options(void); * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_SUBSCRIPTION_TAKE_FAILED if take failed but no error + * occurred in the middleware, or * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC @@ -214,7 +216,6 @@ rcl_ret_t rcl_take( const rcl_subscription_t * subscription, void * ros_message, - bool * taken, rmw_message_info_t * message_info); /// Get the topic name for the subscription. diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index ba2d8a3..34877f3 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -32,8 +32,13 @@ typedef rmw_ret_t rcl_ret_t; #define RCL_RET_PUBLISHER_INVALID 300 // rcl subscription specific ret codes in 4XX #define RCL_RET_SUBSCRIPTION_INVALID 400 +#define RCL_RET_SUBSCRIPTION_TAKE_FAILED 501 // rcl service client specific ret codes in 5XX +#define RCL_RET_CLIENT_INVALID 500 +#define RCL_RET_CLIENT_TAKE_FAILED 501 // rcl service server specific ret codes in 6XX +#define RCL_RET_SERVICE_INVALID 600 +#define RCL_RET_SERVICE_TAKE_FAILED 601 // rcl guard condition specific ret codes in 7XX // rcl timer specific ret codes in 8XX #define RCL_RET_TIMER_INVALID 800 diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index f1608c8..5417433 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -23,8 +23,10 @@ extern "C" #include #include +#include "rcl/client.h" #include "rcl/guard_condition.h" #include "rcl/macros.h" +#include "rcl/service.h" #include "rcl/subscription.h" #include "rcl/timer.h" #include "rcl/types.h" @@ -44,6 +46,12 @@ typedef struct rcl_wait_set_t /// Storage for timer pointers. const rcl_timer_t ** timers; size_t size_of_timers; + /// Storage for client pointers. + const rcl_client_t ** clients; + size_t size_of_clients; + /// Storage for service pointers. + const rcl_service_t ** services; + size_t size_of_services; /// Implementation specific storage. struct rcl_wait_set_impl_t * impl; } rcl_wait_set_t; @@ -89,6 +97,8 @@ rcl_get_zero_initialized_wait_set(void); * \param[in] number_of_subscriptions non-zero size of the subscriptions set * \param[in] number_of_guard_conditions non-zero size of the guard conditions set * \param[in] number_of_timers non-zero size of the timers set + * \param[in] number_of_clients non-zero size of the clients set + * \param[in] number_of_services non-zero size of the services set * \param[in] allocator the allocator to use when allocating space in the sets * \return RCL_RET_OK if the wait set is initialized successfully, or * RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or @@ -104,6 +114,8 @@ rcl_wait_set_init( size_t number_of_subscriptions, size_t number_of_guard_conditions, size_t number_of_timers, + size_t number_of_clients, + size_t number_of_services, rcl_allocator_t allocator); /// Finalize a rcl wait set. @@ -278,6 +290,150 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); +/// Store a pointer to the given client in the next empty spot in the set. +/* This function does not guarantee that the client is not already in the + * wait set. + * + * This function is not thread-safe. + * This function is lock-free. + * + * \param[inout] wait_set struct in which the client is to be stored + * \param[in] client the client to be added to the wait set + * \return RCL_RET_OK if added successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * RCL_RET_WAIT_SET_FULL if the client set is full, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_client( + rcl_wait_set_t * wait_set, + const rcl_client_t * client); + +/// Remove (sets to NULL) the clients in the wait set. +/* This function should be used after passing using rcl_wait, but before + * adding new clients to the set. + * + * Calling this on an uninitialized (zero initialized) wait set will fail. + * + * This function is not thread-safe. + * This function is lock-free. + * + * \param[inout] wait_set struct to have its clients cleared + * \return RCL_RET_OK if cleared successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_clear_clients(rcl_wait_set_t * wait_set); + +/// Reallocate space for the clients in the wait set. +/* This function will deallocate and reallocate the memory for the + * clients set. + * + * A size of 0 will just deallocate the memory and assign NULL to the array. + * + * Allocation and deallocation is done with the allocator given during the + * wait set's initialization. + * + * After calling this function all values in the set will be set to NULL, + * effectively the same as calling rcl_wait_set_clear_clients(). + * + * If the requested size matches the current size, no allocation will be done. + * + * This can be called on an uninitialized (zero initialized) wait set. + * + * This function is not thread-safe. + * + * \param[inout] wait_set struct to have its clients cleared + * \param[in] size a size for the new set + * \return RCL_RET_OK if resized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size); + +/// Store a pointer to the given service in the next empty spot in the set. +/* This function does not guarantee that the service is not already in the + * wait set. + * + * This function is not thread-safe. + * This function is lock-free. + * + * \param[inout] wait_set struct in which the service is to be stored + * \param[in] service the service to be added to the wait set + * \return RCL_RET_OK if added successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * RCL_RET_WAIT_SET_FULL if the service set is full, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_service( + rcl_wait_set_t * wait_set, + const rcl_service_t * service); + +/// Remove (sets to NULL) the services in the wait set. +/* This function should be used after passing using rcl_wait, but before + * adding new services to the set. + * + * Calling this on an uninitialized (zero initialized) wait set will fail. + * + * This function is not thread-safe. + * This function is lock-free. + * + * \param[inout] wait_set struct to have its services cleared + * \return RCL_RET_OK if cleared successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_clear_services(rcl_wait_set_t * wait_set); + +/// Reallocate space for the services in the wait set. +/* This function will deallocate and reallocate the memory for the + * services set. + * + * A size of 0 will just deallocate the memory and assign NULL to the array. + * + * Allocation and deallocation is done with the allocator given during the + * wait set's initialization. + * + * After calling this function all values in the set will be set to NULL, + * effectively the same as calling rcl_wait_set_clear_services(). + * + * If the requested size matches the current size, no allocation will be done. + * + * This can be called on an uninitialized (zero initialized) wait set. + * + * This function is not thread-safe. + * + * \param[inout] wait_set struct to have its services cleared + * \param[in] size a size for the new set + * \return RCL_RET_OK if resized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size); + /// Block until the wait set is ready or until the timeout has been exceeded. /* This function will collect the items in the rcl_wait_set_t and pass them * to the underlying rmw_wait function. diff --git a/rcl/package.xml b/rcl/package.xml index d263184..e6e2c48 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -27,6 +27,7 @@ ament_lint_auto ament_lint_common rmw + example_interfaces std_msgs diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c new file mode 100644 index 0000000..3cb4621 --- /dev/null +++ b/rcl/src/rcl/client.c @@ -0,0 +1,208 @@ +// Copyright 2016 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. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/client.h" + +#include + +#include "rmw/rmw.h" + +#include "./common.h" +#include "./stdatomic_helper.h" + +typedef struct rcl_client_impl_t +{ + rcl_client_options_t options; + rmw_client_t * rmw_handle; + atomic_int_least64_t sequence_number; +} rcl_client_impl_t; + +rcl_client_t +rcl_get_zero_initialized_client() +{ + static rcl_client_t null_client = {0}; + return null_client; +} + +rcl_ret_t +rcl_client_init( + rcl_client_t * client, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_client_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (!node->impl) { + RCL_SET_ERROR_MSG("invalid node"); + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (client->impl) { + RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + // Allocate space for the implementation struct. + client->impl = (rcl_client_impl_t *)allocator->allocate( + sizeof(rcl_client_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Fill out implementation struct. + // rmw handle (create rmw client) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + client->impl->rmw_handle = rmw_create_client( + rcl_node_get_rmw_handle(node), + type_support, + service_name, + &rmw_qos_profile_default); + if (!client->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + goto fail; + } + // options + client->impl->options = *options; + return RCL_RET_OK; +fail: + if (client->impl) { + allocator->deallocate(client->impl, allocator->state); + } + return fail_ret; +} + +rcl_ret_t +rcl_client_fini(rcl_client_t * client, rcl_node_t * node) +{ + (void)node; + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (client->impl) { + rmw_ret_t ret = + rmw_destroy_client(client->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + rcl_allocator_t allocator = client->impl->options.allocator; + allocator.deallocate(client->impl, allocator.state); + } + return result; +} + +rcl_client_options_t +rcl_client_get_default_options() +{ + static rcl_client_options_t default_options; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_default; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +const char * +rcl_client_get_service_name(const rcl_client_t * client) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "client is invalid", return NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl->rmw_handle, "client is invalid", return NULL); + return client->impl->rmw_handle->service_name; +} + +const rcl_client_options_t * +rcl_client_get_options(const rcl_client_t * client) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "client is invalid", return NULL); + return &client->impl->options; +} + +rmw_client_t * +rcl_client_get_rmw_handle(const rcl_client_t * client) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "client is invalid", return NULL); + return client->impl->rmw_handle; +} + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "client is invalid", return RCL_RET_INVALID_ARGUMENT); + *sequence_number = rcl_atomic_load_int64_t(&client->impl->sequence_number); + if (rmw_send_request( + client->impl->rmw_handle, ros_request, sequence_number) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + rcl_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number); + return RCL_RET_OK; +} + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_response( + const rcl_client_t * client, + rmw_request_id_t * request_header, + void * ros_response) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "client is invalid", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT); + + bool taken = false; + if (rmw_take_response( + client->impl->rmw_handle, request_header, ros_response, &taken) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + if (!taken) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_CLIENT_TAKE_FAILED; + } + return RCL_RET_OK; +} + + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c new file mode 100644 index 0000000..531d99e --- /dev/null +++ b/rcl/src/rcl/service.c @@ -0,0 +1,202 @@ +// Copyright 2016 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. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/service.h" + +#include + +#include "./common.h" +#include "rmw/rmw.h" + +typedef struct rcl_service_impl_t +{ + rcl_service_options_t options; + rmw_service_t * rmw_handle; +} rcl_service_impl_t; + +rcl_service_t +rcl_get_zero_initialized_service() +{ + static rcl_service_t null_service = {0}; + return null_service; +} + +rcl_ret_t +rcl_service_init( + rcl_service_t * service, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_service_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (!node->impl) { + RCL_SET_ERROR_MSG("invalid node"); + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (service->impl) { + RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + // Allocate space for the implementation struct. + service->impl = (rcl_service_impl_t *)allocator->allocate( + sizeof(rcl_service_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Fill out implementation struct. + // rmw handle (create rmw service) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + service->impl->rmw_handle = rmw_create_service( + rcl_node_get_rmw_handle(node), + type_support, + service_name, + &rmw_qos_profile_default); + if (!service->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + goto fail; + } + // options + service->impl->options = *options; + return RCL_RET_OK; +fail: + if (service->impl) { + allocator->deallocate(service->impl, allocator->state); + } + return fail_ret; +} + +rcl_ret_t +rcl_service_fini(rcl_service_t * service, rcl_node_t * node) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (service->impl) { + rmw_ret_t ret = + rmw_destroy_service(service->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + rcl_allocator_t allocator = service->impl->options.allocator; + allocator.deallocate(service->impl, allocator.state); + } + return result; +} + +rcl_service_options_t +rcl_service_get_default_options() +{ + static rcl_service_options_t default_options; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_default; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +const char * +rcl_service_get_service_name(const rcl_service_t * service) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "service is invalid", return NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl->rmw_handle, "service is invalid", return NULL); + return service->impl->rmw_handle->service_name; +} + +const rcl_service_options_t * +rcl_service_get_options(const rcl_service_t * service) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "service is invalid", return NULL); + return &service->impl->options; +} + +rmw_service_t * +rcl_service_get_rmw_handle(const rcl_service_t * service) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "service is invalid", return NULL); + return service->impl->rmw_handle; +} + +rcl_ret_t +rcl_take_request( + const rcl_service_t * service, + rmw_request_id_t * request_header, + void * ros_request) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT); + + bool taken = false; + if (rmw_take_request( + service->impl->rmw_handle, request_header, ros_request, &taken) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + if (!taken) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_CLIENT_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_send_response( + const rcl_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT); + + if (rmw_send_response( + service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/stdatomic_helper.h b/rcl/src/rcl/stdatomic_helper.h index 89aeff4..fc344b3 100644 --- a/rcl/src/rcl/stdatomic_helper.h +++ b/rcl/src/rcl/stdatomic_helper.h @@ -61,6 +61,14 @@ rcl_atomic_load_bool(atomic_bool * a_bool) return result; } +static inline int64_t +rcl_atomic_load_int64_t(atomic_int_least64_t * a_int64_t) +{ + int64_t result = 0; + rcl_atomic_load(a_int64_t, result); + return result; +} + static inline uint64_t rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t) { @@ -94,6 +102,14 @@ rcl_atomic_exchange_bool(atomic_bool * a_bool, bool desired) return result; } +static inline int64_t +rcl_atomic_exchange_int64_t(atomic_int_least64_t * a_int64_t, int64_t desired) +{ + int64_t result; + rcl_atomic_exchange(a_int64_t, result, desired); + return result; +} + static inline uint64_t rcl_atomic_exchange_uint64_t(atomic_uint_least64_t * a_uint64_t, uint64_t desired) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 2e58be0..d9cc4f5 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -121,12 +121,10 @@ rcl_ret_t rcl_take( const rcl_subscription_t * subscription, void * ros_message, - bool * taken, rmw_message_info_t * message_info) { RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(taken, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( subscription->impl, "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID); RCL_CHECK_FOR_NULL_WITH_MSG( @@ -136,12 +134,17 @@ rcl_take( rmw_message_info_t dummy_message_info; rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; // Call rmw_take_with_info. + bool taken = false; rmw_ret_t ret = - rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info_local); + rmw_take_with_info(subscription->impl->rmw_handle, ros_message, &taken, message_info_local); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); return RCL_RET_ERROR; } + if (!taken) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 41c877e..5426419 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -35,6 +35,10 @@ typedef struct rcl_wait_set_impl_t rmw_subscriptions_t rmw_subscriptions; size_t guard_condition_index; rmw_guard_conditions_t rmw_guard_conditions; + size_t client_index; + rmw_clients_t rmw_clients; + size_t service_index; + rmw_services_t rmw_services; rmw_waitset_t * rmw_waitset; size_t timer_index; rcl_allocator_t allocator; @@ -48,6 +52,10 @@ rcl_get_zero_initialized_wait_set() .size_of_subscriptions = 0, .guard_conditions = NULL, .size_of_guard_conditions = 0, + .clients = NULL, + .size_of_clients = 0, + .services = NULL, + .size_of_services = 0, .timers = NULL, .size_of_timers = 0, .impl = NULL, @@ -79,6 +87,16 @@ __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) (void)ret; // NO LINT assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } + if (wait_set->clients) { + rcl_ret_t ret = rcl_wait_set_resize_clients(wait_set, 0); + (void)ret; // NO LINT + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. + } + if (wait_set->services) { + rcl_ret_t ret = rcl_wait_set_resize_services(wait_set, 0); + (void)ret; // NO LINT + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. + } if (wait_set->impl) { allocator.deallocate(wait_set->impl, allocator.state); wait_set->impl = NULL; @@ -91,6 +109,8 @@ rcl_wait_set_init( size_t number_of_subscriptions, size_t number_of_guard_conditions, size_t number_of_timers, + size_t number_of_clients, + size_t number_of_services, rcl_allocator_t allocator) { rcl_ret_t fail_ret = RCL_RET_ERROR; @@ -115,11 +135,18 @@ rcl_wait_set_init( wait_set->impl->rmw_subscriptions.subscriber_count = 0; wait_set->impl->rmw_guard_conditions.guard_conditions = NULL; wait_set->impl->rmw_guard_conditions.guard_condition_count = 0; + wait_set->impl->rmw_clients.clients = NULL; + wait_set->impl->rmw_clients.client_count = 0; + wait_set->impl->rmw_services.services = NULL; + wait_set->impl->rmw_services.service_count = 0; + static rmw_guard_conditions_t fixed_guard_conditions; fixed_guard_conditions.guard_conditions = NULL; fixed_guard_conditions.guard_condition_count = 0; wait_set->impl->rmw_waitset = rmw_create_waitset( - &fixed_guard_conditions, 2 * number_of_subscriptions + number_of_guard_conditions); + &fixed_guard_conditions, + 2 * number_of_subscriptions + number_of_guard_conditions + number_of_clients + + number_of_services); if (!wait_set->impl->rmw_waitset) { goto fail; } @@ -156,6 +183,26 @@ rcl_wait_set_init( fail_ret = ret; goto fail; } + // Initialize client space. + ret = rcl_wait_set_resize_clients(wait_set, number_of_clients); + if (ret != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + if ((ret = rcl_wait_set_clear_clients(wait_set)) != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + // Initialize service space. + ret = rcl_wait_set_resize_services(wait_set, number_of_services); + if (ret != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + if ((ret = rcl_wait_set_clear_services(wait_set)) != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } return RCL_RET_OK; fail: if (__wait_set_is_valid(wait_set)) { @@ -236,7 +283,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al memset( \ wait_set->impl->RMWStorage, \ 0, \ - sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount); + sizeof(rmw_ ## Type ## _t *) * wait_set->impl->RMWCount); #define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ @@ -373,6 +420,70 @@ rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size) SET_RESIZE(timer,;,;) // NOLINT } +rcl_ret_t +rcl_wait_set_add_client( + rcl_wait_set_t * wait_set, + const rcl_client_t * client) +{ + SET_ADD(client) + SET_ADD_RMW(client, rmw_clients.clients) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_clear_clients(rcl_wait_set_t * wait_set) +{ + SET_CLEAR(client) + SET_CLEAR_RMW( + clients, + rmw_clients.clients, + rmw_clients.client_count) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size) +{ + SET_RESIZE(client, + SET_RESIZE_RMW_DEALLOC( + rmw_clients.clients, rmw_clients.client_count), + SET_RESIZE_RMW_REALLOC( + client, rmw_clients.clients, rmw_clients.client_count) + ) +} + +rcl_ret_t +rcl_wait_set_add_service( + rcl_wait_set_t * wait_set, + const rcl_service_t * service) +{ + SET_ADD(service) + SET_ADD_RMW(service, rmw_services.services) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_clear_services(rcl_wait_set_t * wait_set) +{ + SET_CLEAR(service) + SET_CLEAR_RMW( + services, + rmw_services.services, + rmw_services.service_count) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size) +{ + SET_RESIZE(service, + SET_RESIZE_RMW_DEALLOC( + rmw_services.services, rmw_services.service_count), + SET_RESIZE_RMW_REALLOC( + service, rmw_services.services, rmw_services.service_count) + ) +} + rcl_ret_t rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) { @@ -382,13 +493,12 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) RCL_SET_ERROR_MSG("wait set is invalid"); return RCL_RET_WAIT_SET_INVALID; } - if (wait_set->size_of_subscriptions == 0 && wait_set->size_of_guard_conditions == 0) { + if (wait_set->size_of_subscriptions == 0 && wait_set->size_of_guard_conditions == 0 && + wait_set->size_of_clients == 0 && wait_set->size_of_services == 0) + { RCL_SET_ERROR_MSG("wait set is empty"); return RCL_RET_WAIT_SET_EMPTY; } - // Create dummy sets for currently unsupported wait-ables. - static rmw_services_t dummy_services = {0, NULL}; - static rmw_clients_t dummy_clients = {0, NULL}; // Calculate the timeout argument. rmw_time_t * timeout_argument; rmw_time_t temporary_timeout_storage; @@ -432,8 +542,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) rmw_ret_t ret = rmw_wait( &wait_set->impl->rmw_subscriptions, &wait_set->impl->rmw_guard_conditions, - &dummy_services, - &dummy_clients, + &wait_set->impl->rmw_services, + &wait_set->impl->rmw_clients, wait_set->impl->rmw_waitset, timeout_argument); // Check for timeout. @@ -446,6 +556,10 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) 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. + rcl_ret = rcl_wait_set_clear_services(wait_set); + assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. + rcl_ret = rcl_wait_set_clear_clients(wait_set); + assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. return RCL_RET_TIMEOUT; } // Check for error. @@ -479,6 +593,20 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) wait_set->guard_conditions[i] = NULL; } } + // Set corresponding rcl client handles NULL. + for (i = 0; i < wait_set->size_of_clients; ++i) { + assert(i < wait_set->impl->rmw_clients.client_count); // Defensive. + if (!wait_set->impl->rmw_clients.clients[i]) { + wait_set->clients[i] = NULL; + } + } + // Set corresponding rcl service handles NULL. + for (i = 0; i < wait_set->size_of_services; ++i) { + assert(i < wait_set->impl->rmw_services.service_count); // Defensive. + if (!wait_set->impl->rmw_services.services[i]) { + wait_set->services[i] = NULL; + } + } return RCL_RET_OK; } diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 6d6c671..9db174e 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -1,4 +1,5 @@ find_package(ament_cmake_gtest REQUIRED) +find_package(example_interfaces REQUIRED) find_package(std_msgs REQUIRED) include(rcl_add_custom_gtest.cmake) @@ -33,6 +34,14 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} ) + rcl_add_custom_gtest(test_client${target_suffix} + SRCS rcl/test_client.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces" + ) + rcl_add_custom_gtest(test_time${target_suffix} SRCS rcl/test_time.cpp ENV ${extra_test_env} @@ -76,6 +85,14 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "std_msgs" ) + rcl_add_custom_gtest(test_service${target_suffix} + SRCS rcl/test_service.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} "example_interfaces" + ) + rcl_add_custom_gtest(test_subscription${target_suffix} SRCS rcl/test_subscription.cpp ENV ${extra_test_env} diff --git a/rcl/test/rcl/test_client.cpp b/rcl/test/rcl/test_client.cpp new file mode 100644 index 0000000..5582530 --- /dev/null +++ b/rcl/test/rcl/test_client.cpp @@ -0,0 +1,188 @@ +// Copyright 2016 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. + +#include + +#include "rcl/client.h" + +#include "rcl/rcl.h" + +#include "example_interfaces/srv/add_two_ints.h" +#include "rosidl_generator_c/string_functions.h" + +#include "../memory_tools/memory_tools.hpp" +#include "../scope_exit.hpp" +#include "rcl/error_handling.h" + +class TestClientFixture : public ::testing::Test +{ +public: + rcl_node_t * node_ptr; + void SetUp() + { + stop_memory_checking(); + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unexpected_malloc_callback(nullptr); + set_on_unexpected_realloc_callback(nullptr); + set_on_unexpected_free_callback(nullptr); + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +/* Basic nominal test of a client. + */ +TEST_F(TestClientFixture, test_client_nominal) { + stop_memory_checking(); + rcl_ret_t ret; + rcl_client_t client = rcl_get_zero_initialized_client(); + + // Initialize the client. + const char * topic_name = "add_two_ints"; + rcl_client_options_t client_options = rcl_client_get_default_options(); + + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( + example_interfaces, srv, AddTwoInts); + ret = rcl_client_init(&client, this->node_ptr, ts, topic_name, &client_options); + + // Check the return code of initialization and that the service name matches what's expected + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(strcmp(rcl_client_get_service_name(&client), topic_name), 0); + + auto client_exit = make_scope_exit([&client, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + + // Initialize the client request. + example_interfaces__srv__AddTwoInts_Request req; + example_interfaces__srv__AddTwoInts_Request__init(&req); + req.a = 1; + req.b = 2; + + // Check that there were no errors while sending the request. + int64_t sequence_number = 0; + ret = rcl_send_request(&client, &req, &sequence_number); + EXPECT_EQ(sequence_number, 1); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); +} + + +/* Testing the client init and fini functions. + */ +TEST_F(TestClientFixture, test_client_init_fini) { + stop_memory_checking(); + rcl_ret_t ret; + // Setup valid inputs. + rcl_client_t client; + + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( + example_interfaces, srv, AddTwoInts); + const char * topic_name = "chatter"; + rcl_client_options_t default_client_options = rcl_client_get_default_options(); + + // Try passing null for client in init. + ret = rcl_client_init(nullptr, this->node_ptr, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for a node pointer in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, nullptr, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing an invalid (uninitialized) node in init. + client = rcl_get_zero_initialized_client(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + ret = rcl_client_init(&client, &invalid_node, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for the type support in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init( + &client, this->node_ptr, nullptr, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for the topic name in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, this->node_ptr, ts, nullptr, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for the options in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, this->node_ptr, ts, topic_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing options with an invalid allocate in allocator with init. + client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options_with_invalid_allocator; + client_options_with_invalid_allocator = rcl_client_get_default_options(); + client_options_with_invalid_allocator.allocator.allocate = nullptr; + ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &client_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing options with an invalid deallocate in allocator with init. + client = rcl_get_zero_initialized_client(); + client_options_with_invalid_allocator = rcl_client_get_default_options(); + client_options_with_invalid_allocator.allocator.deallocate = nullptr; + ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &client_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // An allocator with an invalid realloc will probably work (so we will not test it). + + // Try passing options with a failing allocator with init. + client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options_with_failing_allocator; + client_options_with_failing_allocator = rcl_client_get_default_options(); + client_options_with_failing_allocator.allocator.allocate = failing_malloc; + client_options_with_failing_allocator.allocator.deallocate = failing_free; + client_options_with_failing_allocator.allocator.reallocate = failing_realloc; + ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &client_options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp new file mode 100644 index 0000000..5688146 --- /dev/null +++ b/rcl/test/rcl/test_service.cpp @@ -0,0 +1,196 @@ +// 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. + +#include + +#include +#include +#include + +#include "rcl/service.h" + +#include "rcl/rcl.h" + +#include "example_interfaces/srv/add_two_ints.h" + +#include "../memory_tools/memory_tools.hpp" +#include "../scope_exit.hpp" +#include "rcl/error_handling.h" + +class TestServiceFixture : public ::testing::Test +{ +public: + rcl_node_t * node_ptr; + void SetUp() + { + stop_memory_checking(); + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unexpected_malloc_callback(nullptr); + set_on_unexpected_realloc_callback(nullptr); + set_on_unexpected_free_callback(nullptr); + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +void +wait_for_service_to_be_ready( + rcl_service_t * service, + size_t max_tries, + int64_t period_ms, + bool & success) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto wait_set_exit = make_scope_exit([&wait_set]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + size_t iteration = 0; + do { + ++iteration; + ret = rcl_wait_set_clear_services(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_add_service(&wait_set, service); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + for (size_t i = 0; i < wait_set.size_of_services; ++i) { + if (wait_set.services[i] && wait_set.services[i] == service) { + success = true; + return; + } + } + } while (iteration < max_tries); + success = false; +} + +/* Basic nominal test of a service. + */ +TEST_F(TestServiceFixture, test_service_nominal) { + stop_memory_checking(); + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( + example_interfaces, srv, AddTwoInts); + const char * topic = "add_two_ints"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + // Check that the service name matches what we assigned. + EXPECT_EQ(strcmp(rcl_service_get_service_name(&service), topic), 0); + auto service_exit = make_scope_exit([&service, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init(&client, this->node_ptr, ts, topic, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto client_exit = make_scope_exit([&client, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + + + // TODO(wjwwood): add logic to wait for the connection to be established + // use count_services busy wait mechanism + // until then we will sleep for a short period of time + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // Initialize a request. + example_interfaces__srv__AddTwoInts_Request client_request; + example_interfaces__srv__AddTwoInts_Request__init(&client_request); + client_request.a = 1; + client_request.b = 2; + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + EXPECT_EQ(sequence_number, 1); + example_interfaces__srv__AddTwoInts_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + bool success; + wait_for_service_to_be_ready(&service, 10, 100, success); + ASSERT_TRUE(success); + + // This scope simulates the service responding in a different context so that we can + // test take_request/send_response in a single-threaded, deterministic execution. + { + // Initialize a response. + example_interfaces__srv__AddTwoInts_Response service_response; + example_interfaces__srv__AddTwoInts_Response__init(&service_response); + auto msg_exit = make_scope_exit([&service_response]() { + stop_memory_checking(); + example_interfaces__srv__AddTwoInts_Response__fini(&service_response); + }); + + // Initialize a separate instance of the request and take the pending request. + example_interfaces__srv__AddTwoInts_Request service_request; + example_interfaces__srv__AddTwoInts_Request__init(&service_request); + rmw_request_id_t header; + ret = rcl_take_request(&service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + EXPECT_EQ(1, service_request.a); + EXPECT_EQ(2, service_request.b); + // Simulate a response callback by summing the request and send the response.. + service_response.sum = service_request.a + service_request.b; + ret = rcl_send_response(&service, &header, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + wait_for_service_to_be_ready(&service, 10, 100, success); + + // Initialize the response owned by the client and take the response. + example_interfaces__srv__AddTwoInts_Response client_response; + example_interfaces__srv__AddTwoInts_Response__init(&client_response); + + rmw_request_id_t header; + ret = rcl_take_response(&client, &header, &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(client_response.sum, 3); + EXPECT_EQ(header.sequence_number, 1); +} diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index a585b9b..9184609 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -83,7 +83,7 @@ wait_for_subscription_to_be_ready( bool & success) { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_ret_t ret = rcl_wait_set_init(&wait_set, 1, 0, 0, rcl_get_default_allocator()); + rcl_ret_t ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, rcl_get_default_allocator()); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); auto wait_set_exit = make_scope_exit([&wait_set]() { stop_memory_checking(); @@ -170,10 +170,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription stop_memory_checking(); std_msgs__msg__Int64__fini(&msg); }); - bool taken = false; - ret = rcl_take(&subscription, &msg, &taken, nullptr); + ret = rcl_take(&subscription, &msg, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); - ASSERT_TRUE(taken) << "failed to take a message, even though the subscription was ready"; ASSERT_EQ(42, msg.data); } } @@ -185,7 +183,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_ret_t ret; rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(std_msgs, msg, String); - const char * topic = "chatter"; + const char * topic = "rcl_test_subscription_chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); @@ -228,10 +226,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription stop_memory_checking(); std_msgs__msg__String__fini(&msg); }); - bool taken = false; - ret = rcl_take(&subscription, &msg, &taken, nullptr); + ret = rcl_take(&subscription, &msg, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); - ASSERT_TRUE(taken) << "failed to take a message, even though the subscription was ready"; ASSERT_EQ(std::string(test_string), std::string(msg.data.data, msg.data.size)); } }