niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add services and clients

Browse source
This commit is contained in:
Jackie Kay 2016-02-23 18:17:56 -08:00
parent 1932e052ec
commit 534a49c480
17 changed files with 1760 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rcl/CMakeLists.txt
View file

@ -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

301
rcl/include/rcl/client.h Normal file
View file

@ -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 <rosidl_generator_c/service_type_support.h>
* #include <example_interfaces/srv/add_two_ints.h>
* rosidl_service_type_support_t * ts =
* ROSIDL_GET_SERVICE_TYPE_SUPPORT(example_interfaces, AddTwoInts);
*
* For C++ a template function is used:
*
* #include <rosidl_generator_cpp/service_type_support.hpp>
* #include <example_interfaces/srv/add_two_ints.h>
* 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 <rcl/rcl.h>
* #include <rosidl_generator_c/service_type_support.h>
* #include <example_interfaces/srv/add_two_ints.h>
*
* 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_

2
rcl/include/rcl/guard_condition.h
View file

@ -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.

317
rcl/include/rcl/service.h Normal file
View file

@ -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 <rosidl_generator_c/service_type_support.h>
* #include <example_interfaces/srv/add_two_ints.h>
* rosidl_service_type_support_t * ts =
* ROSIDL_GET_SERVICE_TYPE_SUPPORT(example_interfaces, AddTwoInts);
*
* For C++ a template function is used:
*
* #include <rosidl_generator_cpp/service_type_support.hpp>
* #include <example_interfaces/srv/add_two_ints.h>
* 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 <rcl/rcl.h>
* #include <rosidl_generator_c/service_type_support.h>
* #include <example_interfaces/srv/add_two_ints.h>
*
* 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_

7
rcl/include/rcl/subscription.h
View file

@ -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.

5
rcl/include/rcl/types.h
View file

@ -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

156
rcl/include/rcl/wait.h
View file

@ -23,8 +23,10 @@ extern "C"
#include <stdbool.h>
#include <stddef.h>
#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.

1
rcl/package.xml
View file

@ -27,6 +27,7 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>rmw</test_depend>
<test_depend>example_interfaces</test_depend>
<test_depend>std_msgs</test_depend>
<export>

208
rcl/src/rcl/client.c Normal file
View file

@ -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 <string.h>
#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

202
rcl/src/rcl/service.c Normal file
View file

@ -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 <string.h>
#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

16
rcl/src/rcl/stdatomic_helper.h
View file

@ -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)
{

9
rcl/src/rcl/subscription.c
View file

@ -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;
}

144
rcl/src/rcl/wait.c
View file

@ -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;
}

17
rcl/test/CMakeLists.txt
View file

@ -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}

188
rcl/test/rcl/test_client.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#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();
}

196
rcl/test/rcl/test_service.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <chrono>
#include <string>
#include <thread>
#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);
}

12
rcl/test/rcl/test_subscription.cpp
View file

@ -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));
}
}