diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index f4bf6f2..f229e7b 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -114,7 +114,12 @@ rcl_get_zero_initialized_publisher(); * \param[in] type_support type support object for the topic's type * \param[in] topic_name the name of the topic to publish on * \param[in] options publisher options, including quality of service settings - * \return RMW_RET_OK if publisher was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the publisher was initialized successfully, or + * RCL_RET_NODE_INVALID if the node is invalid, or + * RCL_RET_ALREADY_INIT if the publisher is already initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_publisher_init( @@ -135,7 +140,9 @@ rcl_publisher_init( * * \param[inout] publisher handle to the publisher to be deinitialized * \param[in] node handle to the node used to create the publisher - * \return RMW_RET_OK if publisher was deinitialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if publisher was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); @@ -144,13 +151,6 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); rcl_publisher_options_t rcl_publisher_get_default_options(); -// TODO(wjwwood): I have an idea on how to do a type-checking macro version of -// publish using C11's _Generic "generic selection", but it -// needs support from the generated code (look into it later) -// It might also use static assert, see: -// http://www.robertgamble.net/2012/01/c11-static-assertions.html -// #define RCL_PUBLISH(publisher, ros_message) ... - /// Publish a ROS message on a topic using a publisher. /* It is the job of the caller to ensure that the type of the ros_message * parameter and the type associate with the publisher (via the type support) @@ -190,7 +190,10 @@ rcl_publisher_get_default_options(); * * \param[in] publisher handle to the publisher which will do the publishing * \param[in] ros_message type-erased pointer to the ROS message - * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the message was published successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); @@ -231,7 +234,7 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); * \return options struct if successful, otherwise NULL */ const rcl_publisher_options_t * -rcl_publisher_get_options(rcl_publisher_t * publisher); +rcl_publisher_get_options(const rcl_publisher_t * publisher); /// Return the rmw publisher handle. /* The handle returned is a pointer to the internally held rmw handle. @@ -254,7 +257,7 @@ rcl_publisher_get_options(rcl_publisher_t * publisher); * \return rmw publisher handle if successful, otherwise NULL */ rmw_publisher_t * -rcl_publisher_get_rmw_publisher_handle(rcl_publisher_t * publisher); +rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher); #if __cplusplus } diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index 2dd33fe..e470aeb 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -29,6 +29,7 @@ typedef rmw_ret_t rcl_ret_t; // rcl node specific ret codes in 2XX #define RCL_RET_NODE_INVALID 200 // rcl publisher specific ret codes in 3XX +#define RCL_RET_PUBLISHER_INVALID 300 // rcl subscription specific ret codes in 4XX // rcl service client specific ret codes in 5XX // rcl service server specific ret codes in 6XX diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 50e5aa9..d6e94b3 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -108,8 +108,8 @@ rcl_node_fini(rcl_node_t * node) return RCL_RET_OK; } rcl_ret_t result = RCL_RET_OK; - rmw_ret_t node_ret = rmw_destroy_node(node->impl->rmw_node_handle); - if (node_ret != RMW_RET_OK) { + rmw_ret_t ret = rmw_destroy_node(node->impl->rmw_node_handle); + if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); result = RCL_RET_ERROR; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 319b073..af97185 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -86,6 +86,78 @@ fail: return fail_ret; } +rcl_ret_t +rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (publisher->impl) { + rmw_ret_t ret = + rmw_destroy_publisher(rcl_node_get_rmw_node_handle(node), publisher->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 = publisher->impl->options.allocator; + allocator.deallocate(publisher->impl, allocator.state); + } + return result; +} + +rcl_publisher_options_t +rcl_publisher_get_default_options() +{ + static rcl_publisher_options_t default_options = { + .qos = rmw_qos_profile_default, + }; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +rcl_ret_t +rcl_publish(const rcl_publisher_t * publisher, const void * ros_message) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID); + if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +const char * +rcl_publisher_get_topic_name(const rcl_publisher_t * publisher) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl->rmw_handle, "publisher is invalid", return NULL); + return publisher->impl->rmw_handle->topic_name; +} + +const rcl_publisher_options_t * +rcl_publisher_get_options(const rcl_publisher_t * publisher) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return NULL); + return &publisher->impl->options; +} + +rmw_publisher_t * +rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return NULL); + return publisher->impl->rmw_handle; +} + #if __cplusplus } #endif