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
Browse files Browse the repository at this point in the history 
Signed-off-by: captaintrunky <captaintrunky@gmail.com>
  • Loading branch information
@CaptainTrunky
CaptainTrunky committed Oct 23, 2019
1 parent 4393a7a commit dd4d3e5
Showing 1 changed file with 116 additions and 25 deletions.
141 changes: 116 additions & 25 deletions rmw_connext_shared_cpp/src/node_names.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>
#include <string>
#include <vector>

Expand Down Expand Up @@ -48,38 +49,51 @@ 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;
}

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_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);
}
rcutils_ret = rcutils_string_array_init(node_namespaces, length, &allocator);

rcutils_string_array_t tmp_namespaces_list = rcutils_get_zero_initialized_string_array();
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);
}

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]) {
int named_nodes_num = 0;
int named_namespaces_num = 0;

// 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 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;
goto fail;
}

for (auto i = 1; i < length; ++i) {
Expand All @@ -90,44 +104,100 @@ 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]) {
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 node name");
goto fail;
}

node_namespaces->data[i] = rcutils_strdup(namespace_.c_str(), allocator);
if (!node_namespaces->data[i]) {
++named_nodes_num;

if (namespace_.empty()) {
tmp_namespaces_list.data[i] = nullptr;
continue;
}

tmp_namespaces_list.data[named_namespaces_num] = rcutils_strdup(namespace_.c_str(), allocator);
if (!tmp_namespaces_list.data[named_namespaces_num]) {
RMW_SET_ERROR_MSG("could not allocate memory for node namespace");
goto fail;
}

++named_namespaces_num;
}

// prepare actual output without empty names
// TODO: implement functionality that allocates just enough memory for the output buffer,
// so using additional buffer would be unnessecary
rcutils_ret = rcutils_string_array_init(node_names, named_nodes_num, &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);
}

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;
}

for (auto i = 0; i < named_nodes_num; ++i) {
std::cerr << tmp_names_list.data[i] << '\n';
}
rcutils_ret = rcutils_string_array_fini(&tmp_names_list);
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);
}

rcutils_ret = rcutils_string_array_init(node_namespaces, named_namespaces_num, &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);
}

for (auto i = 0; i < named_namespaces_num; ++i) {
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_namespaces_list);
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);
}

return RMW_RET_OK;

fail:
if (node_names) {
rcutils_ret = rcutils_string_array_fini(node_names);
Expand All @@ -138,14 +208,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();
}
}

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 RMW_RET_BAD_ALLOC;
}

0 comments on commit dd4d3e5

Please sign in to comment.