Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Rmw preallocate #428

Merged
merged 4 commits into from
May 2, 2019
Merged

Rmw preallocate #428

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1eb5467
Proposola of changes for RMW_Preallocate. Related /ros2/rmw#160
Feb 6, 2019
fc155f1
Changed RCL interface
Feb 14, 2019
f611286
Updates for allocation in serialize methods.
mjcarroll May 1, 2019
7126b49
Fix tests for new APIs.
mjcarroll May 1, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 13 additions & 3 deletions rcl/include/rcl/publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
/**
Expand Down Expand Up @@ -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
Expand All @@ -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.
/**
Expand All @@ -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
Expand All @@ -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.
/**
Expand Down
12 changes: 9 additions & 3 deletions rcl/include/rcl/subscription.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
/**
Expand Down Expand Up @@ -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
Expand All @@ -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.
/**
Expand Down Expand Up @@ -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
Expand All @@ -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.
/**
Expand Down
2 changes: 1 addition & 1 deletion rcl/src/rcl/logging_rosout.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
22 changes: 17 additions & 5 deletions rcl/src/rcl/publisher.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit; I like adding one newline, two seems excessive :).


// 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();
Expand Down Expand Up @@ -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
Expand All @@ -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:
Expand Down Expand Up @@ -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;
}
Expand All @@ -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) {
Expand Down
16 changes: 11 additions & 5 deletions rcl/src/rcl/subscription.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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)) {
Expand All @@ -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) {
Expand All @@ -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)) {
Expand All @@ -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) {
Expand Down
8 changes: 4 additions & 4 deletions rcl/test/rcl/test_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
}

Expand Down
8 changes: 4 additions & 4 deletions rcl/test/rcl/test_subscription.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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);
}
Expand Down Expand Up @@ -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;
}
Expand All @@ -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));
}
Expand Down
2 changes: 1 addition & 1 deletion rcl_action/src/rcl_action/action_client.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; \
Expand Down
4 changes: 2 additions & 2 deletions rcl_action/src/rcl_action/action_server.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down Expand Up @@ -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
}
Expand Down
2 changes: 1 addition & 1 deletion rcl_lifecycle/src/com_interface.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down