Skip to content

Commit

Permalink
Add namespace config (#9)
Browse files Browse the repository at this point in the history
  • Loading branch information
@JEnoch
JEnoch authored Oct 12, 2023
1 parent 463f36a commit 2d380bb
Show file tree
Hide file tree
Showing 4 changed files with 83 additions and 35 deletions.
16 changes: 10 additions & 6 deletions DEFAULT_CONFIG.json5
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,18 +17,22 @@
////
// id: "robot-1",

////
//// namespace: A ROS namespace to be used by this bridge.
//// Default: "/"
////
// namespace: "/",

////
//// nodename: A ROS node name to be used by this bridge.
//// Default: "zenoh_bridge_ros2dds"
////
// nodename: "zenoh_bridge_ros2dds",

////
//// namespace: A ROS namespace which:
//// - is used for the "zenoh_bridge_ros2dds" node itself
//// - is added to all discovered interfaces when routed to Zenoh
//// (i.e. a "cmd_vel" topic in the robot will be seen as "namespace/cmd_vel" outside the robot)
//// Note that this also applies to topics with absolute path such as "/rosout", "/tf" and "/tf_static".
//// Default: "/"
////
// namespace: "/",

////
//// domain: The DDS Domain ID. By default set to 0, or to "$ROS_DOMAIN_ID" is this environment variable is defined.
////
Expand Down
32 changes: 30 additions & 2 deletions zenoh-plugin-ros2dds/src/ros2_utils.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,9 @@ use cyclors::{
DDS_INFINITE_TIME,
},
};
use zenoh::prelude::{keyexpr, KeyExpr};
use zenoh::prelude::{keyexpr, KeyExpr, OwnedKeyExpr};

use crate::{dds_utils::get_guid, ke_for_sure};
use crate::{config::Config, dds_utils::get_guid, ke_for_sure};

pub const ROS2_ACTION_CANCEL_GOAL_SRV_TYPE: &str = "action_msgs/srv/CancelGoal";
pub const ROS2_ACTION_STATUS_MSG_TYPE: &str = "action_msgs/msg/GoalStatusArray";
Expand All @@ -41,6 +41,34 @@ lazy_static::lazy_static!(
pub static ref QOS_DEFAULT_ACTION_STATUS: Qos = ros2_action_status_default_qos();
);

/// Convert ROS2 interface name to a Zenoh key expression,
/// prefixing with "namespace" if configured
pub fn ros2_name_to_key_expr(ros2_name: &str, config: &Config) -> OwnedKeyExpr {
// ros2_name as discovered by the bridge starts with a '/'
// config.namespace starts with a '/'
// But a Zenoh key_expr shall not start with a '/'
if config.namespace == "/" {
ke_for_sure!(&ros2_name[1..]).to_owned()
} else {
ke_for_sure!(&config.namespace[1..]) / ke_for_sure!(&ros2_name[1..])
}
}

/// Convert a Zenoh key expression to a ROS2 full interface name,
/// removing "namespace" prefix if configured and present in the key expr
pub fn key_expr_to_ros2_name(key_expr: &keyexpr, config: &Config) -> String {
// Zenoh key_expr never starts with a '/'
// But the full ROS2 name that is returned shall (full == with a namespace, even if just '/')
if config.namespace == "/" {
format!("/{key_expr}")
} else {
match key_expr.as_str().strip_prefix(&config.namespace[1..]) {
Some(s) => s.to_string(),
None => format!("/{key_expr}"),
}
}
}

/// Convert DDS Topic type to ROS2 Message type
pub fn dds_type_to_ros2_message_type(dds_topic: &str) -> String {
let result = dds_topic.replace("::dds_::", "::").replace("::", "/");
Expand Down
2 changes: 1 addition & 1 deletion zenoh-plugin-ros2dds/src/ros_discovery.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -398,7 +398,7 @@ impl std::fmt::Debug for NodeEntitiesInfo {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
writeln!(
f,
"Node {}/{} :",
"Node namespace={} / name={} :",
if &self.node_namespace == "/" {
""
} else {
Expand Down
68 changes: 42 additions & 26 deletions zenoh-plugin-ros2dds/src/routes_mgr.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@ use crate::events::ROS2AnnouncementEvent;
use crate::events::ROS2DiscoveryEvent;
use crate::qos_helpers::adapt_reader_qos_for_writer;
use crate::qos_helpers::adapt_writer_qos_for_reader;
use crate::ros2_utils::key_expr_to_ros2_name;
use crate::ros2_utils::ros2_name_to_key_expr;
use crate::ros_discovery::RosDiscoveryInfoMgr;
use crate::route_action_cli::RouteActionCli;
use crate::route_action_srv::RouteActionSrv;
Expand Down Expand Up @@ -315,7 +317,7 @@ impl<'a> RoutesMgr<'a> {
// with an associated DDS Writer allowing local ROS2 Nodes to discover it
let route = self
.get_or_create_route_subscriber(
format!("/{zenoh_key_expr}"),
key_expr_to_ros2_name(&zenoh_key_expr, &self.context.config),
ros2_type,
keyless,
writer_qos,
Expand Down Expand Up @@ -354,7 +356,7 @@ impl<'a> RoutesMgr<'a> {
// with an associated DDS Reader allowing local ROS2 Nodes to discover it
let route = self
.get_or_create_route_publisher(
format!("/{zenoh_key_expr}"),
key_expr_to_ros2_name(&zenoh_key_expr, &self.context.config),
ros2_type,
keyless,
reader_qos,
Expand Down Expand Up @@ -390,7 +392,11 @@ impl<'a> RoutesMgr<'a> {
// On remote Service Server route announcement, prepare a Service Client route
// with a associated DDS Reader/Writer allowing local ROS2 Nodes to discover it
let route = self
.get_or_create_route_service_cli(format!("/{zenoh_key_expr}"), ros2_type, true)
.get_or_create_route_service_cli(
key_expr_to_ros2_name(&zenoh_key_expr, &self.context.config),
ros2_type,
true,
)
.await?;
route.add_remote_route(&plugin_id, &zenoh_key_expr);
}
Expand Down Expand Up @@ -421,7 +427,11 @@ impl<'a> RoutesMgr<'a> {
// On remote Service Client route announcement, prepare a Service Server route
// with a associated DDS Reader/Writer allowing local ROS2 Nodes to discover it
let route = self
.get_or_create_route_service_srv(format!("/{zenoh_key_expr}"), ros2_type, true)
.get_or_create_route_service_srv(
key_expr_to_ros2_name(&zenoh_key_expr, &self.context.config),
ros2_type,
true,
)
.await?;
route.add_remote_route(&plugin_id, &zenoh_key_expr);
}
Expand Down Expand Up @@ -452,7 +462,10 @@ impl<'a> RoutesMgr<'a> {
// On remote Action Server route announcement, prepare a Action Client route
// with a associated DDS Reader/Writer allowing local ROS2 Nodes to discover it
let route = self
.get_or_create_route_action_cli(format!("/{zenoh_key_expr}"), ros2_type)
.get_or_create_route_action_cli(
key_expr_to_ros2_name(&zenoh_key_expr, &self.context.config),
ros2_type,
)
.await?;
route.add_remote_route(&plugin_id, &zenoh_key_expr);
}
Expand Down Expand Up @@ -483,7 +496,10 @@ impl<'a> RoutesMgr<'a> {
// On remote Action Client route announcement, prepare a Action Server route
// with a associated DDS Reader/Writer allowing local ROS2 Nodes to discover it
let route = self
.get_or_create_route_action_srv(format!("/{zenoh_key_expr}"), ros2_type)
.get_or_create_route_action_srv(
key_expr_to_ros2_name(&zenoh_key_expr, &self.context.config),
ros2_type,
)
.await?;
route.add_remote_route(&plugin_id, &zenoh_key_expr);
}
Expand Down Expand Up @@ -527,13 +543,13 @@ impl<'a> RoutesMgr<'a> {
) -> Result<&mut RoutePublisher<'a>, String> {
match self.routes_publishers.entry(ros2_name.clone()) {
Entry::Vacant(entry) => {
// ROS2 topic name => Zenoh key expr : strip '/' prefix
let zenoh_key_expr = ke_for_sure!(&ros2_name[1..]);
// ROS2 topic name => Zenoh key expr
let zenoh_key_expr = ros2_name_to_key_expr(&ros2_name, &self.context.config);
// create route
let route = RoutePublisher::create(
ros2_name.clone(),
ros2_type,
zenoh_key_expr.to_owned(),
zenoh_key_expr.clone(),
&None,
keyless,
reader_qos,
Expand All @@ -544,7 +560,7 @@ impl<'a> RoutesMgr<'a> {

if admin_space_ref {
// insert reference in admin_space
let admin_ke = *KE_PREFIX_ROUTE_PUBLISHER / zenoh_key_expr;
let admin_ke = *KE_PREFIX_ROUTE_PUBLISHER / &zenoh_key_expr;
self.admin_space
.insert(admin_ke, RouteRef::Publisher(ros2_name));
}
Expand All @@ -565,13 +581,13 @@ impl<'a> RoutesMgr<'a> {
) -> Result<&mut RouteSubscriber<'a>, String> {
match self.routes_subscribers.entry(ros2_name.clone()) {
Entry::Vacant(entry) => {
// ROS2 topic name => Zenoh key expr : strip '/' prefix
let zenoh_key_expr = ke_for_sure!(&ros2_name[1..]);
// ROS2 topic name => Zenoh key expr
let zenoh_key_expr = ros2_name_to_key_expr(&ros2_name, &self.context.config);
// create route
let route = RouteSubscriber::create(
ros2_name.clone(),
ros2_type,
zenoh_key_expr.to_owned(),
zenoh_key_expr.clone(),
keyless,
writer_qos,
self.context.clone(),
Expand All @@ -581,7 +597,7 @@ impl<'a> RoutesMgr<'a> {

if admin_space_ref {
// insert reference in admin_space
let admin_ke = *KE_PREFIX_ROUTE_SUBSCRIBER / zenoh_key_expr;
let admin_ke = *KE_PREFIX_ROUTE_SUBSCRIBER / &zenoh_key_expr;
self.admin_space
.insert(admin_ke, RouteRef::Subscriber(ros2_name));
}
Expand All @@ -600,13 +616,13 @@ impl<'a> RoutesMgr<'a> {
) -> Result<&mut RouteServiceSrv<'a>, String> {
match self.routes_service_srv.entry(ros2_name.clone()) {
Entry::Vacant(entry) => {
// ROS2 topic name => Zenoh key expr : strip '/' prefix
let zenoh_key_expr = ke_for_sure!(&ros2_name[1..]);
// ROS2 topic name => Zenoh key expr
let zenoh_key_expr = ros2_name_to_key_expr(&ros2_name, &self.context.config);
// create route
let route = RouteServiceSrv::create(
ros2_name.clone(),
ros2_type,
zenoh_key_expr.to_owned(),
zenoh_key_expr.clone(),
&None,
self.context.clone(),
)
Expand All @@ -615,7 +631,7 @@ impl<'a> RoutesMgr<'a> {

if admin_space_ref {
// insert reference in admin_space
let admin_ke = *KE_PREFIX_ROUTE_SERVICE_SRV / zenoh_key_expr;
let admin_ke = *KE_PREFIX_ROUTE_SERVICE_SRV / &zenoh_key_expr;
self.admin_space
.insert(admin_ke, RouteRef::ServiceSrv(ros2_name));
}
Expand All @@ -635,12 +651,12 @@ impl<'a> RoutesMgr<'a> {
match self.routes_service_cli.entry(ros2_name.clone()) {
Entry::Vacant(entry) => {
// ROS2 topic name => Zenoh key expr : strip '/' prefix
let zenoh_key_expr = ke_for_sure!(&ros2_name[1..]);
let zenoh_key_expr = ros2_name_to_key_expr(&ros2_name, &self.context.config);
// create route
let route = RouteServiceCli::create(
ros2_name.clone(),
ros2_type,
zenoh_key_expr.to_owned(),
zenoh_key_expr.clone(),
&None,
self.context.clone(),
)
Expand All @@ -649,7 +665,7 @@ impl<'a> RoutesMgr<'a> {

if admin_space_ref {
// insert reference in admin_space
let admin_ke = *KE_PREFIX_ROUTE_SERVICE_CLI / zenoh_key_expr;
let admin_ke = *KE_PREFIX_ROUTE_SERVICE_CLI / &zenoh_key_expr;
self.admin_space
.insert(admin_ke, RouteRef::ServiceCli(ros2_name));
}
Expand All @@ -668,19 +684,19 @@ impl<'a> RoutesMgr<'a> {
match self.routes_action_srv.entry(ros2_name.clone()) {
Entry::Vacant(entry) => {
// ROS2 topic name => Zenoh key expr : strip '/' prefix
let zenoh_key_expr = ke_for_sure!(&ros2_name[1..]);
let zenoh_key_expr = ros2_name_to_key_expr(&ros2_name, &self.context.config);
// create route
let route = RouteActionSrv::create(
ros2_name.clone(),
ros2_type,
zenoh_key_expr.to_owned(),
zenoh_key_expr.clone(),
self.context.clone(),
)
.await?;
log::info!("{route} created");

// insert reference in admin_space
let admin_ke = *KE_PREFIX_ROUTE_ACTION_SRV / zenoh_key_expr;
let admin_ke = *KE_PREFIX_ROUTE_ACTION_SRV / &zenoh_key_expr;
self.admin_space
.insert(admin_ke, RouteRef::ActionSrv(ros2_name));

Expand All @@ -698,7 +714,7 @@ impl<'a> RoutesMgr<'a> {
match self.routes_action_cli.entry(ros2_name.clone()) {
Entry::Vacant(entry) => {
// ROS2 topic name => Zenoh key expr : strip '/' prefix
let zenoh_key_expr = ke_for_sure!(&ros2_name[1..]);
let zenoh_key_expr = ros2_name_to_key_expr(&ros2_name, &self.context.config);
// create route
let route = RouteActionCli::create(
ros2_name.clone(),
Expand All @@ -710,7 +726,7 @@ impl<'a> RoutesMgr<'a> {
log::info!("{route} created");

// insert reference in admin_space
let admin_ke = *KE_PREFIX_ROUTE_ACTION_CLI / zenoh_key_expr;
let admin_ke = *KE_PREFIX_ROUTE_ACTION_CLI / &zenoh_key_expr;
self.admin_space
.insert(admin_ke, RouteRef::ActionCli(ros2_name));

Expand Down

0 comments on commit 2d380bb

Please sign in to comment.