Skip to content
This repository has been archived by the owner on Jun 21, 2023. It is now read-only.

Commit

Permalink
Filtering empty names with naive approach (#362)
Browse files Browse the repository at this point in the history 
Use an over-sized temporary array to store names before copying non-empty names to the output array.

Signed-off-by: captaintrunky <captaintrunky@gmail.com>
  • Loading branch information
@CaptainTrunky @jacobperron
CaptainTrunky authored and jacobperron committed Dec 11, 2019
1 parent b1062f9 commit 223315a
Showing 1 changed file with 121 additions and 34 deletions.
155 changes: 121 additions & 34 deletions rmw_connext_shared_cpp/src/node_names.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,38 +48,56 @@ get_node_names(

DDS::DomainParticipant * participant = static_cast<ConnextNodeInfo *>(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) {
Expand All @@ -90,45 +108,93 @@ get_node_names(
if (DDS::RETCODE_OK == dds_ret) {
auto data = static_cast<unsigned char *>(pbtd.user_data.value.get_contiguous_buffer());
std::vector<uint8_t> 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) {
name = pbtd.participant_name.name;
}
}
}

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

0 comments on commit 223315a

Please sign in to comment.