// Copyright 2016 Open Source Robotics Foundation, Inc.

//

// Licensed under the Apache License, Version 2.0 (the "License");

// you may not use this file except in compliance with the License.

// You may obtain a copy of the License at

//

// http://www.apache.org/licenses/LICENSE-2.0

//

// Unless required by applicable law or agreed to in writing, software

// distributed under the License is distributed on an "AS IS" BASIS,

// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

// See the License for the specific language governing permissions and

// limitations under the License.

#include "rclcpp/node_interfaces/node_graph.hpp"

#include <algorithm>

#include <cstddef>

#include <map>

#include <string>

#include <tuple>

#include <utility>

#include <vector>

#include "rcl/graph.h"

#include "rcl/remap.h"

#include "rclcpp/event.hpp"

#include "rclcpp/exceptions.hpp"

#include "rclcpp/expand_topic_or_service_name.hpp"

#include "rclcpp/graph_listener.hpp"

#include "rclcpp/node_interfaces/node_graph_interface.hpp"

#include "rcpputils/scope_exit.hpp"

using rclcpp::node_interfaces::NodeGraph;

using rclcpp::exceptions::throw_from_rcl_error;

using rclcpp::graph_listener::GraphListener;

NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base)

: node_base_(node_base),

should_add_to_graph_listener_(true),

graph_users_count_(0)

{}

NodeGraph::~NodeGraph()

{

// Remove self from graph listener.

// Exchange with false to prevent others from trying to add this node to the

// graph listener after checking that it was not here.

if (!should_add_to_graph_listener_.exchange(false)) {

// If it was already false, then it needs to now be removed.

node_base_->get_context()->get_graph_listener()->remove_node(this);

}

}

std::map<std::string, std::vector<std::string>>

NodeGraph::get_topic_names_and_types(bool no_demangle) const

{

rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();

rcl_allocator_t allocator = rcl_get_default_allocator();

auto ret = rcl_get_topic_names_and_types(

node_base_->get_rcl_node_handle(),

&allocator,

no_demangle,

&topic_names_and_types);

if (ret != RCL_RET_OK) {

auto error_msg = std::string("failed to get topic names and types: ") +

rcl_get_error_string().str;

rcl_reset_error();

if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {

error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

throw std::runtime_error(error_msg);

}

std::map<std::string, std::vector<std::string>> topics_and_types;

for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {

std::string topic_name = topic_names_and_types.names.data[i];

for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {

topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);

}

}

ret = rcl_names_and_types_fini(&topic_names_and_types);

if (ret != RCL_RET_OK) {

// *INDENT-OFF*

throw std::runtime_error(

std::string("could not destroy topic names and types: ") + rcl_get_error_string().str);

// *INDENT-ON*

}

return topics_and_types;

}

std::map<std::string, std::vector<std::string>>

NodeGraph::get_service_names_and_types() const

{

rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();

rcl_allocator_t allocator = rcl_get_default_allocator();

auto ret = rcl_get_service_names_and_types(

node_base_->get_rcl_node_handle(),

&allocator,

&service_names_and_types);

if (ret != RCL_RET_OK) {

auto error_msg = std::string("failed to get service names and types: ") +

rcl_get_error_string().str;

rcl_reset_error();

if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {

error_msg +=

std::string(", failed also to cleanup service names and types, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

throw std::runtime_error(error_msg);

}

std::map<std::string, std::vector<std::string>> services_and_types;

for (size_t i = 0; i < service_names_and_types.names.size; ++i) {

std::string service_name = service_names_and_types.names.data[i];

for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {

services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);

}

}

ret = rcl_names_and_types_fini(&service_names_and_types);

if (ret != RCL_RET_OK) {

// *INDENT-OFF*

throw std::runtime_error(

std::string("could not destroy service names and types: ") + rcl_get_error_string().str);

// *INDENT-ON*

}

return services_and_types;

}

std::map<std::string, std::vector<std::string>>

NodeGraph::get_service_names_and_types_by_node(

const std::string & node_name,

const std::string & namespace_) const

{

rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();

rcl_allocator_t allocator = rcl_get_default_allocator();

rcl_ret_t ret = rcl_get_service_names_and_types_by_node(

node_base_->get_rcl_node_handle(),

&allocator,

node_name.c_str(),

namespace_.c_str(),

&service_names_and_types);

if (ret != RCL_RET_OK) {

auto error_msg = std::string("failed to get service names and types by node: ") +

rcl_get_error_string().str;

rcl_reset_error();

if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {

error_msg +=

std::string(", failed also to cleanup service names and types, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

throw std::runtime_error(error_msg);

}

std::map<std::string, std::vector<std::string>> services_and_types;

for (size_t i = 0; i < service_names_and_types.names.size; ++i) {

std::string service_name = service_names_and_types.names.data[i];

for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {

services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);

}

}

ret = rcl_names_and_types_fini(&service_names_and_types);

if (ret != RCL_RET_OK) {

throw_from_rcl_error(ret, "could not destroy service names and types");

}

return services_and_types;

}

std::map<std::string, std::vector<std::string>>

NodeGraph::get_client_names_and_types_by_node(

const std::string & node_name,

const std::string & namespace_) const

{

rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();

auto service_names_and_types_finalizer = rcpputils::make_scope_exit(

[&service_names_and_types]() {

if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"), "could not destroy service names and types");

}

});

rcl_allocator_t allocator = rcl_get_default_allocator();

rcl_ret_t ret = rcl_get_client_names_and_types_by_node(

node_base_->get_rcl_node_handle(),

&allocator,

node_name.c_str(),

namespace_.c_str(),

&service_names_and_types);

if (ret != RCL_RET_OK) {

throw_from_rcl_error(ret, "failed to get service names and types by node");

}

std::map<std::string, std::vector<std::string>> services_and_types;

for (size_t i = 0; i < service_names_and_types.names.size; ++i) {

std::string service_name = service_names_and_types.names.data[i];

for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {

services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);

}

}

return services_and_types;

}

std::map<std::string, std::vector<std::string>>

NodeGraph::get_publisher_names_and_types_by_node(

const std::string & node_name,

const std::string & namespace_,

bool no_demangle) const

{

rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();

auto topic_names_and_types_finalizer = rcpputils::make_scope_exit(

[&topic_names_and_types]() {

if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"), "could not destroy topic names and types");

}

});

rcl_allocator_t allocator = rcl_get_default_allocator();

rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node(

node_base_->get_rcl_node_handle(),

&allocator,

no_demangle,

node_name.c_str(),

namespace_.c_str(),

&topic_names_and_types);

if (ret != RCL_RET_OK) {

throw_from_rcl_error(ret, "failed to get topic names and types by node");

}

std::map<std::string, std::vector<std::string>> topics_and_types;

for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {

std::string topic_name = topic_names_and_types.names.data[i];

for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {

topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);

}

}

return topics_and_types;

}

std::map<std::string, std::vector<std::string>>

NodeGraph::get_subscriber_names_and_types_by_node(

const std::string & node_name,

const std::string & namespace_,

bool no_demangle) const

{

rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();

auto topic_names_and_types_finalizer = rcpputils::make_scope_exit(

[&topic_names_and_types]() {

if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"), "could not destroy topic names and types");

}

});

rcl_allocator_t allocator = rcl_get_default_allocator();

rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node(

node_base_->get_rcl_node_handle(),

&allocator,

no_demangle,

node_name.c_str(),

namespace_.c_str(),

&topic_names_and_types);

if (ret != RCL_RET_OK) {

throw_from_rcl_error(ret, "failed to get topic names and types by node");

}

std::map<std::string, std::vector<std::string>> topics_and_types;

for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {

std::string topic_name = topic_names_and_types.names.data[i];

for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {

topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);

}

}

return topics_and_types;

}

std::vector<std::string>

NodeGraph::get_node_names() const

{

std::vector<std::string> nodes;

auto names_and_namespaces = get_node_names_and_namespaces();

std::transform(

names_and_namespaces.begin(),

names_and_namespaces.end(),

std::back_inserter(nodes),

[](std::pair<std::string, std::string> nns) {

std::string return_string;

if (nns.second.back() == '/') {

return_string = nns.second + nns.first;

} else {

return_string = nns.second + '/' + nns.first;

}

// Quick check to make sure that we start with a slash

// Since fully-qualified strings need to

if (return_string.front() != '/') {

return_string = "/" + return_string;

}

return return_string;

}

);

return nodes;

}

std::vector<std::tuple<std::string, std::string, std::string>>

NodeGraph::get_node_names_with_enclaves() const

{

rcutils_string_array_t node_names_c =

rcutils_get_zero_initialized_string_array();

rcutils_string_array_t node_namespaces_c =

rcutils_get_zero_initialized_string_array();

rcutils_string_array_t node_enclaves_c =

rcutils_get_zero_initialized_string_array();

auto allocator = rcl_get_default_allocator();

auto ret = rcl_get_node_names_with_enclaves(

node_base_->get_rcl_node_handle(),

allocator,

&node_names_c,

&node_namespaces_c,

&node_enclaves_c);

if (ret != RCL_RET_OK) {

auto error_msg =

std::string("failed to get node names with enclaves: ") + rcl_get_error_string().str;

rcl_reset_error();

if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {

error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {

error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

if (rcutils_string_array_fini(&node_enclaves_c) != RCUTILS_RET_OK) {

error_msg += std::string(", failed also to cleanup node enclaves, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

throw std::runtime_error(error_msg);

}

std::vector<std::tuple<std::string, std::string, std::string>> node_tuples;

for (size_t i = 0; i < node_names_c.size; ++i) {

if (node_names_c.data[i] && node_namespaces_c.data[i] && node_enclaves_c.data[i]) {

node_tuples.emplace_back(

std::make_tuple(node_names_c.data[i], node_namespaces_c.data[i], node_enclaves_c.data[i]));

}

}

std::string error("failed to finalize array");

rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c);

if (ret_names != RCUTILS_RET_OK) {

error += std::string(", could not destroy node names, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c);

if (ret_ns != RCUTILS_RET_OK) {

error += std::string(", could not destroy node namespaces, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

rcl_ret_t ret_ecv = rcutils_string_array_fini(&node_enclaves_c);

if (ret_ecv != RCUTILS_RET_OK) {

error += std::string(", could not destroy node enclaves, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK || ret_ecv != RCUTILS_RET_OK) {

throw std::runtime_error(error);

}

return node_tuples;

}

std::vector<std::pair<std::string, std::string>>

NodeGraph::get_node_names_and_namespaces() const

{

rcutils_string_array_t node_names_c =

rcutils_get_zero_initialized_string_array();

rcutils_string_array_t node_namespaces_c =

rcutils_get_zero_initialized_string_array();

auto allocator = rcl_get_default_allocator();

auto ret = rcl_get_node_names(

node_base_->get_rcl_node_handle(),

allocator,

&node_names_c,

&node_namespaces_c);

if (ret != RCL_RET_OK) {

auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str;

rcl_reset_error();

if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {

error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {

error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

RCUTILS_LOG_ERROR_NAMED("rclcpp", "%s", error_msg.c_str());

throw std::runtime_error(error_msg);

}

std::vector<std::pair<std::string, std::string>> node_names;

node_names.reserve(node_names_c.size);

for (size_t i = 0; i < node_names_c.size; ++i) {

if (node_names_c.data[i] && node_namespaces_c.data[i]) {

node_names.emplace_back(node_names_c.data[i], node_namespaces_c.data[i]);

}

}

std::string error;

rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c);

if (ret_names != RCUTILS_RET_OK) {

error = std::string("could not destroy node names: ") + rcl_get_error_string().str;

}

rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c);

if (ret_ns != RCUTILS_RET_OK) {

error += std::string(", could not destroy node namespaces: ") + rcl_get_error_string().str;

}

if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK) {

RCUTILS_LOG_ERROR_NAMED("rclcpp", "%s", error.c_str());

throw std::runtime_error(error);

}

return node_names;

}

size_t

NodeGraph::count_publishers(const std::string & topic_name) const

{

auto rcl_node_handle = node_base_->get_rcl_node_handle();

auto fqdn = rclcpp::expand_topic_or_service_name(

topic_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

false); // false = not a service

size_t count;

auto ret = rcl_count_publishers(rcl_node_handle, fqdn.c_str(), &count);

if (ret != RMW_RET_OK) {

// *INDENT-OFF*

throw std::runtime_error(

std::string("could not count publishers: ") + rmw_get_error_string().str);

// *INDENT-ON*

}

return count;

}

size_t

NodeGraph::count_subscribers(const std::string & topic_name) const

{

auto rcl_node_handle = node_base_->get_rcl_node_handle();

auto fqdn = rclcpp::expand_topic_or_service_name(

topic_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

false); // false = not a service

size_t count;

auto ret = rcl_count_subscribers(rcl_node_handle, fqdn.c_str(), &count);

if (ret != RMW_RET_OK) {

// *INDENT-OFF*

throw std::runtime_error(

std::string("could not count subscribers: ") + rmw_get_error_string().str);

// *INDENT-ON*

}

return count;

}

size_t

NodeGraph::count_clients(const std::string & service_name) const

{

auto rcl_node_handle = node_base_->get_rcl_node_handle();

auto fqdn = rclcpp::expand_topic_or_service_name(

service_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

true);

size_t count;

auto ret = rcl_count_clients(rcl_node_handle, fqdn.c_str(), &count);

if (ret != RMW_RET_OK) {

// *INDENT-OFF*

throw std::runtime_error(

std::string("could not count clients: ") + rmw_get_error_string().str);

// *INDENT-ON*

}

return count;

}

size_t

NodeGraph::count_services(const std::string & service_name) const

{

auto rcl_node_handle = node_base_->get_rcl_node_handle();

auto fqdn = rclcpp::expand_topic_or_service_name(

service_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

true);

size_t count;

auto ret = rcl_count_services(rcl_node_handle, fqdn.c_str(), &count);

if (ret != RMW_RET_OK) {

// *INDENT-OFF*

throw std::runtime_error(

std::string("could not count services: ") + rmw_get_error_string().str);

// *INDENT-ON*

}

return count;

}

const rcl_guard_condition_t *

NodeGraph::get_graph_guard_condition() const

{

return rcl_node_get_graph_guard_condition(node_base_->get_rcl_node_handle());

}

void

NodeGraph::notify_graph_change()

{

{

std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);

bool bad_ptr_encountered = false;

for (auto & event_wptr : graph_events_) {

auto event_ptr = event_wptr.lock();

if (event_ptr) {

event_ptr->set();

} else {

bad_ptr_encountered = true;

}

}

if (bad_ptr_encountered) {

// remove invalid pointers with the erase-remove idiom

graph_events_.erase(

std::remove_if(

graph_events_.begin(),

graph_events_.end(),

[](const rclcpp::Event::WeakPtr & wptr) {

return wptr.expired();

}),

graph_events_.end());

// update graph_users_count_

graph_users_count_.store(graph_events_.size());

}

}

graph_cv_.notify_all();

try {

node_base_->trigger_notify_guard_condition();

} catch (const rclcpp::exceptions::RCLError & ex) {

throw std::runtime_error(

std::string("failed to notify wait set on graph change: ") + ex.what());

}

}

void

NodeGraph::notify_shutdown()

{

// notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown().

graph_cv_.notify_all();

}

rclcpp::Event::SharedPtr

NodeGraph::get_graph_event()

{

auto event = rclcpp::Event::make_shared();

{

std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);

graph_events_.push_back(event);

graph_users_count_++;

}

// on first call, add node to graph_listener_

if (should_add_to_graph_listener_.exchange(false)) {

node_base_->get_context()->get_graph_listener()->add_node(this);

node_base_->get_context()->get_graph_listener()->start_if_not_started();

}

return event;

}

void

NodeGraph::wait_for_graph_change(

rclcpp::Event::SharedPtr event,

std::chrono::nanoseconds timeout)

{

using rclcpp::exceptions::InvalidEventError;

using rclcpp::exceptions::EventNotRegisteredError;

if (!event) {

throw InvalidEventError();

}

{

std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);

bool event_in_graph_events = false;

for (const auto & event_wptr : graph_events_) {

if (event == event_wptr.lock()) {

event_in_graph_events = true;

break;

}

}

if (!event_in_graph_events) {

throw EventNotRegisteredError();

}

}

auto pred = [&event, context = node_base_->get_context()]() {

return event->check() || !rclcpp::ok(context);

};

std::unique_lock<std::mutex> graph_lock(graph_mutex_);

if (!pred()) {

graph_cv_.wait_for(graph_lock, timeout, pred);

}

}

size_t

NodeGraph::count_graph_users() const

{

return graph_users_count_.load();

}

static

std::vector<rclcpp::TopicEndpointInfo>

convert_to_topic_info_list(const rcl_topic_endpoint_info_array_t & info_array)

{

std::vector<rclcpp::TopicEndpointInfo> topic_info_list;

topic_info_list.reserve(info_array.size);

for (size_t i = 0; i < info_array.size; ++i) {

topic_info_list.emplace_back(info_array.info_array[i]);

}

return topic_info_list;

}

template<const char * EndpointType, typename FunctionT>

static std::vector<rclcpp::TopicEndpointInfo>

get_info_by_topic(

rclcpp::node_interfaces::NodeBaseInterface * node_base,

const std::string & topic_name,

bool no_mangle,

FunctionT rcl_get_info_by_topic)

{

std::string fqdn;

auto rcl_node_handle = node_base->get_rcl_node_handle();

if (no_mangle) {

fqdn = topic_name;

} else {

fqdn = rclcpp::expand_topic_or_service_name(

topic_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

false); // false = not a service

// Get the node options

const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle);

if (nullptr == node_options) {

throw std::runtime_error("Need valid node options in get_info_by_topic()");

}

const rcl_arguments_t * global_args = nullptr;

if (node_options->use_global_arguments) {

global_args = &(rcl_node_handle->context->global_arguments);

}

char * remapped_topic_name = nullptr;

rcl_ret_t ret = rcl_remap_topic_name(

&(node_options->arguments),

global_args,

fqdn.c_str(),

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

node_options->allocator,

&remapped_topic_name);

if (RCL_RET_OK != ret) {

throw_from_rcl_error(ret, std::string("Failed to remap topic name ") + fqdn);

} else if (nullptr != remapped_topic_name) {

fqdn = remapped_topic_name;

node_options->allocator.deallocate(remapped_topic_name, node_options->allocator.state);

}

}

rcutils_allocator_t allocator = rcutils_get_default_allocator();

rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array();

rcl_ret_t ret =

rcl_get_info_by_topic(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array);

if (RCL_RET_OK != ret) {

auto error_msg =

std::string("Failed to get information by topic for ") + EndpointType + std::string(":");

if (RCL_RET_UNSUPPORTED == ret) {

error_msg += std::string("function not supported by RMW_IMPLEMENTATION");

} else {

error_msg += rcl_get_error_string().str;

}

rcl_reset_error();

if (RCL_RET_OK != rcl_topic_endpoint_info_array_fini(&info_array, &allocator)) {

error_msg += std::string(", failed also to cleanup topic info array, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

throw_from_rcl_error(ret, error_msg);

}

std::vector<rclcpp::TopicEndpointInfo> topic_info_list = convert_to_topic_info_list(info_array);

ret = rcl_topic_endpoint_info_array_fini(&info_array, &allocator);

if (RCL_RET_OK != ret) {

throw_from_rcl_error(ret, "rcl_topic_info_array_fini failed.");

}

return topic_info_list;

}

static constexpr char kPublisherEndpointTypeName[] = "publishers";

std::vector<rclcpp::TopicEndpointInfo>

NodeGraph::get_publishers_info_by_topic(

const std::string & topic_name,

bool no_mangle) const

{

return get_info_by_topic<kPublisherEndpointTypeName>(

node_base_,

topic_name,

no_mangle,

rcl_get_publishers_info_by_topic);

}

static constexpr char kSubscriptionEndpointTypeName[] = "subscriptions";

std::vector<rclcpp::TopicEndpointInfo>

NodeGraph::get_subscriptions_info_by_topic(

const std::string & topic_name,

bool no_mangle) const

{

return get_info_by_topic<kSubscriptionEndpointTypeName>(

node_base_,

topic_name,

no_mangle,

rcl_get_subscriptions_info_by_topic);

}

static

std::vector<rclcpp::ServiceEndpointInfo>

convert_to_service_info_list(const rcl_service_endpoint_info_array_t & info_array)

{

std::vector<rclcpp::ServiceEndpointInfo> service_info_list;

service_info_list.reserve(info_array.size);

for (size_t i = 0; i < info_array.size; ++i) {

service_info_list.emplace_back(info_array.info_array[i]);

}

return service_info_list;

}

template<const char * EndpointType, typename FunctionT>

static std::vector<rclcpp::ServiceEndpointInfo>

get_info_by_service(

rclcpp::node_interfaces::NodeBaseInterface * node_base,

const std::string & service_name,

bool no_mangle,

FunctionT rcl_get_info_by_service)

{

std::string fqdn;

auto rcl_node_handle = node_base->get_rcl_node_handle();

if (no_mangle) {

fqdn = service_name;

} else {

fqdn = rclcpp::expand_topic_or_service_name(

service_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

true);

// Get the node options

const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle);

if (nullptr == node_options) {

throw std::runtime_error("Need valid node options in get_info_by_service()");

}

const rcl_arguments_t * global_args = nullptr;

if (node_options->use_global_arguments) {

global_args = &(rcl_node_handle->context->global_arguments);

}

char * remapped_service_name = nullptr;

rcl_ret_t ret = rcl_remap_service_name(

&(node_options->arguments),

global_args,

fqdn.c_str(),

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

node_options->allocator,

&remapped_service_name);

if (RCL_RET_OK != ret) {

throw_from_rcl_error(ret, std::string("Failed to remap service name ") + fqdn);

} else if (nullptr != remapped_service_name) {

fqdn = remapped_service_name;

node_options->allocator.deallocate(remapped_service_name, node_options->allocator.state);

}

}

rcutils_allocator_t allocator = rcutils_get_default_allocator();

rcl_service_endpoint_info_array_t info_array =

rcl_get_zero_initialized_service_endpoint_info_array();

rcl_ret_t ret =

rcl_get_info_by_service(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array);

if (RCL_RET_OK != ret) {

auto error_msg =

std::string("Failed to get information by service for ") + EndpointType + std::string(":");

if (RCL_RET_UNSUPPORTED == ret) {

error_msg += std::string("function not supported by RMW_IMPLEMENTATION");

} else {

error_msg += rcl_get_error_string().str;

}

rcl_reset_error();

if (RCL_RET_OK != rcl_service_endpoint_info_array_fini(&info_array, &allocator)) {

error_msg += std::string(", failed also to cleanup service info array, leaking memory: ") +

rcl_get_error_string().str;

rcl_reset_error();

}

throw_from_rcl_error(ret, error_msg);

}

std::vector<rclcpp::ServiceEndpointInfo> service_info_list =

convert_to_service_info_list(info_array);

ret = rcl_service_endpoint_info_array_fini(&info_array, &allocator);

if (RCL_RET_OK != ret) {

throw_from_rcl_error(ret, "rcl_service_info_array_fini failed.");

}

return service_info_list;

}

static constexpr char kClientEndpointTypeName[] = "clients";

std::vector<rclcpp::ServiceEndpointInfo>

NodeGraph::get_clients_info_by_service(

const std::string & service_name,

bool no_mangle) const

{

return get_info_by_service<kClientEndpointTypeName>(

node_base_,

service_name,

no_mangle,

rcl_get_clients_info_by_service);

}

static constexpr char kServerEndpointTypeName[] = "servers";

std::vector<rclcpp::ServiceEndpointInfo>

NodeGraph::get_servers_info_by_service(

const std::string & service_name,

bool no_mangle) const

{

return get_info_by_service<kServerEndpointTypeName>(

node_base_,

service_name,

no_mangle,

rcl_get_servers_info_by_service);

}

std::string &

rclcpp::TopicEndpointInfo::node_name()

{

return node_name_;

}

const std::string &

rclcpp::TopicEndpointInfo::node_name() const

{

return node_name_;

}

std::string &

rclcpp::TopicEndpointInfo::node_namespace()

{

return node_namespace_;

}

const std::string &

rclcpp::TopicEndpointInfo::node_namespace() const

{

return node_namespace_;

}

std::string &

rclcpp::TopicEndpointInfo::topic_type()

{

return topic_type_;

}

const std::string &

rclcpp::TopicEndpointInfo::topic_type() const

{

return topic_type_;

}

rclcpp::EndpointType &

rclcpp::TopicEndpointInfo::endpoint_type()

{

return endpoint_type_;

}

const rclcpp::EndpointType &

rclcpp::TopicEndpointInfo::endpoint_type() const

{

return endpoint_type_;

}

std::array<uint8_t, RMW_GID_STORAGE_SIZE> &

rclcpp::TopicEndpointInfo::endpoint_gid()

{

return endpoint_gid_;

}

const std::array<uint8_t, RMW_GID_STORAGE_SIZE> &

rclcpp::TopicEndpointInfo::endpoint_gid() const

{

return endpoint_gid_;

}

rclcpp::QoS &

rclcpp::TopicEndpointInfo::qos_profile()

{

return qos_profile_;

}

const rclcpp::QoS &

rclcpp::TopicEndpointInfo::qos_profile() const

{

return qos_profile_;

}

rosidl_type_hash_t &

rclcpp::TopicEndpointInfo::topic_type_hash()

{

return topic_type_hash_;

}

const rosidl_type_hash_t &

rclcpp::TopicEndpointInfo::topic_type_hash() const

{

return topic_type_hash_;

}

std::string &

rclcpp::ServiceEndpointInfo::node_name()

{

return node_name_;

}

const std::string &

rclcpp::ServiceEndpointInfo::node_name() const

{

return node_name_;

}

std::string &

rclcpp::ServiceEndpointInfo::node_namespace()

{

return node_namespace_;

}

const std::string &

rclcpp::ServiceEndpointInfo::node_namespace() const

{

return node_namespace_;

}

std::string &

rclcpp::ServiceEndpointInfo::service_type()

{

return service_type_;

}

const std::string &

rclcpp::ServiceEndpointInfo::service_type() const

{

return service_type_;

}

rclcpp::EndpointType &

rclcpp::ServiceEndpointInfo::endpoint_type()

{

return endpoint_type_;

}