From 4810efb63547e5f36c608b46985eb50fc282e3bc Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Fri, 29 Nov 2024 17:44:57 +0100 Subject: [PATCH 1/7] Add USER_DATA to ros_discovery_info Reader/Writer --- zenoh-plugin-ros2dds/src/ros_discovery.rs | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/zenoh-plugin-ros2dds/src/ros_discovery.rs b/zenoh-plugin-ros2dds/src/ros_discovery.rs index 170a538..38c708a 100644 --- a/zenoh-plugin-ros2dds/src/ros_discovery.rs +++ b/zenoh-plugin-ros2dds/src/ros_discovery.rs @@ -34,16 +34,22 @@ use zenoh::{ // Contributors: // ZettaScale Zenoh Team, // -use crate::dds_utils::{ddsrt_iov_len_from_usize, delete_dds_entity, get_guid}; use crate::{ dds_types::DDSRawSample, gid::Gid, ros2_utils::{ros_distro_is_less_than, ROS_DISTRO}, ChannelEvent, ROS_DISCOVERY_INFO_PUSH_INTERVAL_MS, }; +use crate::{ + dds_utils::{ddsrt_iov_len_from_usize, delete_dds_entity, get_guid}, + ros2_utils::{USER_DATA_KEYHASH_KEY, USER_DATA_PROPS_SEPARATOR}, +}; pub const ROS_DISCOVERY_INFO_TOPIC_NAME: &str = "ros_discovery_info"; const ROS_DISCOVERY_INFO_TOPIC_TYPE: &str = "rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_"; +// Type hash required since Iron in Reader/Writer USER_DATA QoS +const ROS_DISCOVERY_INFO_TYPE_HASH: &str = + "RIHS01_91a0593bacdcc50ea9bdcf849a938b128412cc1ea821245c663bcd26f83c295e"; pub struct RosDiscoveryInfoMgr { reader: dds_entity_t, @@ -87,6 +93,16 @@ impl RosDiscoveryInfoMgr { .unwrap() .into_raw(); + // Since Iron, the Reader/Writer on `ros_discovery_info` topic are expected to have the type hash in USER_DATA QoS + let user_data_qos: Option> = if ros_distro_is_less_than("iron") { + None + } else { + let mut s = USER_DATA_KEYHASH_KEY.to_string(); + s.push_str(ROS_DISCOVERY_INFO_TYPE_HASH); + s.push(USER_DATA_PROPS_SEPARATOR); + Some(s.into_bytes()) + }; + unsafe { // Create topic (for reader/writer creation) let t = cdds_create_blob_topic(participant, cton, ctyn, true); @@ -108,6 +124,7 @@ impl RosDiscoveryInfoMgr { qos.ignore_local = Some(IgnoreLocal { kind: IgnoreLocalKind::PARTICIPANT, }); + qos.user_data = user_data_qos.clone(); let qos_native = qos.to_qos_native(); let reader = dds_create_reader(participant, t, qos_native, std::ptr::null()); Qos::delete_qos_native(qos_native); @@ -137,6 +154,7 @@ impl RosDiscoveryInfoMgr { qos.ignore_local = Some(IgnoreLocal { kind: IgnoreLocalKind::PARTICIPANT, }); + qos.user_data = user_data_qos.clone(); let qos_native = qos.to_qos_native(); let writer = dds_create_writer(participant, t, qos_native, std::ptr::null()); Qos::delete_qos_native(qos_native); From f3d184cc6d914ad16a76fea1a66fbb3cf2d6192c Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Fri, 29 Nov 2024 17:46:10 +0100 Subject: [PATCH 2/7] Add USER_DATA to pub/sub liveliness tokens --- zenoh-plugin-ros2dds/src/liveliness_mgt.rs | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/zenoh-plugin-ros2dds/src/liveliness_mgt.rs b/zenoh-plugin-ros2dds/src/liveliness_mgt.rs index 35e9d70..554fae4 100644 --- a/zenoh-plugin-ros2dds/src/liveliness_mgt.rs +++ b/zenoh-plugin-ros2dds/src/liveliness_mgt.rs @@ -21,6 +21,8 @@ use zenoh::key_expr::{ keyexpr, OwnedKeyExpr, }; +use crate::ros2_utils::ros_distro_is_less_than; + const SLASH_REPLACEMSNT_CHAR: &str = "ยง"; kedefine!( @@ -195,7 +197,7 @@ fn unescape_slashes(ke: &keyexpr) -> OwnedKeyExpr { // NOTE: only significant Qos for ROS2 are serialized // See https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Quality-of-Service-Settings.html // -// format: ":::," +// format: ":::,[:]" // where each element is "" if default QoS, or an integer in case of enum, and 'K' for !keyless pub fn qos_to_key_expr(keyless: bool, qos: &Qos) -> OwnedKeyExpr { use std::io::Write; @@ -217,6 +219,14 @@ pub fn qos_to_key_expr(keyless: bool, qos: &Qos) -> OwnedKeyExpr { write!(&mut w, "{},{}", *kind as isize, depth).unwrap(); } + // Since Iron USER_DATA QoS contains the type_hash and must be forwarded to remote bridge for Reader/Writer creation + if !ros_distro_is_less_than("iron") { + write!(w, ":").unwrap(); + if let Some(v) = &qos.user_data { + write!(&mut w, "{}", String::from_utf8_lossy(v)).unwrap(); + } + } + unsafe { let s: String = String::from_utf8_unchecked(w); OwnedKeyExpr::from_string_unchecked(s) @@ -225,8 +235,8 @@ pub fn qos_to_key_expr(keyless: bool, qos: &Qos) -> OwnedKeyExpr { fn key_expr_to_qos(ke: &keyexpr) -> Result<(bool, Qos), String> { let elts: Vec<&str> = ke.split(':').collect(); - if elts.len() != 4 { - return Err(format!("Internal Error: unexpected QoS expression: '{ke}' - 4 elements between : were expected")); + if elts.len() < 4 { + return Err(format!("Internal Error: unexpected QoS expression: '{ke}' - at least 4 elements between ':' were expected")); } let mut qos = Qos::default(); let keyless = elts[0].is_empty(); @@ -253,6 +263,10 @@ fn key_expr_to_qos(ke: &keyexpr) -> Result<(bool, Qos), String> { _ => return Err(format!("Internal Error: unexpected QoS expression: '{ke}' - failed to parse History in 4th element")), } } + // The USER_DATA might be present as 5th element + if elts.len() > 4 && !elts[4].is_empty() { + qos.user_data = Some(elts[4].into()); + } Ok((keyless, qos)) } From 481c9b5136904645a42b8a409a99b85029715252 Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Fri, 29 Nov 2024 17:48:01 +0100 Subject: [PATCH 3/7] Add type_hash utils --- zenoh-plugin-ros2dds/src/ros2_utils.rs | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/zenoh-plugin-ros2dds/src/ros2_utils.rs b/zenoh-plugin-ros2dds/src/ros2_utils.rs index 6995f51..c139adf 100644 --- a/zenoh-plugin-ros2dds/src/ros2_utils.rs +++ b/zenoh-plugin-ros2dds/src/ros2_utils.rs @@ -14,6 +14,7 @@ use std::{ env::VarError, + str, sync::atomic::{AtomicU32, Ordering}, }; @@ -40,6 +41,11 @@ pub const ROS2_ACTION_STATUS_MSG_TYPE: &str = "action_msgs/msg/GoalStatusArray"; // ROS_DISTRO value assumed if the environment variable is not set pub const ASSUMED_ROS_DISTRO: &str = "iron"; +// Separator used by ROS 2 in USER_DATA QoS +pub const USER_DATA_PROPS_SEPARATOR: char = ';'; +// Key for type hash used by ROS 2 in USER_DATA QoS +pub const USER_DATA_KEYHASH_KEY: &str = "typehash="; + lazy_static::lazy_static!( pub static ref ROS_DISTRO: String = get_ros_distro(); @@ -393,6 +399,20 @@ pub fn check_ros_name(name: &str) -> Result<(), String> { } } +pub fn extract_type_hash(qos: &Qos) -> Option { + if let Some(v) = &qos.user_data { + if let Ok(s) = str::from_utf8(v) { + if let Some(mut start) = s.find(USER_DATA_KEYHASH_KEY) { + start += USER_DATA_KEYHASH_KEY.len(); + if let Some(end) = s[start..].find(USER_DATA_PROPS_SEPARATOR) { + return Some(s[start..(start + end)].into()); + } + } + } + } + None +} + lazy_static::lazy_static!( pub static ref CLIENT_ID_COUNTER: AtomicU32 = AtomicU32::default(); ); From cbaec52a36d150bb6aef8d4c056728ca00044e1c Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Fri, 29 Nov 2024 18:30:41 +0100 Subject: [PATCH 4/7] Liveliness token: don't add last ':' is USER_DATA is not set --- zenoh-plugin-ros2dds/src/liveliness_mgt.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/zenoh-plugin-ros2dds/src/liveliness_mgt.rs b/zenoh-plugin-ros2dds/src/liveliness_mgt.rs index 554fae4..9ad8f1c 100644 --- a/zenoh-plugin-ros2dds/src/liveliness_mgt.rs +++ b/zenoh-plugin-ros2dds/src/liveliness_mgt.rs @@ -221,9 +221,8 @@ pub fn qos_to_key_expr(keyless: bool, qos: &Qos) -> OwnedKeyExpr { // Since Iron USER_DATA QoS contains the type_hash and must be forwarded to remote bridge for Reader/Writer creation if !ros_distro_is_less_than("iron") { - write!(w, ":").unwrap(); if let Some(v) = &qos.user_data { - write!(&mut w, "{}", String::from_utf8_lossy(v)).unwrap(); + write!(&mut w, ":{}", String::from_utf8_lossy(v)).unwrap(); } } From 0c63b2c57106768282533ebe59eccabb52c6d12f Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Mon, 2 Dec 2024 17:06:39 +0100 Subject: [PATCH 5/7] Add USER_DATA to Actions feedback and status Readers/Writers --- zenoh-plugin-ros2dds/src/ros2_utils.rs | 36 +++++++++++++++++++---- zenoh-plugin-ros2dds/src/ros_discovery.rs | 6 ++-- 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/zenoh-plugin-ros2dds/src/ros2_utils.rs b/zenoh-plugin-ros2dds/src/ros2_utils.rs index c139adf..e1c64cc 100644 --- a/zenoh-plugin-ros2dds/src/ros2_utils.rs +++ b/zenoh-plugin-ros2dds/src/ros2_utils.rs @@ -37,6 +37,9 @@ use crate::{config::Config, dds_utils::get_guid}; 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"; +// Type hash for action_msgs/msg/GoalStatusArray in Iron and Jazzy (might change in future versions) +pub const ROS2_ACTION_STATUS_MSG_TYPE_HASH: &str = + "RIHS01_91a0593bacdcc50ea9bdcf849a938b128412cc1ea821245c663bcd26f83c295e"; // ROS_DISTRO value assumed if the environment variable is not set pub const ASSUMED_ROS_DISTRO: &str = "iron"; @@ -44,7 +47,7 @@ pub const ASSUMED_ROS_DISTRO: &str = "iron"; // Separator used by ROS 2 in USER_DATA QoS pub const USER_DATA_PROPS_SEPARATOR: char = ';'; // Key for type hash used by ROS 2 in USER_DATA QoS -pub const USER_DATA_KEYHASH_KEY: &str = "typehash="; +pub const USER_DATA_TYPEHASH_KEY: &str = "typehash="; lazy_static::lazy_static!( pub static ref ROS_DISTRO: String = get_ros_distro(); @@ -318,7 +321,7 @@ fn ros2_service_default_qos() -> Qos { } fn ros2_action_feedback_default_qos() -> Qos { - Qos { + let mut qos = Qos { history: Some(History { kind: HistoryKind::KEEP_LAST, depth: 10, @@ -343,13 +346,21 @@ fn ros2_action_feedback_default_qos() -> Qos { kind: IgnoreLocalKind::PARTICIPANT, }), ..Default::default() + }; + if !ros_distro_is_less_than("iron") { + // NOTE: the type hash should be a real one instead of this invalid type hash. + // However, `rmw_cyclonedds_cpp` doesn't do any type checking (yet). + // And the way to forward the type hash for actions (and services) raise questions + // that are described in https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/pull/349 + insert_type_hash(&mut qos, "RIHS01_0000000000000000000000000000000000000000000000000000000000000000"); } + qos } fn ros2_action_status_default_qos() -> Qos { // Default Status topic QoS copied from: // https://github.com/ros2/rcl/blob/8f7f4f0804a34ee9d9ecd2d7e75a57ce2b7ced5d/rcl_action/include/rcl_action/default_qos.h#L30 - Qos { + let mut qos = Qos { durability: Some(Durability { kind: DurabilityKind::TRANSIENT_LOCAL, }), @@ -373,7 +384,12 @@ fn ros2_action_status_default_qos() -> Qos { kind: IgnoreLocalKind::PARTICIPANT, }), ..Default::default() + }; + if !ros_distro_is_less_than("iron") { + // add type_hash in USER_DATA QoS + insert_type_hash(&mut qos, ROS2_ACTION_STATUS_MSG_TYPE_HASH); } + qos } pub fn is_service_for_action(ros2_service_name: &str) -> bool { @@ -402,8 +418,8 @@ pub fn check_ros_name(name: &str) -> Result<(), String> { pub fn extract_type_hash(qos: &Qos) -> Option { if let Some(v) = &qos.user_data { if let Ok(s) = str::from_utf8(v) { - if let Some(mut start) = s.find(USER_DATA_KEYHASH_KEY) { - start += USER_DATA_KEYHASH_KEY.len(); + if let Some(mut start) = s.find(USER_DATA_TYPEHASH_KEY) { + start += USER_DATA_TYPEHASH_KEY.len(); if let Some(end) = s[start..].find(USER_DATA_PROPS_SEPARATOR) { return Some(s[start..(start + end)].into()); } @@ -413,6 +429,16 @@ pub fn extract_type_hash(qos: &Qos) -> Option { None } +pub fn insert_type_hash(qos: &mut Qos, type_hash: &str) { + let mut s = USER_DATA_TYPEHASH_KEY.to_string(); + s.push_str(type_hash); + s.push(USER_DATA_PROPS_SEPARATOR); + match qos.user_data { + Some(ref mut v) => v.extend(s.into_bytes().iter()), + None => qos.user_data = Some(s.into_bytes()), + } +} + lazy_static::lazy_static!( pub static ref CLIENT_ID_COUNTER: AtomicU32 = AtomicU32::default(); ); diff --git a/zenoh-plugin-ros2dds/src/ros_discovery.rs b/zenoh-plugin-ros2dds/src/ros_discovery.rs index 38c708a..0aec45b 100644 --- a/zenoh-plugin-ros2dds/src/ros_discovery.rs +++ b/zenoh-plugin-ros2dds/src/ros_discovery.rs @@ -42,12 +42,12 @@ use crate::{ }; use crate::{ dds_utils::{ddsrt_iov_len_from_usize, delete_dds_entity, get_guid}, - ros2_utils::{USER_DATA_KEYHASH_KEY, USER_DATA_PROPS_SEPARATOR}, + ros2_utils::{USER_DATA_PROPS_SEPARATOR, USER_DATA_TYPEHASH_KEY}, }; pub const ROS_DISCOVERY_INFO_TOPIC_NAME: &str = "ros_discovery_info"; const ROS_DISCOVERY_INFO_TOPIC_TYPE: &str = "rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_"; -// Type hash required since Iron in Reader/Writer USER_DATA QoS +// Type hash for rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ in Iron and Jazzy (might change in future versions) const ROS_DISCOVERY_INFO_TYPE_HASH: &str = "RIHS01_91a0593bacdcc50ea9bdcf849a938b128412cc1ea821245c663bcd26f83c295e"; @@ -97,7 +97,7 @@ impl RosDiscoveryInfoMgr { let user_data_qos: Option> = if ros_distro_is_less_than("iron") { None } else { - let mut s = USER_DATA_KEYHASH_KEY.to_string(); + let mut s = USER_DATA_TYPEHASH_KEY.to_string(); s.push_str(ROS_DISCOVERY_INFO_TYPE_HASH); s.push(USER_DATA_PROPS_SEPARATOR); Some(s.into_bytes()) From cab317d94e5e9986c00c41af720dba3bce8dd1a3 Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Mon, 2 Dec 2024 17:08:27 +0100 Subject: [PATCH 6/7] fix format --- zenoh-plugin-ros2dds/src/ros2_utils.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/zenoh-plugin-ros2dds/src/ros2_utils.rs b/zenoh-plugin-ros2dds/src/ros2_utils.rs index e1c64cc..a28d7d6 100644 --- a/zenoh-plugin-ros2dds/src/ros2_utils.rs +++ b/zenoh-plugin-ros2dds/src/ros2_utils.rs @@ -352,7 +352,10 @@ fn ros2_action_feedback_default_qos() -> Qos { // However, `rmw_cyclonedds_cpp` doesn't do any type checking (yet). // And the way to forward the type hash for actions (and services) raise questions // that are described in https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/pull/349 - insert_type_hash(&mut qos, "RIHS01_0000000000000000000000000000000000000000000000000000000000000000"); + insert_type_hash( + &mut qos, + "RIHS01_0000000000000000000000000000000000000000000000000000000000000000", + ); } qos } From 88f3195ec830aaf1a89feffb07e330094eecb92f Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Tue, 3 Dec 2024 14:03:30 +0100 Subject: [PATCH 7/7] Comment unused code (might be uselful later) --- zenoh-plugin-ros2dds/src/ros2_utils.rs | 27 +++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/zenoh-plugin-ros2dds/src/ros2_utils.rs b/zenoh-plugin-ros2dds/src/ros2_utils.rs index a28d7d6..e33f8b6 100644 --- a/zenoh-plugin-ros2dds/src/ros2_utils.rs +++ b/zenoh-plugin-ros2dds/src/ros2_utils.rs @@ -418,19 +418,20 @@ pub fn check_ros_name(name: &str) -> Result<(), String> { } } -pub fn extract_type_hash(qos: &Qos) -> Option { - if let Some(v) = &qos.user_data { - if let Ok(s) = str::from_utf8(v) { - if let Some(mut start) = s.find(USER_DATA_TYPEHASH_KEY) { - start += USER_DATA_TYPEHASH_KEY.len(); - if let Some(end) = s[start..].find(USER_DATA_PROPS_SEPARATOR) { - return Some(s[start..(start + end)].into()); - } - } - } - } - None -} +/// For potential use later (type_hash in admin space?) +// pub fn extract_type_hash(qos: &Qos) -> Option { +// if let Some(v) = &qos.user_data { +// if let Ok(s) = str::from_utf8(v) { +// if let Some(mut start) = s.find(USER_DATA_TYPEHASH_KEY) { +// start += USER_DATA_TYPEHASH_KEY.len(); +// if let Some(end) = s[start..].find(USER_DATA_PROPS_SEPARATOR) { +// return Some(s[start..(start + end)].into()); +// } +// } +// } +// } +// None +// } pub fn insert_type_hash(qos: &mut Qos, type_hash: &str) { let mut s = USER_DATA_TYPEHASH_KEY.to_string();