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())