bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
{
for (auto subscription : subscription_handles_) {
if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
return false;
}
}
for (auto client : client_handles_) {
if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Couldn't add client to wait set: %s", rcl_get_error_string_safe());
return false;
}
}
for (auto service : service_handles_) {
if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Couldn't add service to wait set: %s", rcl_get_error_string_safe());
return false;
}
}
for (auto timer : timer_handles_) {
if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
return false;
}
}
for (auto guard_condition : guard_conditions_) {
if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Couldn't add guard_condition to wait set: %s",
rcl_get_error_string_safe());
return false;
}
}
return true;
}
static std::string convert_to_std_string(void * data)
{
auto c_string = static_cast<rosidl_generator_c__String *>(data);
if (!c_string) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_dynamic_cpp",
"Failed to cast data as rosidl_generator_c__String");
return "";
}
if (!c_string->data) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_dynamic_cpp",
"rosidl_generator_c_String had invalid data");
return "";
}
return std::string(c_string->data);
}
static size_t next_field_align(void * data, size_t current_alignment)
{
auto c_string = static_cast<rosidl_generator_c__String *>(data);
if (!c_string) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_dynamic_cpp",
"Failed to cast data as rosidl_generator_c__String");
return current_alignment;
}
if (!c_string->data) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_dynamic_cpp",
"rosidl_generator_c_String had invalid data");
return current_alignment;
}
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
current_alignment += 4;
return current_alignment + strlen(c_string->data) + 1;
}
virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message(size_t capacity)
{
auto msg = new rcl_serialized_message_t;
*msg = rmw_get_zero_initialized_serialized_message();
auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(msg,
[](rmw_serialized_message_t * msg) {
auto ret = rmw_serialized_message_fini(msg);
delete msg;
if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"failed to destroy serialized message: %s", rcl_get_error_string().str);
}
});
return serialized_msg;
}
rmw_ret_t
__rmw_get_node_names(
const char * identifier,
const rmw_node_t * node,
rcutils_string_array_t * node_names,
rcutils_string_array_t * node_namespaces)
{
if (!node) {
RMW_SET_ERROR_MSG("null node handle");
return RMW_RET_ERROR;
}
if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) {
return RMW_RET_ERROR;
}
if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) {
return RMW_RET_ERROR;
}
// Get participant pointer from node
if (node->implementation_identifier != identifier) {
RMW_SET_ERROR_MSG("node handle not from this implementation");
return RMW_RET_ERROR;
}
auto impl = static_cast<CustomParticipantInfo *>(node->data);
auto participant_names = impl->listener->get_discovered_names();
auto participant_ns = impl->listener->get_discovered_namespaces();
rcutils_allocator_t allocator = rcutils_get_default_allocator();
rcutils_ret_t rcutils_ret =
rcutils_string_array_init(node_names, participant_names.size() + 1, &allocator);
if (rcutils_ret != RCUTILS_RET_OK) {
RMW_SET_ERROR_MSG(rcutils_get_error_string().str);
goto fail;
}
rcutils_ret =
rcutils_string_array_init(node_namespaces, participant_names.size() + 1, &allocator);
if (rcutils_ret != RCUTILS_RET_OK) {
RMW_SET_ERROR_MSG(rcutils_get_error_string().str);
goto fail;
}
for (size_t i = 0; i < participant_names.size() + 1; ++i) {
if (0 == i) {
node_names->data[i] = rcutils_strdup(node->name, allocator);
node_namespaces->data[i] = rcutils_strdup(node->namespace_, allocator);
} else {
node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator);
node_namespaces->data[i] = rcutils_strdup(participant_ns[i - 1].c_str(), allocator);
}
if (!node_names->data[i] || !node_namespaces->data[i]) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
}
return RMW_RET_OK;
fail:
if (node_names) {
rcutils_ret = rcutils_string_array_fini(node_names);
if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
if (node_namespaces) {
rcutils_ret = rcutils_string_array_fini(node_namespaces);
if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
return RMW_RET_BAD_ALLOC;
}