From 223315a131a272544608a70407327e91e2432b6a Mon Sep 17 00:00:00 2001 From: CaptainTrunky Date: Wed, 11 Dec 2019 05:26:02 +0300 Subject: [PATCH] Filtering empty names with naive approach (#362) Use an over-sized temporary array to store names before copying non-empty names to the output array. Signed-off-by: captaintrunky --- rmw_connext_shared_cpp/src/node_names.cpp | 155 +++++++++++++++++----- 1 file changed, 121 insertions(+), 34 deletions(-) diff --git a/rmw_connext_shared_cpp/src/node_names.cpp b/rmw_connext_shared_cpp/src/node_names.cpp index 079f8dae..1fefa70b 100644 --- a/rmw_connext_shared_cpp/src/node_names.cpp +++ b/rmw_connext_shared_cpp/src/node_names.cpp @@ -48,38 +48,56 @@ get_node_names( DDS::DomainParticipant * participant = static_cast(node->data)->participant; DDS::InstanceHandleSeq handles; + if (participant->get_discovered_participants(handles) != DDS::RETCODE_OK) { RMW_SET_ERROR_MSG("unable to fetch discovered participants."); return RMW_RET_ERROR; } + + DDS::DomainParticipantQos participant_qos; + DDS::ReturnCode_t status = participant->get_qos(participant_qos); + + if (status != DDS::RETCODE_OK) { + RMW_SET_ERROR_MSG("failed to get default participant qos"); + return RMW_RET_ERROR; + } + + rmw_ret_t final_ret = RMW_RET_OK; + auto length = handles.length() + 1; // add yourself + rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rcutils_ret_t rcutils_ret = rcutils_string_array_init(node_names, length, &allocator); + + // Pre-allocate temporary buffer for all node names & namespaces + // Nodes that are not created by ROS 2 could provide empty names + // Such names should not be returned + rcutils_string_array_t tmp_names_list = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t tmp_namespaces_list = rcutils_get_zero_initialized_string_array(); + + int named_nodes_num = 1; + + rcutils_ret_t rcutils_ret = rcutils_string_array_init(&tmp_names_list, length, &allocator); + if (rcutils_ret != RCUTILS_RET_OK) { RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; } - rcutils_ret = rcutils_string_array_init(node_namespaces, length, &allocator); + + rcutils_ret = rcutils_string_array_init(&tmp_namespaces_list, length, &allocator); + if (rcutils_ret != RCUTILS_RET_OK) { RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; } - DDS::DomainParticipantQos participant_qos; - DDS::ReturnCode_t status = participant->get_qos(participant_qos); - if (status != DDS::RETCODE_OK) { - RMW_SET_ERROR_MSG("failed to get default participant qos"); - return RMW_RET_ERROR; - } - node_names->data[0] = rcutils_strdup(participant_qos.participant_name.name, allocator); - if (!node_names->data[0]) { - RMW_SET_ERROR_MSG("could not allocate memory for node name"); - return RMW_RET_BAD_ALLOC; - } - node_namespaces->data[0] = rcutils_strdup(node->namespace_, allocator); - if (!node_names->data[0]) { - RMW_SET_ERROR_MSG("could not allocate memory for node namespace"); - return RMW_RET_BAD_ALLOC; + // add yourself + tmp_names_list.data[0] = rcutils_strdup(participant_qos.participant_name.name, allocator); + if (!tmp_names_list.data[0]) { + RMW_SET_ERROR_MSG("could not allocate memory for a node name"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; } for (auto i = 1; i < length; ++i) { @@ -90,16 +108,20 @@ get_node_names( if (DDS::RETCODE_OK == dds_ret) { auto data = static_cast(pbtd.user_data.value.get_contiguous_buffer()); std::vector kv(data, data + pbtd.user_data.value.length()); + auto map = rmw::impl::cpp::parse_key_value(kv); + auto name_found = map.find("name"); auto ns_found = map.find("namespace"); if (name_found != map.end()) { name = std::string(name_found->second.begin(), name_found->second.end()); } - if (name_found != map.end()) { + + if (ns_found != map.end()) { namespace_ = std::string(ns_found->second.begin(), ns_found->second.end()); } + if (name.empty()) { // use participant name if no name was found in the user data if (pbtd.participant_name.name) { @@ -107,28 +129,72 @@ get_node_names( } } } + + // ignore discovered participants without a name if (name.empty()) { - // ignore discovered participants without a name - node_names->data[i] = nullptr; - node_namespaces->data[i] = nullptr; + tmp_names_list.data[i] = nullptr; + tmp_namespaces_list.data[i] = nullptr; continue; } - node_names->data[i] = rcutils_strdup(name.c_str(), allocator); - if (!node_names->data[i]) { - RMW_SET_ERROR_MSG("could not allocate memory for node name"); - goto fail; + tmp_names_list.data[named_nodes_num] = rcutils_strdup(name.c_str(), allocator); + if (!tmp_names_list.data[named_nodes_num]) { + RMW_SET_ERROR_MSG("could not allocate memory for a node's name"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; } - node_namespaces->data[i] = rcutils_strdup(namespace_.c_str(), allocator); - if (!node_namespaces->data[i]) { - RMW_SET_ERROR_MSG("could not allocate memory for node namespace"); - goto fail; + tmp_namespaces_list.data[named_nodes_num] = rcutils_strdup(namespace_.c_str(), allocator); + if (!tmp_namespaces_list.data[named_nodes_num]) { + RMW_SET_ERROR_MSG("could not allocate memory for a node's namespace"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; } + + ++named_nodes_num; + } + + // prepare actual output without empty names + rcutils_ret = rcutils_string_array_init(node_names, named_nodes_num, &allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG("could not allocate memory for node_names output"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; + } + + rcutils_ret = rcutils_string_array_init(node_namespaces, named_nodes_num, &allocator); + + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG("could not allocate memory for node_namespaces output"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; + } + + for (auto i = 0; i < named_nodes_num; ++i) { + node_names->data[i] = rcutils_strdup(tmp_names_list.data[i], allocator); + tmp_names_list.data[i] = nullptr; + + node_namespaces->data[i] = rcutils_strdup(tmp_namespaces_list.data[i], allocator); + tmp_namespaces_list.data[i] = nullptr; + } + + rcutils_ret = rcutils_string_array_fini(&tmp_names_list); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG("failed to free tmp_names_list"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; + } + + rcutils_ret = rcutils_string_array_fini(&tmp_namespaces_list); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG("failed to free tmp_namespaces_list"); + final_ret = rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + goto cleanup; } return RMW_RET_OK; -fail: + +cleanup: if (node_names) { rcutils_ret = rcutils_string_array_fini(node_names); if (rcutils_ret != RCUTILS_RET_OK) { @@ -138,14 +204,35 @@ get_node_names( 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_connext_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + "failed to cleanup during error handling: %s", rcutils_get_error_string().str + ); rcutils_reset_error(); } } - return RMW_RET_BAD_ALLOC; + + rcutils_ret = rcutils_string_array_fini(&tmp_names_list); + if (rcutils_ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_connext_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str + ); + rcutils_reset_error(); + } + + rcutils_ret = rcutils_string_array_fini(&tmp_namespaces_list); + if (rcutils_ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_connext_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str + ); + rcutils_reset_error(); + } + + return final_ret; }