diff --git a/zenoh-plugin-ros2dds/src/ros2_utils.rs b/zenoh-plugin-ros2dds/src/ros2_utils.rs index 9a99ae8..f72cb0c 100644 --- a/zenoh-plugin-ros2dds/src/ros2_utils.rs +++ b/zenoh-plugin-ros2dds/src/ros2_utils.rs @@ -12,7 +12,10 @@ // ZettaScale Zenoh Team, // -use std::sync::atomic::{AtomicU32, Ordering}; +use std::{ + env::VarError, + sync::atomic::{AtomicU32, Ordering}, +}; use cyclors::{ dds_entity_t, @@ -29,7 +32,12 @@ 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"; +// ROS_DISTRO value assumed if the environment variable is not set +pub const ASSUMED_ROS_DISTRO: &str = "iron"; + lazy_static::lazy_static!( + pub static ref ROS_DISTRO: String = get_ros_distro(); + pub static ref KE_SUFFIX_ACTION_SEND_GOAL: &'static keyexpr = ke_for_sure!("_action/send_goal"); pub static ref KE_SUFFIX_ACTION_CANCEL_GOAL: &'static keyexpr = ke_for_sure!("_action/cancel_goal"); pub static ref KE_SUFFIX_ACTION_GET_RESULT: &'static keyexpr = ke_for_sure!("_action/get_result"); @@ -41,6 +49,38 @@ lazy_static::lazy_static!( pub static ref QOS_DEFAULT_ACTION_STATUS: Qos = ros2_action_status_default_qos(); ); +pub fn get_ros_distro() -> String { + match std::env::var("ROS_DISTRO") { + Ok(s) if !s.is_empty() => { + log::debug!("ROS_DISTRO detected: {s}"); + s + } + Ok(_) | Err(VarError::NotPresent) => { + log::warn!( + "ROS_DISTRO environment variable is not set. \ + Assuming '{ASSUMED_ROS_DISTRO}', but this could lead to errors on 'ros_discovery_info' \ + (see https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/issues/21)" + ); + ASSUMED_ROS_DISTRO.to_string() + } + Err(VarError::NotUnicode(s)) => { + log::warn!( + "ROS_DISTRO environment variable is invalid ('{s:?}'). \ + Assuming '{ASSUMED_ROS_DISTRO}', but this could lead to errors on 'ros_discovery_info' \ + (see https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/issues/21)" + ); + ASSUMED_ROS_DISTRO.to_string() + } + } +} + +/// Check if the ROS_DISTRO is older than `distro`, comparing the 1st char. +/// None is returned if ROS_DISTRO is not set. +pub fn ros_distro_is_less_than(distro: &str) -> bool { + assert!(!distro.is_empty()); + ROS_DISTRO.chars().next() < distro.chars().next() +} + /// 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 { diff --git a/zenoh-plugin-ros2dds/src/ros_discovery.rs b/zenoh-plugin-ros2dds/src/ros_discovery.rs index 5615d26..36f2e53 100644 --- a/zenoh-plugin-ros2dds/src/ros_discovery.rs +++ b/zenoh-plugin-ros2dds/src/ros_discovery.rs @@ -1,4 +1,5 @@ use crate::dds_types::DDSRawSample; +use crate::ros2_utils::ros_distro_is_less_than; use crate::{ChannelEvent, ROS_DISCOVERY_INFO_PUSH_INTERVAL_MS}; // // Copyright (c) 2022 ZettaScale Technology @@ -284,8 +285,8 @@ impl RosDiscoveryInfoMgr { Ok(i) => Some(i), Err(e) => { log::warn!( - "Error receiving ParticipantEntitiesInfo on ros_discovery_info: {}", - e + "Error receiving ParticipantEntitiesInfo on ros_discovery_info: {} - payload: {}", + e, sample.hex_encode() ); None } @@ -471,11 +472,11 @@ fn serialize_ros_gid(gid: &Gid, serializer: S) -> Result where S: Serializer, { - if serializer.is_human_readable() { + if serializer.is_human_readable() || !ros_distro_is_less_than("iron") { gid.serialize(serializer) } else { - // Data size for gid in ROS messages in 24 bytes, while a DDS gid is 16 bytes. - // Rely on "impl Serialize for Gid" for the 16 bytes, and add the last 8 bytes. + // #21: prior to iron the Gid type in ROS messages was 'char[24] data'. + // Then 8 bytes shall be added since here it's defined as 16 bytes (as per DDS spec) Serialize::serialize(&(gid, &BYTES_8), serializer) } } @@ -484,12 +485,12 @@ fn deserialize_ros_gid<'de, D>(deserializer: D) -> Result where D: Deserializer<'de>, { - if deserializer.is_human_readable() { + if deserializer.is_human_readable() || !ros_distro_is_less_than("iron") { // Rely on impl<'de> Deserialize<'de> for Gid Deserialize::deserialize(deserializer) } else { - // Data size for gid in ROS messages in 24 bytes, while a DDS gid is 16 bytes. - // Rely on "impl<'de> Deserialize<'de> for Gid" for the 16 bytes, and ignore the last 8 bytes + // #21: prior to iron the Gid type in ROS messages was 'char[24] data'. + // then 8 bytes shall be removed since here it's defined as 16 bytes (as per DDS spec) let (result, _ignore): (Gid, [u8; 8]) = Deserialize::deserialize(deserializer)?; Ok(result) } @@ -502,11 +503,11 @@ where let is_human_readable = serializer.is_human_readable(); let mut seq: ::SerializeSeq = serializer.serialize_seq(Some(gids.len()))?; for gid in gids { - if is_human_readable { + if is_human_readable || !ros_distro_is_less_than("iron") { seq.serialize_element(gid)?; } else { - // Data size for gid in ROS messages in 24 bytes, while a DDS gid is 16 bytes. - // Rely on "impl Serialize for Gid" for the 16 bytes, and add the last 8 bytes. + // #21: prior to iron the Gid type in ROS messages was 'char[24] data'. + // Then 8 bytes shall be added since here it's defined as 16 bytes (as per DDS spec) seq.serialize_element(&(gid, &BYTES_8))?; } } @@ -517,16 +518,16 @@ fn deserialize_ros_gids<'de, D>(deserializer: D) -> Result, D::Erro where D: Deserializer<'de>, { - if deserializer.is_human_readable() { + if deserializer.is_human_readable() || !ros_distro_is_less_than("iron") { Deserialize::deserialize(deserializer) } else { - // Data size for gid in ROS messages in 24 bytes, while a DDS gid is 16 bytes. - // Deserialize as Vec<[u8; 24]>, consider 16 bytes only for each + // #21: prior to iron the Gid type in ROS messages was 'char[24] data'. + // then 8 bytes shall be removed since here it's defined as 16 bytes (as per DDS spec) let ros_gids: Vec<[u8; 24]> = Deserialize::deserialize(deserializer)?; - // NOTE: a DDS gid is 16 bytes only. ignore the last 8 bytes Ok(ros_gids .iter() .map(|ros_gid| { + // Ignore the last 8 bytes TryInto::<&[u8; 16]>::try_into(&ros_gid[..16]) .unwrap() .into()