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.

Sign up for GitHub

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

Fix incompatibility with Iron and Rolling (fix #21) #22

Merged
merged 2 commits into from
Nov 30, 2023
Merged

Fix incompatibility with Iron and Rolling (fix #21) #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
39cbaf1
Fix incompatibility with Iron and Rolling (fix #21)
JEnoch Nov 29, 2023
ec365a8
Fix tests and add 1
JEnoch Nov 30, 2023
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
42 changes: 41 additions & 1 deletion zenoh-plugin-ros2dds/src/ros2_utils.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,10 @@
// ZettaScale Zenoh Team, <[email protected]>
//

use std::sync::atomic::{AtomicU32, Ordering};
use std::{
env::VarError,
sync::atomic::{AtomicU32, Ordering},
};

use cyclors::{
dds_entity_t,
Expand All @@ -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");
Expand All @@ -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 {
Expand Down
117 changes: 99 additions & 18 deletions zenoh-plugin-ros2dds/src/ros_discovery.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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
}
Expand Down Expand Up @@ -471,11 +472,11 @@ fn serialize_ros_gid<S>(gid: &Gid, serializer: S) -> Result<S::Ok, S::Error>
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)
}
}
Expand All @@ -484,12 +485,12 @@ fn deserialize_ros_gid<'de, D>(deserializer: D) -> Result<Gid, D::Error>
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)
}
Expand All @@ -502,11 +503,11 @@ where
let is_human_readable = serializer.is_human_readable();
let mut seq: <S as Serializer>::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))?;
}
}
Expand All @@ -517,16 +518,16 @@ fn deserialize_ros_gids<'de, D>(deserializer: D) -> Result<HashSet<Gid>, 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()
Expand Down Expand Up @@ -564,16 +565,21 @@ where
}

mod tests {

#[test]
fn test_serde() {
#[ignore]
// Test ignored as it cannot be run at the same time than test_serde_after_iron()
// Both need different ROS_DISTRO env var, that cannot be changed between the 2 tests
// Run this test individually or with `cargo test -- --ignored``
fn test_serde_prior_to_iron() {
use super::*;
use crate::ros2_utils::get_ros_distro;
use std::str::FromStr;

// ros_discovery_message sent by a component_container node started as such:
// ros_discovery_message sent by a component_container node on Humble started as such:
// - ros2 run rclcpp_components component_container --ros-args --remap __ns:=/TEST
// - ros2 component load /TEST/ComponentManager composition composition::Listener
// - ros2 component load /TEST/ComponentManager composition composition::Talker
std::env::set_var("ROS_DISTRO", "humble");
let ros_discovery_info_cdr: Vec<u8> = hex::decode(
"000100000110de17b1eaf995400c9ac8000001c1000000000000000003000000\
060000002f5445535400000011000000436f6d706f6e656e744d616e61676572\
Expand Down Expand Up @@ -613,6 +619,7 @@ mod tests {
)
.unwrap();

println!("ROS_DISTRO={}", get_ros_distro());
let part_info: ParticipantEntitiesInfo = cdr::deserialize(&ros_discovery_info_cdr).unwrap();
println!("{:?}", part_info);

Expand Down Expand Up @@ -643,4 +650,78 @@ mod tests {
assert_eq!(node_talker.reader_gid_seq.len(), 7);
assert_eq!(node_talker.writer_gid_seq.len(), 9);
}

#[test]
fn test_serde_after_iron() {
use super::*;
use crate::ros2_utils::get_ros_distro;
use std::str::FromStr;

// ros_discovery_message sent by a component_container node on Iron started as such:
// - ros2 run rclcpp_components component_container --ros-args --remap __ns:=/TEST
// - ros2 component load /TEST/ComponentManager composition composition::Listener
// - ros2 component load /TEST/ComponentManager composition composition::Talker
std::env::set_var("ROS_DISTRO", "iron");
let ros_discovery_info_cdr: Vec<u8> = hex::decode(
"00010000010f20a26b2fbd8000000000000001c103000000060000002f544553\
5400000011000000436f6d706f6e656e744d616e616765720000000005000000\
010f20a26b2fbd800000000000000404010f20a26b2fbd800000000000000504\
010f20a26b2fbd800000000000000704010f20a26b2fbd800000000000000904\
010f20a26b2fbd800000000000000b0405000000010f20a26b2fbd8000000000\
00000303010f20a26b2fbd800000000000000603010f20a26b2fbd8000000000\
00000803010f20a26b2fbd800000000000000a03010f20a26b2fbd8000000000\
00000c03020000002f000000090000006c697374656e65720000000009000000\
010f20a26b2fbd800000000000000e04010f20a26b2fbd800000000000001004\
010f20a26b2fbd800000000000001204010f20a26b2fbd800000000000001404\
010f20a26b2fbd800000000000001604010f20a26b2fbd800000000000001804\
010f20a26b2fbd800000000000001b04010f20a26b2fbd800000000000001c04\
010f20a26b2fbd800000000000001e0409000000010f20a26b2fbd8000000000\
00000d03010f20a26b2fbd800000000000000f03010f20a26b2fbd8000000000\
00001103010f20a26b2fbd800000000000001303010f20a26b2fbd8000000000\
00001503010f20a26b2fbd800000000000001703010f20a26b2fbd8000000000\
00001903010f20a26b2fbd800000000000001a03010f20a26b2fbd8000000000\
00001d03020000002f0000000700000074616c6b6572000008000000010f20a2\
6b2fbd800000000000002004010f20a26b2fbd800000000000002204010f20a2\
6b2fbd800000000000002404010f20a26b2fbd800000000000002604010f20a2\
6b2fbd800000000000002804010f20a26b2fbd800000000000002a04010f20a2\
6b2fbd800000000000002d04010f20a26b2fbd800000000000002e040a000000\
010f20a26b2fbd800000000000001f03010f20a26b2fbd800000000000002103\
010f20a26b2fbd800000000000002303010f20a26b2fbd800000000000002503\
010f20a26b2fbd800000000000002703010f20a26b2fbd800000000000002903\
010f20a26b2fbd800000000000002b03010f20a26b2fbd800000000000002c03\
010f20a26b2fbd800000000000002f03010f20a26b2fbd800000000000003003",
)
.unwrap();

println!("ROS_DISTRO={}", get_ros_distro());
let part_info: ParticipantEntitiesInfo = cdr::deserialize(&ros_discovery_info_cdr).unwrap();
println!("{:?}", part_info);

assert_eq!(
part_info.gid,
Gid::from_str("010f20a26b2fbd8000000000000001c1").unwrap()
);
assert_eq!(part_info.node_entities_info_seq.len(), 3);

let node_componentmgr = part_info
.node_entities_info_seq
.get("/TEST/ComponentManager")
.unwrap();
assert_eq!(node_componentmgr.node_namespace, "/TEST");
assert_eq!(node_componentmgr.node_name, "ComponentManager");
assert_eq!(node_componentmgr.reader_gid_seq.len(), 5);
assert_eq!(node_componentmgr.writer_gid_seq.len(), 5);

let node_listener = part_info.node_entities_info_seq.get("/listener").unwrap();
assert_eq!(node_listener.node_namespace, "/");
assert_eq!(node_listener.node_name, "listener");
assert_eq!(node_listener.reader_gid_seq.len(), 9);
assert_eq!(node_listener.writer_gid_seq.len(), 9);

let node_talker = part_info.node_entities_info_seq.get("/talker").unwrap();
assert_eq!(node_talker.node_namespace, "/");
assert_eq!(node_talker.node_name, "talker");
assert_eq!(node_talker.reader_gid_seq.len(), 8);
assert_eq!(node_talker.writer_gid_seq.len(), 10);
}
}
Loading