diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index f008864..6a9f867 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -156,7 +156,7 @@ rcl_subscription_get_default_options(); /// Take a ROS message from a topic using a rcl subscription. /* It is the job of the caller to ensure that the type of the ros_message - * parameter and the type associate with the subscription, via the type + * argument and the type associate with the subscription, 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. @@ -164,6 +164,7 @@ rcl_subscription_get_default_options(); * TODO(wjwwood) blocking of take? * TODO(wjwwood) pre-, during-, and post-conditions for message ownership? * TODO(wjwwood) is rcl_take thread-safe? + * TODO(wjwwood) Should there be an rcl_message_info_t? * * The ros_message pointer should point to an already allocated ROS message * struct of the correct type, into which the taken ROS message will be copied @@ -181,9 +182,9 @@ rcl_subscription_get_default_options(); * message instance, like what the GUID of the publisher which published it * originally or whether or not the message received from within the same * process. - * The message_info parameter should be an already allocated rmw_message_info_t + * The message_info argument should be an already allocated rmw_message_info_t * structure. - * Passing NULL for message_info will be ignored. + * Passing NULL for message_info will result in the argument being ignored. * * \param[in] subscription the handle to the subscription from which to take * \param[inout] ros_message type-erased ptr to a allocated ROS message @@ -191,6 +192,7 @@ rcl_subscription_get_default_options(); * \param[out] message_info rmw struct which contains meta-data for the message * \return RCL_RET_OK if the message was published, or * 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_ERROR if an unspecified error occurs. */ @@ -205,7 +207,7 @@ rcl_take( /* This function returns the subscription's internal topic name string. * This function can fail, and therefore return NULL, if the: * - subscription is NULL - * - subscription is invalid (never called init, called fini, or invalid node) + * - subscription is invalid (never called init, called fini, or invalid) * * The returned string is only valid as long as the subscription is valid. * The value of the string may change if the topic name changes, and therefore @@ -224,7 +226,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); /* This function returns the subscription's internal options struct. * This function can fail, and therefore return NULL, if the: * - subscription is NULL - * - subscription is invalid (never called init, called fini, or invalid node) + * - subscription is invalid (never called init, called fini, or invalid) * * The returned struct is only valid as long as the subscription is valid. * The values in the struct may change if the subscription's options change, @@ -243,7 +245,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription); /* The handle returned is a pointer to the internally held rmw handle. * This function can fail, and therefore return NULL, if the: * - subscription is NULL - * - subscription is invalid (never called init, called fini, or invalid node) + * - subscription is invalid (never called init, called fini, or invalid) * * The returned handle is only valid as long as the rcl subscription is valid. * diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index e470aeb..0abb885 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -31,6 +31,7 @@ typedef rmw_ret_t rcl_ret_t; // rcl publisher specific ret codes in 3XX #define RCL_RET_PUBLISHER_INVALID 300 // rcl subscription specific ret codes in 4XX +#define RCL_RET_SUBSCRIPTION_INVALID 400 // rcl service client specific ret codes in 5XX // rcl service server specific ret codes in 6XX // rcl guard condition specific ret codes in 7XX diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index 827c33b..379611f 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -23,8 +23,8 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/types.h" -#define RCL_CHECK_ARGUMENT_FOR_NULL(parameter, error_return_type) \ - RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " argument is null", return error_return_type) +#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) \ + RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return error_return_type) #define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ RCL_SET_ERROR_MSG(msg); \ diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index af97185..c7dc189 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -108,9 +108,9 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) rcl_publisher_options_t rcl_publisher_get_default_options() { - static rcl_publisher_options_t default_options = { - .qos = rmw_qos_profile_default, - }; + static rcl_publisher_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; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 3136616..880bf31 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -19,7 +19,160 @@ extern "C" #include "rcl/subscription.h" +#include "rmw/rmw.h" +#include "./common.h" +typedef struct rcl_subscription_impl_t { + rcl_subscription_options_t options; + rmw_subscription_t * rmw_handle; +} rcl_subscription_impl_t; + +rcl_subscription_t +rcl_get_zero_initialized_subscription() +{ + static rcl_subscription_t null_subscription = {0}; + return null_subscription; +} + +rcl_ret_t +rcl_subscription_init( + rcl_subscription_t * subscription, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_subscription_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (subscription->impl) { + RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninialized"); + 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 memory for the implementation struct. + subscription->impl = (rcl_subscription_impl_t *)allocator->allocate( + sizeof(rcl_subscription_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Fill out the implemenation struct. + // rmw_handle + // TODO(wjwwood): pass allocator once supported in rmw api. + subscription->impl->rmw_handle = rmw_create_subscription( + rcl_node_get_rmw_node_handle(node), + type_support, + topic_name, + &rmw_qos_profile_default, + options->ignore_local_publications); + if (!subscription->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + goto fail; + } + // options + subscription->impl->options = *options; + return RCL_RET_OK; +fail: + if (subscription->impl) { + allocator->deallocate(subscription->impl, allocator->state); + } + return fail_ret; +} + +rcl_ret_t +rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (subscription->impl) { + rmw_ret_t ret = + rmw_destroy_subscription(rcl_node_get_rmw_node_handle(node), subscription->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 = subscription->impl->options.allocator; + allocator.deallocate(subscription->impl, allocator.state); + } + return result; +} + +rcl_subscription_options_t +rcl_subscription_get_default_options() +{ + static rcl_subscription_options_t default_options = { + .ignore_local_publications = false, + }; + // 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; +} + +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( + subscription->impl->rmw_handle, + "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID); + // If message_info is NULL, use a place holder which can be discarded. + 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. + rmw_ret_t ret = + rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +const char * +rcl_subscription_get_topic_name(const rcl_subscription_t * subscription) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl->rmw_handle, + "subscription is invalid", return NULL); + return subscription->impl->rmw_handle->topic_name; +} + +const rcl_subscription_options_t * +rcl_subscription_get_options(rcl_subscription_t * subscription) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return NULL); + return &subscription->impl->options; +} + +rmw_subscription_t * +rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return NULL); + return subscription->impl->rmw_handle; +} #if __cplusplus }