Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow to bridge between ROS_LOCALHOST=0 and ROS_LOCALHOST=1 #78

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions include/domain_bridge/domain_bridge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ class DomainBridge
* \param type: Name of the topic type (e.g. "example_interfaces/msg/String")
* \param from_domain_id: Domain ID the bridge will use to subscribe to the topic.
* \param to_domain_id: Domain ID the bridge will use to publish to the topic.
* \param from_local_host_only: Indicates the localhost mode that apply to the subscriber of the topic.
* \param to_local_host_only: Indicates the localhost mode that apply to the publisher of the topic.
* \param options: Options for bridging the topic.
*/
DOMAIN_BRIDGE_PUBLIC
Expand All @@ -122,6 +124,8 @@ class DomainBridge
const std::string & type,
size_t from_domain_id,
size_t to_domain_id,
DomainBridgeOptions::LocalHostOnly from_local_host_only,
DomainBridgeOptions::LocalHostOnly to_local_host_only,
const TopicBridgeOptions & options = TopicBridgeOptions());

/// Bridge a topic from one domain to another.
Expand All @@ -146,6 +150,8 @@ class DomainBridge
const std::string & service,
size_t from_domain_id,
size_t to_domain_id,
DomainBridgeOptions::LocalHostOnly from_local_host_only,
DomainBridgeOptions::LocalHostOnly to_local_host_only,
const ServiceBridgeOptions & options = ServiceBridgeOptions());

/// Get bridged topics.
Expand Down
4 changes: 4 additions & 0 deletions include/domain_bridge/domain_bridge_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,10 @@ class DomainBridgeImpl;
class DomainBridgeOptions
{
public:
enum class LocalHostOnly
{
Default, Enabled, Disabled
};
enum class Mode
{
Normal,
Expand Down
8 changes: 5 additions & 3 deletions include/domain_bridge/service_bridge_impl.inc
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ add_service_bridge(
std::shared_ptr<rclcpp::ClientBase> client);

rclcpp::Node::SharedPtr
get_node_for_domain(DomainBridgeImpl & impl, std::size_t domain_id);
get_node_for_domain(DomainBridgeImpl & impl, std::size_t domain_id, domain_bridge::DomainBridgeOptions::LocalHostOnly local_host_only);

const std::string &
get_node_name(const DomainBridgeImpl & impl);
Expand All @@ -67,6 +67,8 @@ DomainBridge::bridge_service(
const std::string & service_name,
size_t from_domain_id,
size_t to_domain_id,
DomainBridgeOptions::LocalHostOnly from_local_host_only,
DomainBridgeOptions::LocalHostOnly to_local_host_only,
const ServiceBridgeOptions & options)
{
const auto & node_name = detail::get_node_name(*impl_);
Expand Down Expand Up @@ -97,8 +99,8 @@ DomainBridge::bridge_service(
return;
}

rclcpp::Node::SharedPtr from_domain_node = detail::get_node_for_domain(*impl_, from_domain_id);
rclcpp::Node::SharedPtr to_domain_node = detail::get_node_for_domain(*impl_, to_domain_id);
rclcpp::Node::SharedPtr from_domain_node = detail::get_node_for_domain(*impl_, from_domain_id, from_local_host_only);
rclcpp::Node::SharedPtr to_domain_node = detail::get_node_for_domain(*impl_, to_domain_id, to_local_host_only);

// Create a client for the 'from_domain'
auto client = from_domain_node->create_client<ServiceT>(
Expand Down
20 changes: 20 additions & 0 deletions include/domain_bridge/topic_bridge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,14 @@ struct TopicBridge
/// Domain ID that the publisher uses
std::size_t to_domain_id;

// local host mode for subscriber
DomainBridgeOptions::LocalHostOnly from_local_host_only =
DomainBridgeOptions::LocalHostOnly::Default;

// local host mode for publisher
DomainBridgeOptions::LocalHostOnly to_local_host_only =
DomainBridgeOptions::LocalHostOnly::Default;

/// Less-than operator.
/**
* Sort by 'from_domain_id',
Expand All @@ -57,6 +65,18 @@ struct TopicBridge
if (to_domain_id > other.to_domain_id) {
return false;
}
if (from_local_host_only < other.from_local_host_only) {
return true;
}
if (from_local_host_only > other.from_local_host_only) {
return false;
}
if (to_local_host_only < other.to_local_host_only) {
return true;
}
if (to_local_host_only > other.to_local_host_only) {
return false;
}
int name_compare = topic_name.compare(other.topic_name);
if (name_compare < 0) {
return true;
Expand Down
68 changes: 56 additions & 12 deletions src/domain_bridge/domain_bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,18 @@

#include "wait_for_graph_events.hpp"


template<>
struct std::hash<std::pair<std::size_t, domain_bridge::DomainBridgeOptions::LocalHostOnly>>
{
size_t operator()(
const std::pair<std::size_t, domain_bridge::DomainBridgeOptions::LocalHostOnly> & key) const
{
return std::hash<std::size_t>{}(key.first) ^ std::hash<int>{}(static_cast<int>(key.second));
}
};


namespace domain_bridge
{

Expand Down Expand Up @@ -99,7 +111,8 @@ class SerializedPublisher
class DomainBridgeImpl
{
public:
using NodeMap = std::unordered_map<std::size_t, std::shared_ptr<rclcpp::Node>>;
using NodeMap = std::unordered_map<
std::pair<std::size_t, DomainBridgeOptions::LocalHostOnly>, std::shared_ptr<rclcpp::Node>>;
using TopicBridgeMap = std::map<
TopicBridge,
std::pair<
Expand Down Expand Up @@ -131,10 +144,27 @@ class DomainBridgeImpl

~DomainBridgeImpl() = default;

rclcpp::Context::SharedPtr create_context_with_domain_id(std::size_t domain_id)
rclcpp::Context::SharedPtr create_context_with_domain_id(
std::size_t domain_id,
DomainBridgeOptions::LocalHostOnly local_host_only)
{
auto context = std::make_shared<rclcpp::Context>();
rclcpp::InitOptions options;
// A hack which by-pass the mutex in rclcpp::InitOptions, but should be safe in this context.
rcl_init_options_t * init_options = const_cast<rcl_init_options_t *>(
options.get_rcl_init_options());
switch (local_host_only) {
case DomainBridgeOptions::LocalHostOnly::Default:
break;
case DomainBridgeOptions::LocalHostOnly::Disabled:
rcl_init_options_get_rmw_init_options(init_options)->localhost_only =
RMW_LOCALHOST_ONLY_DISABLED;
break;
case DomainBridgeOptions::LocalHostOnly::Enabled:
rcl_init_options_get_rmw_init_options(init_options)->localhost_only =
RMW_LOCALHOST_ONLY_ENABLED;
break;
}
options.auto_initialize_logging(false).set_domain_id(domain_id);
context->init(0, nullptr, options);
return context;
Expand All @@ -149,21 +179,24 @@ class DomainBridgeImpl
.start_parameter_event_publisher(false);
}

rclcpp::Node::SharedPtr get_node_for_domain(std::size_t domain_id)
rclcpp::Node::SharedPtr get_node_for_domain(
std::size_t domain_id,
DomainBridgeOptions::LocalHostOnly local_host_only)
{
auto domain_id_node_pair = node_map_.find(domain_id);
std::pair<std::size_t, DomainBridgeOptions::LocalHostOnly> key{domain_id, local_host_only};
auto domain_id_node_pair = node_map_.find(key);

// If we don't already have a node for the domain, create one
if (node_map_.end() == domain_id_node_pair) {
if (options_.on_new_domain_callback_) {
options_.on_new_domain_callback_(domain_id);
}
auto context = create_context_with_domain_id(domain_id);
auto context = create_context_with_domain_id(domain_id, local_host_only);
auto node_options = create_node_options(context);
std::ostringstream oss;
oss << options_.name() << "_" << std::to_string(domain_id);
auto node = std::make_shared<rclcpp::Node>(oss.str(), node_options);
node_map_[domain_id] = node;
node_map_[key] = node;
return node;
}

Expand Down Expand Up @@ -317,13 +350,16 @@ class DomainBridgeImpl
const std::string & type = topic_bridge.type_name;
std::size_t from_domain_id = topic_bridge.from_domain_id;
std::size_t to_domain_id = topic_bridge.to_domain_id;
DomainBridgeOptions::LocalHostOnly from_local_host_only = topic_bridge.from_local_host_only;
DomainBridgeOptions::LocalHostOnly to_local_host_only = topic_bridge.to_local_host_only;

if (topic_options.reversed()) {
std::swap(to_domain_id, from_domain_id);
std::swap(to_local_host_only, from_local_host_only);
}

// Ensure 'to' domain and 'from' domain are not equal
if (to_domain_id == from_domain_id) {
if (to_domain_id == from_domain_id && from_local_host_only == to_local_host_only) {
std::cerr << "Cannot bridge topic '" << topic << "' from domain " <<
std::to_string(from_domain_id) << " to domain " << std::to_string(to_domain_id) <<
". Domain IDs must be different." << std::endl;
Expand All @@ -348,8 +384,10 @@ class DomainBridgeImpl
bridged_topics_[topic_bridge] = {nullptr, nullptr};
}

rclcpp::Node::SharedPtr from_domain_node = get_node_for_domain(from_domain_id);
rclcpp::Node::SharedPtr to_domain_node = get_node_for_domain(to_domain_id);
rclcpp::Node::SharedPtr from_domain_node = get_node_for_domain(
from_domain_id,
from_local_host_only);
rclcpp::Node::SharedPtr to_domain_node = get_node_for_domain(to_domain_id, to_local_host_only);

auto create_bridge =
[this, topic, topic_remapped, topic_bridge, topic_options, from_domain_node, to_domain_node]
Expand Down Expand Up @@ -552,9 +590,11 @@ class DomainBridgeImpl
namespace detail
{
rclcpp::Node::SharedPtr
get_node_for_domain(DomainBridgeImpl & impl, std::size_t domain_id)
get_node_for_domain(
DomainBridgeImpl & impl, std::size_t domain_id,
DomainBridgeOptions::LocalHostOnly localhost_only)
{
return impl.get_node_for_domain(domain_id);
return impl.get_node_for_domain(domain_id, localhost_only);
}

bool
Expand Down Expand Up @@ -623,9 +663,13 @@ void DomainBridge::bridge_topic(
const std::string & type,
std::size_t from_domain_id,
std::size_t to_domain_id,
DomainBridgeOptions::LocalHostOnly from_local_host_only,
DomainBridgeOptions::LocalHostOnly to_local_host_only,
const TopicBridgeOptions & options)
{
impl_->bridge_topic({topic, type, from_domain_id, to_domain_id}, options);
impl_->bridge_topic(
{topic, type, from_domain_id, to_domain_id, from_local_host_only,
to_local_host_only}, options);
}

void DomainBridge::bridge_topic(
Expand Down
47 changes: 46 additions & 1 deletion src/domain_bridge/parse_domain_bridge_yaml_config.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,21 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi
return options;
}

static DomainBridgeOptions::LocalHostOnly string_to_local_host_only(
std::filesystem::path file_path,
const std::string & local_host_only)
{
if (local_host_only == "default") {
return DomainBridgeOptions::LocalHostOnly::Default;
} else if (local_host_only == "enabled") {
return DomainBridgeOptions::LocalHostOnly::Enabled;
} else if (local_host_only == "disabled") {
return DomainBridgeOptions::LocalHostOnly::Disabled;
} else {
throw YamlParsingError(file_path, "Invalid local host only option '" + local_host_only + "'");
}
}

DomainBridgeConfig parse_domain_bridge_yaml_config(std::filesystem::path file_path)
{
DomainBridgeConfig domain_bridge_config;
Expand Down Expand Up @@ -192,6 +207,21 @@ update_domain_bridge_config_from_yaml(
default_to_domain = config["to_domain"].as<std::size_t>();
is_default_to_domain = true;
}
// Check for any default localhost mode
DomainBridgeOptions::LocalHostOnly default_from_local_host_only =
domain_bridge::DomainBridgeOptions::LocalHostOnly::Default;
DomainBridgeOptions::LocalHostOnly default_to_local_host_only =
domain_bridge::DomainBridgeOptions::LocalHostOnly::Default;
if (config["from_local_host_only"]) {
default_from_local_host_only = string_to_local_host_only(
file_path,
config["from_local_host_only"].as<std::string>());
}
if (config["to_local_host_only"]) {
default_to_local_host_only = string_to_local_host_only(
file_path,
config["to_local_host_only"].as<std::string>());
}
if (config["mode"]) {
try {
auto mode_str = config["mode"].as<std::string>();
Expand Down Expand Up @@ -243,6 +273,19 @@ update_domain_bridge_config_from_yaml(
}
}

DomainBridgeOptions::LocalHostOnly from_local_host_only = default_from_local_host_only;
if (config["from_local_host_only"]) {
from_local_host_only = string_to_local_host_only(
file_path,
config["from_local_host_only"].as<std::string>());
}
DomainBridgeOptions::LocalHostOnly to_local_host_only = default_to_local_host_only;
if (config["to_local_host_only"]) {
to_local_host_only = string_to_local_host_only(
file_path,
config["to_local_host_only"].as<std::string>());
}

// Parse topic bridge options
TopicBridgeOptions options;
if (topic_info["remap"]) {
Expand All @@ -259,7 +302,9 @@ update_domain_bridge_config_from_yaml(
}

// Add topic bridge to config
domain_bridge_config.topics.push_back({{topic, type, from_domain_id, to_domain_id}, options});
domain_bridge_config.topics.push_back(
{{topic, type, from_domain_id, to_domain_id,
from_local_host_only, to_local_host_only}, options});
}
}
}
Expand Down
Loading