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

Rmw preallocate (#428)

Browse source 
* Proposola of changes for RMW_Preallocate. Related /ros2/rmw#160

Signed-off-by: Gonzalo de Pedro <gonzalo@depedro.com.ar>

* Changed RCL interface

Signed-off-by: Gonzalo de Pedro <gonzalo@depedro.com.ar>

* Updates for allocation in serialize methods.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Fix tests for new APIs.

Signed-off-by: Michael Carroll <michael@openrobotics.org>
This commit is contained in:
Michael Carroll 2019-05-02 11:32:13 -05:00 • committed by GitHub
parent 4262d4cb29
commit 3d48555597
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
10 changed files with 63 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

16
rcl/include/rcl/publisher.h
View file

@ -150,7 +150,8 @@ rcl_publisher_init(
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
const rcl_publisher_options_t * options);
const rcl_publisher_options_t * options
);
/// Finalize a rcl_publisher_t.
/**
@ -244,6 +245,7 @@ rcl_publisher_get_default_options(void);
*
* \param[in] publisher handle to the publisher which will do the publishing
* \param[in] ros_message type-erased pointer to the ROS message
* \param[in] allocation structure pointer, used for memory preallocation (may be NULL)
* \return `RCL_RET_OK` if the message was published successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or
@ -252,7 +254,11 @@ rcl_publisher_get_default_options(void);
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
rcl_publish(
const rcl_publisher_t * publisher,
const void * ros_message,
rmw_publisher_allocation_t * allocation
);
/// Publish a serialized message on a topic using a publisher.
/**
@ -279,6 +285,7 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
*
* \param[in] publisher handle to the publisher which will do the publishing
* \param[in] serialized_message pointer to the already serialized message in raw form
* \param[in] allocation structure pointer, used for memory preallocation (may be NULL)
* \return `RCL_RET_OK` if the message was published successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or
@ -288,7 +295,10 @@ RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_publish_serialized_message(
const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message);
const rcl_publisher_t * publisher,
const rcl_serialized_message_t * serialized_message,
rmw_publisher_allocation_t * allocation
);
/// Get the topic name for the publisher.
/**

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

@ -153,7 +153,8 @@ rcl_subscription_init(
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
const rcl_subscription_options_t * options);
const rcl_subscription_options_t * options
);
/// Finalize a rcl_subscription_t.
/**
@ -246,6 +247,7 @@ rcl_subscription_get_default_options(void);
* \param[in] subscription the handle to the subscription from which to take
* \param[inout] ros_message type-erased ptr to a allocated ROS message
* \param[out] message_info rmw struct which contains meta-data for the message
* \param[in] allocation structure pointer used for memory preallocation (may be NULL)
* \return `RCL_RET_OK` if the message was published, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or
@ -260,7 +262,9 @@ rcl_ret_t
rcl_take(
const rcl_subscription_t * subscription,
void * ros_message,
rmw_message_info_t * message_info);
rmw_message_info_t * message_info,
rmw_subscription_allocation_t * allocation
);
/// Take a serialized raw message from a topic using a rcl subscription.
/**
@ -289,6 +293,7 @@ rcl_take(
* \param[in] subscription the handle to the subscription from which to take
* \param[inout] serialized_message pointer to a (pre-allocated) serialized message.
* \param[out] message_info rmw struct which contains meta-data for the message
* \param[in] allocation structure pointer used for memory preallocation (may be NULL)
* \return `RCL_RET_OK` if the message was published, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or
@ -303,7 +308,8 @@ rcl_ret_t
rcl_take_serialized_message(
const rcl_subscription_t * subscription,
rcl_serialized_message_t * serialized_message,
rmw_message_info_t * message_info);
rmw_message_info_t * message_info,
rmw_subscription_allocation_t * allocation);
/// Get the topic name for the subscription.
/**

2
rcl/src/rcl/logging_rosout.c
View file

@ -260,7 +260,7 @@ void rcl_logging_rosout_output_handler(
rosidl_generator_c__String__assign(&log_message->msg, msg_array.buffer);
rosidl_generator_c__String__assign(&log_message->file, location->file_name);
rosidl_generator_c__String__assign(&log_message->function, location->function_name);
status = rcl_publish(&entry.publisher, log_message);
status = rcl_publish(&entry.publisher, log_message, NULL);
if (RCL_RET_OK != status) {
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to publish log message to rosout: ");
RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str);

22
rcl/src/rcl/publisher.c
View file

@ -53,7 +53,8 @@ rcl_publisher_init(
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
const rcl_publisher_options_t * options)
const rcl_publisher_options_t * options
)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
@ -74,6 +75,8 @@ rcl_publisher_init(
RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing publisher for topic name '%s'", topic_name);
// Expand the given topic name.
rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version
rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
@ -162,6 +165,7 @@ rcl_publisher_init(
sizeof(rcl_publisher_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
// Fill out implementation struct.
// rmw handle (create rmw publisher)
// TODO(wjwwood): pass along the allocator to rmw when it supports it
@ -188,11 +192,13 @@ rcl_publisher_init(
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized");
// context
publisher->impl->context = node->context;
goto cleanup;
fail:
if (publisher->impl) {
allocator->deallocate(publisher->impl, allocator->state);
}
ret = fail_ret;
// Fall through to cleanup
cleanup:
@ -245,13 +251,16 @@ rcl_publisher_get_default_options()
}
rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
rcl_publish(
const rcl_publisher_t * publisher,
const void * ros_message,
rmw_publisher_allocation_t * allocation)
{
if (!rcl_publisher_is_valid(publisher)) {
return RCL_RET_PUBLISHER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) {
if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation) != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
return RCL_RET_ERROR;
}
@ -260,13 +269,16 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
rcl_ret_t
rcl_publish_serialized_message(
const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message)
const rcl_publisher_t * publisher,
const rcl_serialized_message_t * serialized_message,
rmw_publisher_allocation_t * allocation)
{
if (!rcl_publisher_is_valid(publisher)) {
return RCL_RET_PUBLISHER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(serialized_message, RCL_RET_INVALID_ARGUMENT);
rmw_ret_t ret = rmw_publish_serialized_message(publisher->impl->rmw_handle, serialized_message);
rmw_ret_t ret = rmw_publish_serialized_message(publisher->impl->rmw_handle, serialized_message,
allocation);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
if (ret == RMW_RET_BAD_ALLOC) {

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

@ -49,7 +49,8 @@ rcl_subscription_init(
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
const rcl_subscription_options_t * options)
const rcl_subscription_options_t * options
)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
@ -235,7 +236,9 @@ rcl_ret_t
rcl_take(
const rcl_subscription_t * subscription,
void * ros_message,
rmw_message_info_t * message_info)
rmw_message_info_t * message_info,
rmw_subscription_allocation_t * allocation
)
{
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message");
if (!rcl_subscription_is_valid(subscription)) {
@ -249,7 +252,8 @@ rcl_take(
// 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, allocation);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
if (RMW_RET_BAD_ALLOC == ret) {
@ -269,7 +273,9 @@ rcl_ret_t
rcl_take_serialized_message(
const rcl_subscription_t * subscription,
rcl_serialized_message_t * serialized_message,
rmw_message_info_t * message_info)
rmw_message_info_t * message_info,
rmw_subscription_allocation_t * allocation
)
{
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message");
if (!rcl_subscription_is_valid(subscription)) {
@ -282,7 +288,7 @@ rcl_take_serialized_message(
// Call rmw_take_with_info.
bool taken = false;
rmw_ret_t ret = rmw_take_serialized_message_with_info(
subscription->impl->rmw_handle, serialized_message, &taken, message_info_local);
subscription->impl->rmw_handle, serialized_message, &taken, message_info_local, allocation);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
if (RMW_RET_BAD_ALLOC == ret) {

8
rcl/test/rcl/test_publisher.cpp
View file

@ -93,7 +93,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin
test_msgs__msg__BasicTypes msg;
test_msgs__msg__BasicTypes__init(&msg);
msg.int64_value = 42;
ret = rcl_publish(&publisher, &msg);
ret = rcl_publish(&publisher, &msg, nullptr);
test_msgs__msg__BasicTypes__fini(&msg);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
@ -116,7 +116,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin
test_msgs__msg__Strings msg;
test_msgs__msg__Strings__init(&msg);
ASSERT_TRUE(rosidl_generator_c__String__assign(&msg.string_value, "testing"));
ret = rcl_publish(&publisher, &msg);
ret = rcl_publish(&publisher, &msg, nullptr);
test_msgs__msg__Strings__fini(&msg);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
@ -160,14 +160,14 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publishers_diff
test_msgs__msg__BasicTypes msg_int;
test_msgs__msg__BasicTypes__init(&msg_int);
msg_int.int64_value = 42;
ret = rcl_publish(&publisher, &msg_int);
ret = rcl_publish(&publisher, &msg_int, nullptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
test_msgs__msg__BasicTypes__fini(&msg_int);
test_msgs__msg__Strings msg_string;
test_msgs__msg__Strings__init(&msg_string);
ASSERT_TRUE(rosidl_generator_c__String__assign(&msg_string.string_value, "testing"));
ret = rcl_publish(&publisher_in_namespace, &msg_string);
ret = rcl_publish(&publisher_in_namespace, &msg_string, nullptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}

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

@ -163,7 +163,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
test_msgs__msg__BasicTypes msg;
test_msgs__msg__BasicTypes__init(&msg);
msg.int64_value = 42;
ret = rcl_publish(&publisher, &msg);
ret = rcl_publish(&publisher, &msg, nullptr);
test_msgs__msg__BasicTypes__fini(&msg);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
@ -176,7 +176,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
test_msgs__msg__BasicTypes__fini(&msg);
});
ret = rcl_take(&subscription, &msg, nullptr);
ret = rcl_take(&subscription, &msg, nullptr, nullptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ASSERT_EQ(42, msg.int64_value);
}
@ -214,7 +214,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
test_msgs__msg__Strings msg;
test_msgs__msg__Strings__init(&msg);
ASSERT_TRUE(rosidl_generator_c__String__assign(&msg.string_value, test_string));
ret = rcl_publish(&publisher, &msg);
ret = rcl_publish(&publisher, &msg, nullptr);
test_msgs__msg__Strings__fini(&msg);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
@ -227,7 +227,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
test_msgs__msg__Strings__fini(&msg);
});
ret = rcl_take(&subscription, &msg, nullptr);
ret = rcl_take(&subscription, &msg, nullptr, nullptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ASSERT_EQ(std::string(test_string), std::string(msg.string_value.data, msg.string_value.size));
}

2
rcl_action/src/rcl_action/action_client.c
View file

@ -390,7 +390,7 @@ rcl_action_take_cancel_response(
RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type, RCL_RET_INVALID_ARGUMENT); \
rmw_message_info_t message_info; /* ignored */ \
rcl_ret_t ret = rcl_take( \
&action_client->impl->Type ## _subscription, ros_ ## Type, &message_info); \
&action_client->impl->Type ## _subscription, ros_ ## Type, &message_info, NULL); \
if (RCL_RET_OK != ret) { \
if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { \
return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \

4
rcl_action/src/rcl_action/action_server.c
View file

@ -491,7 +491,7 @@ rcl_action_publish_feedback(
}
RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = rcl_publish(&action_server->impl->feedback_publisher, ros_feedback);
rcl_ret_t ret = rcl_publish(&action_server->impl->feedback_publisher, ros_feedback, NULL);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR; // error already set
}
@ -559,7 +559,7 @@ rcl_action_publish_status(
}
RCL_CHECK_ARGUMENT_FOR_NULL(status_message, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = rcl_publish(&action_server->impl->status_publisher, status_message);
rcl_ret_t ret = rcl_publish(&action_server->impl->status_publisher, status_message, NULL);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR; // error already set
}

2
rcl_lifecycle/src/com_interface.c
View file

@ -255,7 +255,7 @@ rcl_lifecycle_com_interface_publish_notification(
msg.goal_state.id = goal->id;
rosidl_generator_c__String__assign(&msg.goal_state.label, goal->label);
return rcl_publish(&com_interface->pub_transition_event, &msg);
return rcl_publish(&com_interface->pub_transition_event, &msg, NULL);
}
#ifdef __cplusplus