From 38344071cf4037099a2982d41669b0611d1a005d Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 5 Sep 2023 14:56:10 +0300 Subject: [PATCH 1/7] - fix datatype handling by switching to * everywhere - remove some unwraps, use error handling - build with up-to-date rosrust, change smth for compatibility with it's API - split bridging mode to remote and local bridging modes, support this modes in bridge operation - some more fixes here and there --- zenoh-bridge-ros1/Cargo.toml | 3 + zenoh-bridge-ros1/src/main.rs | 4 +- zenoh-plugin-ros1/examples/ros1_pub.rs | 6 +- zenoh-plugin-ros1/examples/ros1_service.rs | 6 +- .../examples/ros1_standalone_pub.rs | 6 +- .../examples/ros1_standalone_sub.rs | 6 +- zenoh-plugin-ros1/examples/ros1_sub.rs | 6 +- zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs | 16 ++++-- .../src/ros_to_zenoh_bridge/bridge_type.rs | 4 +- .../ros_to_zenoh_bridge/bridges_storage.rs | 13 +++-- .../src/ros_to_zenoh_bridge/discovery.rs | 55 ++++++++++--------- .../src/ros_to_zenoh_bridge/environment.rs | 11 +++- .../src/ros_to_zenoh_bridge/ros1_client.rs | 15 +++-- .../ros1_to_zenoh_bridge_impl.rs | 7 ++- .../src/ros_to_zenoh_bridge/test_helpers.rs | 10 ++-- .../src/ros_to_zenoh_bridge/topic_bridge.rs | 55 ++++++++++++------- .../src/ros_to_zenoh_bridge/topic_mapping.rs | 15 +++++ zenoh-plugin-ros1/tests/discovery_test.rs | 26 ++++++++- zenoh-plugin-ros1/tests/ping_pong_test.rs | 12 ++-- zenoh-plugin-ros1/tests/rosmaster_test.rs | 6 +- 20 files changed, 193 insertions(+), 89 deletions(-) diff --git a/zenoh-bridge-ros1/Cargo.toml b/zenoh-bridge-ros1/Cargo.toml index b5787f7..6ac8e00 100644 --- a/zenoh-bridge-ros1/Cargo.toml +++ b/zenoh-bridge-ros1/Cargo.toml @@ -34,6 +34,9 @@ zenoh = { workspace = true } zenoh-plugin-trait = { workspace = true } zenoh-plugin-ros1 = { path = "../zenoh-plugin-ros1/", default-features = false } +[features] +preserve_topic_metadata = [] # development feature, do not use it + [[bin]] name = "zenoh-bridge-ros1" path = "src/main.rs" diff --git a/zenoh-bridge-ros1/src/main.rs b/zenoh-bridge-ros1/src/main.rs index cf6d58e..def0404 100644 --- a/zenoh-bridge-ros1/src/main.rs +++ b/zenoh-bridge-ros1/src/main.rs @@ -183,7 +183,9 @@ async fn main() { let config = parse_args(); // create a zenoh Runtime (to share with plugins) - let runtime = zenoh::runtime::Runtime::new(config).await.unwrap(); + let runtime = zenoh::runtime::Runtime::new(config) + .await + .expect("Error creating runtime"); // start ros1 plugin use zenoh_plugin_trait::Plugin; diff --git a/zenoh-plugin-ros1/examples/ros1_pub.rs b/zenoh-plugin-ros1/examples/ros1_pub.rs index 0c2b03d..ef6706f 100644 --- a/zenoh-plugin-ros1/examples/ros1_pub.rs +++ b/zenoh-plugin-ros1/examples/ros1_pub.rs @@ -36,9 +36,11 @@ async fn main() { // create ROS1 node and publisher print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new( - (Environment::ros_name().get() + "_test_source_node").as_str(), + let ros1_node = rosrust::api::Ros::new_raw( Environment::ros_master_uri().get().as_str(), + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + (Environment::ros_name().get() + "_test_source_node").as_str(), ) .unwrap(); println!(" OK!"); diff --git a/zenoh-plugin-ros1/examples/ros1_service.rs b/zenoh-plugin-ros1/examples/ros1_service.rs index cb8205a..5ba08a8 100644 --- a/zenoh-plugin-ros1/examples/ros1_service.rs +++ b/zenoh-plugin-ros1/examples/ros1_service.rs @@ -38,9 +38,11 @@ async fn main() { // create ROS1 node and service print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new( - (Environment::ros_name().get() + "_test_service_node").as_str(), + let ros1_node = rosrust::api::Ros::new_raw( Environment::ros_master_uri().get().as_str(), + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + (Environment::ros_name().get() + "_test_service_node").as_str(), ) .unwrap(); println!(" OK!"); diff --git a/zenoh-plugin-ros1/examples/ros1_standalone_pub.rs b/zenoh-plugin-ros1/examples/ros1_standalone_pub.rs index cf060e0..8d27b86 100644 --- a/zenoh-plugin-ros1/examples/ros1_standalone_pub.rs +++ b/zenoh-plugin-ros1/examples/ros1_standalone_pub.rs @@ -31,9 +31,11 @@ async fn main() { // create ROS1 node and publisher print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new( - (Environment::ros_name().get() + "_test_source_node").as_str(), + let ros1_node = rosrust::api::Ros::new_raw( Environment::ros_master_uri().get().as_str(), + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + (Environment::ros_name().get() + "_test_source_node").as_str(), ) .expect("Error creating ROS1 Node!"); println!(" OK!"); diff --git a/zenoh-plugin-ros1/examples/ros1_standalone_sub.rs b/zenoh-plugin-ros1/examples/ros1_standalone_sub.rs index fbd7b59..93ae10e 100644 --- a/zenoh-plugin-ros1/examples/ros1_standalone_sub.rs +++ b/zenoh-plugin-ros1/examples/ros1_standalone_sub.rs @@ -32,9 +32,11 @@ async fn main() { // create ROS1 node and subscriber print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new( - (Environment::ros_name().get() + "_test_subscriber_node").as_str(), + let ros1_node = rosrust::api::Ros::new_raw( Environment::ros_master_uri().get().as_str(), + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + (Environment::ros_name().get() + "_test_subscriber_node").as_str(), ) .expect("Error creating ROS1 Node!"); println!(" OK!"); diff --git a/zenoh-plugin-ros1/examples/ros1_sub.rs b/zenoh-plugin-ros1/examples/ros1_sub.rs index f5aa104..c4147e4 100644 --- a/zenoh-plugin-ros1/examples/ros1_sub.rs +++ b/zenoh-plugin-ros1/examples/ros1_sub.rs @@ -37,9 +37,11 @@ async fn main() { // create ROS1 node and subscriber print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new( - (Environment::ros_name().get() + "_test_subscriber_node").as_str(), + let ros1_node = rosrust::api::Ros::new_raw( Environment::ros_master_uri().get().as_str(), + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + (Environment::ros_name().get() + "_test_subscriber_node").as_str(), ) .unwrap(); println!(" OK!"); diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs index 0608172..ddd6bb1 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs @@ -14,8 +14,9 @@ use async_std::task::JoinHandle; +use log::error; use zenoh; -use zenoh_core::AsyncResolve; +use zenoh_core::{zresult::ZResult, AsyncResolve}; use std::sync::{ atomic::{AtomicBool, Ordering::Relaxed}, @@ -73,9 +74,9 @@ pub struct Ros1ToZenohBridge { task_handle: Box>, } impl Ros1ToZenohBridge { - pub async fn new_with_own_session(config: zenoh::config::Config) -> Self { - let session = zenoh::open(config).res_async().await.unwrap().into_arc(); - Self::new_with_external_session(session) + pub async fn new_with_own_session(config: zenoh::config::Config) -> ZResult { + let session = zenoh::open(config).res_async().await?.into_arc(); + Ok(Self::new_with_external_session(session)) } pub fn new_with_external_session(session: Arc) -> Self { @@ -93,14 +94,17 @@ impl Ros1ToZenohBridge { //PRIVATE: async fn run(session: Arc, flag: Arc) { - work_cycle( + if let Err(e) = work_cycle( Environment::ros_master_uri().get().as_str(), session, flag, |_v| {}, |_status| {}, ) - .await; + .await + { + error!("Error occured while running the bridge: {e}") + } } async fn async_await(&mut self) { diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs index 98d4e95..873c0a2 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs @@ -12,7 +12,9 @@ // ZettaScale Zenoh Team, // -#[derive(Clone, Copy, PartialEq, Eq)] +use strum_macros::Display; + +#[derive(Clone, Copy, PartialEq, Eq, Display)] pub enum BridgeType { Publisher, Subscriber, diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs index dbd176b..dad3400 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -146,16 +146,16 @@ impl<'a> ComplementaryElementAccessor<'a> { } Entry::Vacant(val) => { let key = val.key().clone(); - val.insert(TopicBridge::new( + let inserted = val.insert(TopicBridge::new( key, self.access.b_type, self.access.declaration_interface.clone(), self.access.ros1_client.clone(), self.access.zenoh_client.clone(), - Environment::bridging_mode().get(), - )) - .set_has_complementary_in_zenoh(true) - .await; + Environment::remote_bridging_mode().get(), + Environment::local_bridging_mode().get(), + )); + inserted.set_has_complementary_in_zenoh(true).await; } } } @@ -219,7 +219,8 @@ impl<'a> ElementAccessor<'a> { self.access.declaration_interface.clone(), self.access.ros1_client.clone(), self.access.zenoh_client.clone(), - Environment::bridging_mode().get(), + Environment::remote_bridging_mode().get(), + Environment::local_bridging_mode().get(), )); inserted.set_present_in_ros1(true).await; smth_changed = true; diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs index 0001e0e..7cb8bb7 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs @@ -62,8 +62,6 @@ impl RemoteResources { + Sync + 'static, { - let subscriber: Option; - // make proper discovery keyexpr let mut formatter = discovery_format::formatter(); let discovery_keyexpr = zenoh::keformat!( @@ -96,15 +94,13 @@ impl RemoteResources { .build() .await; - match subscription { - Ok(s) => { - subscriber = Some(s); - } + let subscriber = match subscription { + Ok(s) => Some(s), Err(e) => { error!("ROS1 Discovery: error creating querying subscriber: {}", e); - subscriber = None; + None } - } + }; Self { _subscriber: subscriber, @@ -148,13 +144,23 @@ impl RemoteResources { F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send>, { //let discovery_namespace = discovery.discovery_namespace().ok_or("No discovery_namespace present!")?; - let datatype_bytes = hex::decode( - discovery - .data_type() - .ok_or("No data_type present!")? - .as_str(), - )?; - let datatype = std::str::from_utf8(&datatype_bytes)?; + let datatype; + #[cfg(feature = "preserve_topic_metadata")] + { + let datatype_bytes = hex::decode( + discovery + .data_type() + .ok_or("No data_type present!")? + .as_str(), + )?; + datatype = std::str::from_utf8(&datatype_bytes)?; + } + + #[cfg(not(feature = "preserve_topic_metadata"))] + { + datatype = "*"; + } + let resource_class = discovery .resource_class() .ok_or("No resource_class present!")? @@ -189,7 +195,7 @@ impl LocalResource { resource_class: &str, topic: &rosrust::api::Topic, session: Arc, - ) -> LocalResource { + ) -> ZResult { // make proper discovery keyexpr let mut formatter = discovery_format::formatter(); let discovery_keyexpr = zenoh::keformat!( @@ -199,13 +205,12 @@ impl LocalResource { data_type = hex::encode(topic.datatype.as_bytes()), bridge_namespace = bridge_namespace, topic = make_zenoh_key(topic) - ) - .unwrap(); + )?; let _declaration = AlohaDeclaration::new(session, discovery_keyexpr, Duration::from_secs(1)); - Self { _declaration } + Ok(Self { _declaration }) } } @@ -231,7 +236,7 @@ impl LocalResources { &self, topic: &rosrust::api::Topic, b_type: BridgeType, - ) -> LocalResource { + ) -> ZResult { match b_type { BridgeType::Publisher => self.declare_publisher(topic).await, BridgeType::Subscriber => self.declare_subscriber(topic).await, @@ -240,22 +245,22 @@ impl LocalResources { } } - pub async fn declare_publisher(&self, topic: &rosrust::api::Topic) -> LocalResource { + pub async fn declare_publisher(&self, topic: &rosrust::api::Topic) -> ZResult { self.declare(topic, ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS) .await } - pub async fn declare_subscriber(&self, topic: &rosrust::api::Topic) -> LocalResource { + pub async fn declare_subscriber(&self, topic: &rosrust::api::Topic) -> ZResult { self.declare(topic, ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS) .await } - pub async fn declare_service(&self, topic: &rosrust::api::Topic) -> LocalResource { + pub async fn declare_service(&self, topic: &rosrust::api::Topic) -> ZResult { self.declare(topic, ROS1_DISCOVERY_INFO_SERVICES_CLASS) .await } - pub async fn declare_client(&self, topic: &rosrust::api::Topic) -> LocalResource { + pub async fn declare_client(&self, topic: &rosrust::api::Topic) -> ZResult { self.declare(topic, ROS1_DISCOVERY_INFO_CLIENTS_CLASS).await } @@ -264,7 +269,7 @@ impl LocalResources { &self, topic: &rosrust::api::Topic, resource_class: &str, - ) -> LocalResource { + ) -> ZResult { LocalResource::new( &self.discovery_namespace, &self.bridge_namespace, diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs index 8a69007..1d4d897 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -95,8 +95,12 @@ impl Environment { return Entry::new("WITH_ROSMASTER", false); } - pub fn bridging_mode() -> Entry<'static, BridgingMode> { - return Entry::new("ROS_BRIDGING_MODE", BridgingMode::Auto); + pub fn remote_bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("ROS_REMOTE_BRIDGING_MODE", BridgingMode::Auto); + } + + pub fn local_bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("ROS_LOCAL_BRIDGING_MODE", BridgingMode::Auto); } pub fn master_polling_interval() -> Entry<'static, DurationString> { @@ -112,7 +116,8 @@ impl Environment { Self::ros_hostname(), Self::ros_name(), Self::ros_namespace(), - Self::bridging_mode().into(), + Self::remote_bridging_mode().into(), + Self::local_bridging_mode().into(), Self::master_polling_interval().into(), Self::with_rosmaster().into(), ] diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs index 429e275..a368cd7 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs @@ -14,6 +14,7 @@ use log::debug; use rosrust; +use zenoh_core::{zerror, zresult::ZResult}; pub struct Ros1Client { ros: rosrust::api::Ros, @@ -21,10 +22,16 @@ pub struct Ros1Client { impl Ros1Client { // PUBLIC - pub fn new(name: &str, master_uri: &str) -> Ros1Client { - Ros1Client { - ros: rosrust::api::Ros::new(name, master_uri).unwrap(), - } + pub fn new(name: &str, master_uri: &str) -> ZResult { + Ok(Ros1Client { + ros: rosrust::api::Ros::new_raw( + master_uri, + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + name, + ) + .map_err(|e| zerror!("{e}"))?, + }) } pub fn subscribe( diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index 95ec356..7c90913 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -15,6 +15,7 @@ use async_std::sync::Mutex; use zenoh; +use zenoh_core::zresult::ZResult; use std::{ sync::{ @@ -57,14 +58,15 @@ pub async fn work_cycle( flag: Arc, ros_status_callback: RosStatusCallback, statistics_callback: BridgeStatisticsCallback, -) where +) -> ZResult<()> +where RosStatusCallback: Fn(RosStatus), BridgeStatisticsCallback: Fn(BridgeStatus), { let ros1_client = Arc::new(ros1_client::Ros1Client::new( Environment::ros_name().get().as_str(), ros_master_uri, - )); + )?); let zenoh_client = Arc::new(zenoh_client::ZenohClient::new(session.clone())); let local_resources = Arc::new(LocalResources::new( @@ -83,6 +85,7 @@ pub async fn work_cycle( let mut bridge = RosToZenohBridge::new(ros_status_callback, statistics_callback); bridge.run(ros1_client, bridges, flag).await; + Ok(()) } async fn make_discovery<'a>( diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs index cea9e65..3895bc5 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -135,7 +135,8 @@ impl RunningBridge { *my_status = status; }, ) - .await; + .await + .unwrap(); } pub async fn assert_ros_error(&self) { @@ -177,8 +178,9 @@ impl RunningBridge { ) -> bool { wait( move || { - let val = self.bridge_status.lock().unwrap(); - *val == (status)() + let expected = (status)(); + let real = self.bridge_status.lock().unwrap(); + *real == expected }, timeout, ) @@ -255,7 +257,7 @@ impl BridgeChecker { pub fn new(config: zenoh::config::Config, ros_master_uri: &str) -> BridgeChecker { let session = zenoh::open(config).res_sync().unwrap().into_arc(); BridgeChecker { - ros_client: ros1_client::Ros1Client::new("test_ros_node", ros_master_uri), + ros_client: ros1_client::Ros1Client::new("test_ros_node", ros_master_uri).unwrap(), zenoh_client: zenoh_client::ZenohClient::new(session.clone()), local_resources: LocalResources::new("*".to_string(), "*".to_string(), session), expected_bridge_status: Arc::new(RwLock::new(BridgeStatus::default())), diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs index e4fc19b..cc1b8c4 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -19,7 +19,7 @@ use super::{ ros1_client, zenoh_client, }; use log::error; -use std::sync::Arc; +use std::{fmt::Display, sync::Arc}; use strum_macros::{Display, EnumString}; #[derive(PartialEq, Eq, EnumString, Clone, Display)] @@ -35,7 +35,8 @@ pub struct TopicBridge { ros1_client: Arc, zenoh_client: Arc, - briging_mode: BridgingMode, + remote_bridging_mode: BridgingMode, + local_bridging_mode: BridgingMode, required_on_ros1_side: bool, required_on_zenoh_side: bool, @@ -45,6 +46,12 @@ pub struct TopicBridge { bridge: Option, } +impl Display for TopicBridge { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{}:{:?}", self.b_type, self.topic) + } +} + impl TopicBridge { pub fn new( topic: rosrust::api::Topic, @@ -52,14 +59,16 @@ impl TopicBridge { declaration_interface: Arc, ros1_client: Arc, zenoh_client: Arc, - briging_mode: BridgingMode, + remote_bridging_mode: BridgingMode, + local_bridging_mode: BridgingMode, ) -> Self { Self { topic, b_type, ros1_client, zenoh_client, - briging_mode, + remote_bridging_mode, + local_bridging_mode, required_on_ros1_side: false, required_on_zenoh_side: false, declaration_interface, @@ -91,10 +100,7 @@ impl TopicBridge { } pub fn is_actual(&self) -> bool { - match self.briging_mode { - BridgingMode::Lazy => self.required_on_ros1_side || self.required_on_zenoh_side, - BridgingMode::Auto => self.required_on_ros1_side, - } + self.required_on_ros1_side || self.required_on_zenoh_side } //PRIVATE: @@ -106,11 +112,14 @@ impl TopicBridge { async fn recalc_declaration(&mut self) { match (self.required_on_ros1_side, &self.declaration) { (true, None) => { - self.declaration = Some( - self.declaration_interface - .declare_with_type(&self.topic, self.b_type) - .await, - ); + match self + .declaration_interface + .declare_with_type(&self.topic, self.b_type) + .await + { + Ok(decl) => self.declaration = Some(decl), + Err(e) => error!("{self}: error declaring discovery: {e}"), + } } (false, Some(_)) => { self.declaration = None; @@ -121,13 +130,19 @@ impl TopicBridge { async fn recalc_bridging(&mut self) { let is_discovered_client = self.b_type == BridgeType::Client && self.required_on_zenoh_side; - let is_required = self.required_on_ros1_side - && (self.briging_mode == BridgingMode::Auto || self.required_on_zenoh_side); - if is_required || is_discovered_client { - self.create_bridge().await; - } else { - self.bridge = None; + let is_required = is_discovered_client + || match (self.required_on_zenoh_side, self.required_on_ros1_side) { + (true, true) => true, + (true, false) => self.remote_bridging_mode == BridgingMode::Auto, + (false, true) => self.local_bridging_mode == BridgingMode::Auto, + (false, false) => false, + }; + + match (is_required, self.bridge.as_ref()) { + (true, Some(_)) => {} + (true, None) => self.create_bridge().await, + (false, _) => self.bridge = None, } } @@ -145,7 +160,7 @@ impl TopicBridge { } Err(e) => { self.bridge = None; - error!("Error creating bridge: {}", e); + error!("{self}: error creating bridge: {e}"); } } } diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs index 8d1f9ca..5c55090 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs @@ -54,6 +54,7 @@ impl Ros1TopicMapping { result } + #[cfg(feature = "preserve_topic_metadata")] fn fill( dst: &mut HashSet, data: &[rosrust::api::TopicData], @@ -75,4 +76,18 @@ impl Ros1TopicMapping { } } } + + #[cfg(not(feature = "preserve_topic_metadata"))] + fn fill( + dst: &mut HashSet, + data: &[rosrust::api::TopicData], + _topics: &[rosrust::api::Topic], + ) { + for item in data.iter() { + dst.insert(rosrust::api::Topic { + name: item.name.clone(), + datatype: "*".to_string(), + }); + } + } } diff --git a/zenoh-plugin-ros1/tests/discovery_test.rs b/zenoh-plugin-ros1/tests/discovery_test.rs index bfb9ddc..67c4016 100644 --- a/zenoh-plugin-ros1/tests/discovery_test.rs +++ b/zenoh-plugin-ros1/tests/discovery_test.rs @@ -19,6 +19,7 @@ use std::{ use async_std::prelude::FutureExt; use multiset::HashMultiSet; +use rosrust::api::Topic; use zenoh::{prelude::keyexpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; use zenoh_plugin_ros1::ros_to_zenoh_bridge::{ @@ -149,8 +150,31 @@ impl DiscoveryCollector { container: &Arc>>, expected: HashMultiSet, ) { + #[cfg(feature = "preserve_topic_metadata")] while expected != *container.lock().unwrap() { - async_std::task::sleep(core::time::Duration::from_millis(1)).await; + async_std::task::sleep(core::time::Duration::from_millis(10)).await; + } + #[cfg(not(feature = "preserve_topic_metadata"))] + { + let comparator = || { + let locked = container.lock().unwrap(); + if locked.len() != expected.len() { + return false; + } + for e in expected.iter() { + if !locked.contains(&Topic { + name: e.name.clone(), + datatype: "*".to_string(), + }) { + return false; + } + } + + true + }; + while !comparator() { + async_std::task::sleep(core::time::Duration::from_millis(10)).await; + } } } } diff --git a/zenoh-plugin-ros1/tests/ping_pong_test.rs b/zenoh-plugin-ros1/tests/ping_pong_test.rs index 83f12e1..e9e3bac 100644 --- a/zenoh-plugin-ros1/tests/ping_pong_test.rs +++ b/zenoh-plugin-ros1/tests/ping_pong_test.rs @@ -384,7 +384,8 @@ impl PingPong { let discovery_resource = backend .local_resources .declare_client(&BridgeChecker::make_topic(key)) - .await; + .await + .unwrap(); let zenoh_query = ZenohQuery::new(backend, key.to_string(), cycles.clone()); PingPong { @@ -406,7 +407,8 @@ impl PingPong { let discovery_resource = backend .local_resources .declare_service(&BridgeChecker::make_topic(key)) - .await; + .await + .unwrap(); let zenoh_queryable = backend .make_zenoh_queryable(key, |q| { async_std::task::spawn(async move { @@ -437,7 +439,8 @@ impl PingPong { let discovery_resource = backend .local_resources .declare_subscriber(&BridgeChecker::make_topic(key)) - .await; + .await + .unwrap(); let c = cycles.clone(); let rpub = ros1_pub.clone(); @@ -471,7 +474,8 @@ impl PingPong { let discovery_resource = backend .local_resources .declare_publisher(&BridgeChecker::make_topic(key)) - .await; + .await + .unwrap(); let c = cycles.clone(); let zpub = zenoh_pub.clone(); diff --git a/zenoh-plugin-ros1/tests/rosmaster_test.rs b/zenoh-plugin-ros1/tests/rosmaster_test.rs index 0ad5de9..62b204a 100644 --- a/zenoh-plugin-ros1/tests/rosmaster_test.rs +++ b/zenoh-plugin-ros1/tests/rosmaster_test.rs @@ -40,9 +40,11 @@ fn start_and_stop_master_and_check_connectivity() { }); // start ros1 client - let ros1_client = rosrust::api::Ros::new( - "start_and_stop_master_and_check_connectivity_client", + let ros1_client = rosrust::api::Ros::new_raw( "http://localhost:11311/", + &rosrust::api::resolve::hostname(), + &rosrust::api::resolve::namespace(), + "start_and_stop_master_and_check_connectivity_client", ) .unwrap(); From 76880b11dd3eacbb9f72fb8fc8522dbc3f94e3ba Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 7 Sep 2023 16:03:16 +0300 Subject: [PATCH 2/7] Add tests for isolation problem --- zenoh-plugin-ros1/tests/ros_test.rs | 267 ++++++++++++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 zenoh-plugin-ros1/tests/ros_test.rs diff --git a/zenoh-plugin-ros1/tests/ros_test.rs b/zenoh-plugin-ros1/tests/ros_test.rs new file mode 100644 index 0000000..8adc780 --- /dev/null +++ b/zenoh-plugin-ros1/tests/ros_test.rs @@ -0,0 +1,267 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::time::Duration; + +use rosrust::{Publisher, RawMessage, Subscriber}; +use zenoh_plugin_ros1::ros_to_zenoh_bridge::{ + ros1_client::Ros1Client, + test_helpers::{wait, BridgeChecker, IsolatedROSMaster, ROSEnvironment}, +}; + +fn wait_for_rosclient_to_connect(rosclient: &Ros1Client) -> bool { + async_std::task::block_on(wait( + || rosclient.topic_types().is_ok(), + Duration::from_secs(10), + )) +} + +fn wait_for_publishers(subscriber: &Subscriber, count: usize) -> bool { + async_std::task::block_on(wait( + || subscriber.publisher_count() == count, + Duration::from_secs(10), + )) +} + +fn wait_for_subscribers(publisher: &Publisher, count: usize) -> bool { + async_std::task::block_on(wait( + || publisher.subscriber_count() == count, + Duration::from_secs(10), + )) +} + +#[test] +fn check_rosclient_connectivity_ros_then_client() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let rosclient = Ros1Client::new("test_client", &roscfg.master_uri()).unwrap(); + assert!(wait_for_rosclient_to_connect(&rosclient)); +} + +#[test] +fn check_rosclient_connectivity_client_then_ros() { + // reserve isolated ros config + let roscfg = IsolatedROSMaster::default(); + + // start rosclient + let rosclient = + Ros1Client::new("test_client", &roscfg.master_uri()).expect("error creating Ros1Client!"); + + // start rosmaster + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // check that the client is connected + assert!(wait_for_rosclient_to_connect(&rosclient)); +} + +#[test] +fn check_rosclient_pub_sub_connection_pub_then_sub() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclients + let rosclient1 = + Ros1Client::new("test_client1", &roscfg.master_uri()).expect("error creating Ros1Client1!"); + let rosclient2 = + Ros1Client::new("test_client2", &roscfg.master_uri()).expect("error creating Ros1Client2!"); + assert!(wait_for_rosclient_to_connect(&rosclient1)); + assert!(wait_for_rosclient_to_connect(&rosclient2)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("check_rosclient_pub_sub_connection"); + + // create publisher and subscriber + let publisher = rosclient1 + .publish(&shared_topic) + .expect("error creating publisher!"); + let subscriber = rosclient2 + .subscribe(&shared_topic, |_: RawMessage| {}) + .expect("error creating subscriber!"); + + // check that they are connected + assert!(wait_for_publishers(&subscriber, 1)); + assert!(wait_for_subscribers(&publisher, 1)); + + // wait and check that they are still connected + std::thread::sleep(Duration::from_secs(5)); + assert!(subscriber.publisher_count() == 1); + assert!(publisher.subscriber_count() == 1); +} + +#[test] +fn check_rosclient_pub_sub_connection_sub_then_pub() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclients + let rosclient1 = + Ros1Client::new("test_client1", &roscfg.master_uri()).expect("error creating Ros1Client1!"); + let rosclient2 = + Ros1Client::new("test_client2", &roscfg.master_uri()).expect("error creating Ros1Client2!"); + assert!(wait_for_rosclient_to_connect(&rosclient1)); + assert!(wait_for_rosclient_to_connect(&rosclient2)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("check_rosclient_pub_sub_connection"); + + // create subscriber and publisher + let subscriber = rosclient2 + .subscribe(&shared_topic, |_: RawMessage| {}) + .expect("error creating subscriber!"); + let publisher = rosclient1 + .publish(&shared_topic) + .expect("error creating publisher!"); + + // check that they are connected + assert!(wait_for_publishers(&subscriber, 1)); + assert!(wait_for_subscribers(&publisher, 1)); + + // wait and check that they are still connected + std::thread::sleep(Duration::from_secs(5)); + assert!(subscriber.publisher_count() == 1); + assert!(publisher.subscriber_count() == 1); +} + +#[test] +fn check_rosclient_pub_sub_isolation_pub_then_sub() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let rosclient = + Ros1Client::new("test_client", &roscfg.master_uri()).expect("error creating Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&rosclient)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("check_rosclient_pub_sub_isolation"); + + // create publisher and subscriber + let publisher = rosclient + .publish(&shared_topic) + .expect("error creating publisher!"); + let subscriber = rosclient + .subscribe(&shared_topic, |_: RawMessage| {}) + .expect("error creating subscriber!"); + + // check that they are isolated + assert!(subscriber.publisher_count() == 0); + assert!(publisher.subscriber_count() == 0); + + // wait and check that they are still isolated + std::thread::sleep(Duration::from_secs(5)); + assert!(subscriber.publisher_count() == 0); + assert!(publisher.subscriber_count() == 0); +} + +#[test] +fn check_rosclient_pub_sub_isolation_sub_then_pub() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let rosclient = + Ros1Client::new("test_client", &roscfg.master_uri()).expect("error creating Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&rosclient)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("check_rosclient_pub_sub_isolation"); + + // create publisher and subscriber + let subscriber = rosclient + .subscribe(&shared_topic, |_: RawMessage| {}) + .expect("error creating subscriber!"); + let publisher = rosclient + .publish(&shared_topic) + .expect("error creating publisher!"); + + // check that they are isolated + assert!(subscriber.publisher_count() == 0); + assert!(publisher.subscriber_count() == 0); + + // wait and check that they are still isolated + std::thread::sleep(Duration::from_secs(5)); + assert!(subscriber.publisher_count() == 0); + assert!(publisher.subscriber_count() == 0); +} + +#[test] +fn prove_rosclient_service_non_isolation_service_then_client() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let rosclient = + Ros1Client::new("test_client", &roscfg.master_uri()).expect("error creating Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&rosclient)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("prove_rosclient_service_non_isolation"); + + // create service and client + let _service = rosclient + .service::(&shared_topic, Ok) + .expect("error creating service!"); + let client = rosclient + .client(&shared_topic) + .expect("error creating client!"); + + // check that they are isolated + assert!(client.probe(Duration::from_secs(1)).is_ok()); + assert!(client.req(&RawMessage::default()).is_ok()); + + // wait and check that they are still isolated + std::thread::sleep(Duration::from_secs(5)); + assert!(client.probe(Duration::from_secs(1)).is_ok()); + assert!(client.req(&RawMessage::default()).is_ok()); +} + +#[test] +fn prove_rosclient_service_non_isolation_client_then_service() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let rosclient = + Ros1Client::new("test_client", &roscfg.master_uri()).expect("error creating Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&rosclient)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("prove_rosclient_service_non_isolation"); + + // create client and service + let client = rosclient + .client(&shared_topic) + .expect("error creating client!"); + let _service = rosclient + .service::(&shared_topic, Ok) + .expect("error creating service!"); + + // check that they are isolated + assert!(client.probe(Duration::from_secs(1)).is_ok()); + assert!(client.req(&RawMessage::default()).is_ok()); + + // wait and check that they are still isolated + std::thread::sleep(Duration::from_secs(5)); + assert!(client.probe(Duration::from_secs(1)).is_ok()); + assert!(client.req(&RawMessage::default()).is_ok()); +} From 1ba76022d3f001a0a0557e61fe96e273f0cdb5bd Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 8 Sep 2023 20:30:28 +0400 Subject: [PATCH 3/7] - bridging mode support corrected - add per-topic bridging mode setting - add Disabled bridging mode - use z*lock! macroses - small config refactoring - client bridging disabled in the default config - added tests for ROS1 client library behaviour --- zenoh-bridge-ros1/src/main.rs | 71 ++++++++---- zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs | 4 + .../ros_to_zenoh_bridge/bridges_storage.rs | 79 +++++++------ .../src/ros_to_zenoh_bridge/bridging_mode.rs | 70 ++++++++++++ .../src/ros_to_zenoh_bridge/environment.rs | 105 ++++++++++++++++-- .../ros_to_zenoh_bridge/ros1_master_ctrl.rs | 6 +- .../ros1_to_zenoh_bridge_impl.rs | 14 +-- .../src/ros_to_zenoh_bridge/test_helpers.rs | 5 +- .../src/ros_to_zenoh_bridge/topic_bridge.rs | 21 +--- zenoh-plugin-ros1/tests/bridge_to_bridge.rs | 16 ++- zenoh-plugin-ros1/tests/ping_pong_test.rs | 9 +- zenoh-plugin-ros1/tests/ros_test.rs | 88 +++++++++++++++ 12 files changed, 391 insertions(+), 97 deletions(-) create mode 100644 zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridging_mode.rs diff --git a/zenoh-bridge-ros1/src/main.rs b/zenoh-bridge-ros1/src/main.rs index def0404..fcd5933 100644 --- a/zenoh-bridge-ros1/src/main.rs +++ b/zenoh-bridge-ros1/src/main.rs @@ -16,6 +16,7 @@ use clap::{App, Arg}; use std::str::FromStr; use zenoh::config::Config; use zenoh::prelude::*; +use zenoh_plugin_ros1::ros_to_zenoh_bridge::environment::Environment; macro_rules! insert_json5 { ($config: expr, $args: expr, $key: expr, if $name: expr) => { @@ -83,34 +84,62 @@ r#"--rest-http-port=[PORT | IP:PORT] \ // ros1 related arguments: // .arg(Arg::from_usage( -r#"-u, --ros_master_uri=[ENDPOINT] \ +r#"--ros_master_uri=[ENDPOINT] \ 'A URI of the ROS1 Master to connect to, the defailt is http://localhost:11311/'"# )) .arg(Arg::from_usage( -r#"-h, --ros_hostname=[String] 'A hostname to send to ROS1 Master, the default is system's hostname'"# +r#"--ros_hostname=[String] 'A hostname to send to ROS1 Master, the default is system's hostname'"# )) .arg(Arg::from_usage( -r#"-n, --ros_name=[String] 'A bridge node's name for ROS1, the default is "ros1_to_zenoh_bridge"'"# +r#"--ros_name=[String] 'A bridge node's name for ROS1, the default is "ros1_to_zenoh_bridge"'"# )) .arg(Arg::from_usage( -r#"-b, --ros_bridging_mode=[String] \ -'Mode defining the moment to bridge topics. Accepted values:' - - "auto"(default) - bridge topics once they are declared locally - - "lazy" - bridge topics once they are declared both locally and required remotely through discovery -Warn: this setting is ignored for local ROS1 clients, as they require a tricky discovery mechanism"# +r#"--subscriber_bridging_mode=[String] \ +'Mode of subscriber's topic bridging. Accepted values:' + - "auto"(default) - bridge topics once they are declared locally or discovered remotely + - "lazy_auto" - bridge topics once they are both declared locally and discovered remotely + - "disabled" - never bridge topics. This setting will also suppress the topic discovery."# )) .arg(Arg::from_usage( -r#"-p, --ros_master_polling_interval=[String] \ -'An interval how to poll the ROS1 master for status -Bridge polls ROS1 master to get information on local topics, as this is the only way to keep -this info updated. This is the interval of this polling. The default is "100ms". -Accepted value:' +r#"--publisher_bridging_mode=[String] \ +'Mode of publisher's topic bridging. Accepted values:' + - "auto"(default) - bridge topics once they are declared locally or discovered remotely + - "lazy_auto" - bridge topics once they are both declared locally and discovered remotely + - "disabled" - never bridge topics. This setting will also suppress the topic discovery."# + )) + .arg(Arg::from_usage( +r#"--service_bridging_mode=[String] \ +'Mode of service's topic bridging. Accepted values:' + - "auto"(default) - bridge topics once they are declared locally or discovered remotely + - "lazy_auto" - bridge topics once they are both declared locally and discovered remotely + - "disabled" - never bridge topics. This setting will also suppress the topic discovery."# + )) + .arg(Arg::from_usage( +r#"--client_bridging_mode=[String] \ +'Mode of client's topic bridging. Accepted values:' + - "auto" - bridge topics once they are discovered remotely + - "disabled"(default) - never bridge topics. This setting will also suppress the topic discovery. + NOTE: there are some pecularities on how ROS1 handles clients: + - ROS1 doesn't provide any client discovery mechanism + - ROS1 doesn't allow multiple services on the same topic + Due to this, client's bridging works differently compared to pub\sub bridging: + - lazy bridging mode is not available as there is no way to discover local ROS1 clients + - client bridging is disabled by default, as it may brake the local ROS1 system if it intends to have client and service interacting on the same topic + In order to use client bridging, you have two options: + - globally select auto bridging mode (with caution!) with this option + - bridge specific topics using 'client_topic_custom_bridging_mode' option (with a little bit less caution!)"# + )) + .arg(Arg::from_usage( +r#"--ros_master_polling_interval=[String] \ +'An interval to poll the ROS1 master for status +Bridge polls ROS1 master to get information on local topics. This option is the interval of this polling. The default is "100ms". +Accepted value:' A string such as 100ms, 2s, 5m The string format is [0-9]+(ns|us|ms|[smhdwy])"# )) .arg(Arg::from_usage( -r#"-r, --with_rosmaster=[bool] 'An option wether the bridge should run it's own rosmaster process, the default is "false"'"# +r#"--with_rosmaster=[bool] 'An option wether the bridge should run it's own rosmaster process, the default is "false"'"# )); let args = app.get_matches(); @@ -158,12 +187,14 @@ r#"-r, --with_rosmaster=[bool] 'An option wether the bridge should run it's ow } // apply ros1 related arguments over config - insert_json5!(config, args, "plugins/ros1/ros_master_uri", if "ros_master_uri",); - insert_json5!(config, args, "plugins/ros1/ros_hostname", if "ros_hostname",); - insert_json5!(config, args, "plugins/ros1/ros_name", if "ros_name", ); - insert_json5!(config, args, "plugins/ros1/ros_bridging_mode", if "ros_bridging_mode", ); - insert_json5!(config, args, "plugins/ros1/ros_master_polling_interval", if "ros_master_polling_interval", ); - insert_json5!(config, args, "plugins/ros1/with_rosmaster", if "with_rosmaster", ); + // run through the bridge's supported config options and fill them from command line options + let plugin_configuration_entries = Environment::env(); + for entry in plugin_configuration_entries.iter() { + let lowercase_name = entry.name.to_lowercase(); + let lowercase_path = format!("plugins/ros1/{}", lowercase_name); + insert_json5!(config, args, lowercase_path.as_str(), if lowercase_name.as_str(),); + } + config } diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs index ddd6bb1..df88c84 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs @@ -32,6 +32,8 @@ pub mod aloha_subscription; #[cfg(feature = "test")] pub mod bridge_type; #[cfg(feature = "test")] +pub mod bridging_mode; +#[cfg(feature = "test")] pub mod discovery; #[cfg(feature = "test")] pub mod ros1_client; @@ -51,6 +53,8 @@ mod aloha_subscription; #[cfg(not(feature = "test"))] mod bridge_type; #[cfg(not(feature = "test"))] +mod bridging_mode; +#[cfg(not(feature = "test"))] mod discovery; #[cfg(not(feature = "test"))] mod ros1_client; diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs index dad3400..f71b326 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -18,9 +18,14 @@ use std::{ }; use super::{ - bridge_type::BridgeType, discovery::LocalResources, environment::Environment, ros1_client, - ros1_to_zenoh_bridge_impl::BridgeStatus, topic_bridge::TopicBridge, - topic_mapping::Ros1TopicMapping, zenoh_client, + bridge_type::BridgeType, + bridging_mode::{bridging_mode, BridgingMode}, + discovery::LocalResources, + ros1_client, + ros1_to_zenoh_bridge_impl::BridgeStatus, + topic_bridge::TopicBridge, + topic_mapping::Ros1TopicMapping, + zenoh_client, }; struct Bridges { @@ -140,22 +145,24 @@ impl<'a> ComplementaryElementAccessor<'a> { } pub async fn complementary_entity_discovered(&mut self, topic: rosrust::api::Topic) { - match self.access.container.entry(topic) { - Entry::Occupied(mut val) => { - val.get_mut().set_has_complementary_in_zenoh(true).await; - } - Entry::Vacant(val) => { - let key = val.key().clone(); - let inserted = val.insert(TopicBridge::new( - key, - self.access.b_type, - self.access.declaration_interface.clone(), - self.access.ros1_client.clone(), - self.access.zenoh_client.clone(), - Environment::remote_bridging_mode().get(), - Environment::local_bridging_mode().get(), - )); - inserted.set_has_complementary_in_zenoh(true).await; + let b_mode = bridging_mode(self.access.b_type, topic.name.as_str()); + if b_mode != BridgingMode::Disabled { + match self.access.container.entry(topic) { + Entry::Occupied(mut val) => { + val.get_mut().set_has_complementary_in_zenoh(true).await; + } + Entry::Vacant(val) => { + let key = val.key().clone(); + let inserted = val.insert(TopicBridge::new( + key, + self.access.b_type, + self.access.declaration_interface.clone(), + self.access.ros1_client.clone(), + self.access.zenoh_client.clone(), + b_mode, + )); + inserted.set_has_complementary_in_zenoh(true).await; + } } } } @@ -208,22 +215,24 @@ impl<'a> ElementAccessor<'a> { // run through the topics and create corresponding bridges for ros_topic in part_of_ros_state.iter() { - match self.access.container.entry(ros_topic.clone()) { - Entry::Occupied(_val) => { - debug_assert!(false); // that shouldn't happen - } - Entry::Vacant(val) => { - let inserted = val.insert(TopicBridge::new( - ros_topic.clone(), - self.access.b_type, - self.access.declaration_interface.clone(), - self.access.ros1_client.clone(), - self.access.zenoh_client.clone(), - Environment::remote_bridging_mode().get(), - Environment::local_bridging_mode().get(), - )); - inserted.set_present_in_ros1(true).await; - smth_changed = true; + let b_mode = bridging_mode(self.access.b_type, ros_topic.name.as_str()); + if b_mode != BridgingMode::Disabled { + match self.access.container.entry(ros_topic.clone()) { + Entry::Occupied(_val) => { + debug_assert!(false); // that shouldn't happen + } + Entry::Vacant(val) => { + let inserted = val.insert(TopicBridge::new( + ros_topic.clone(), + self.access.b_type, + self.access.declaration_interface.clone(), + self.access.ros1_client.clone(), + self.access.zenoh_client.clone(), + b_mode, + )); + inserted.set_present_in_ros1(true).await; + smth_changed = true; + } } } } diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridging_mode.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridging_mode.rs new file mode 100644 index 0000000..0cadc71 --- /dev/null +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/bridging_mode.rs @@ -0,0 +1,70 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use serde::{Deserialize, Serialize}; +use strum_macros::{Display, EnumString}; + +use super::{bridge_type::BridgeType, environment::Environment}; + +#[derive(PartialEq, Eq, EnumString, Clone, Display)] +#[strum(serialize_all = "snake_case")] +#[derive(Serialize, Deserialize, Debug)] +pub enum BridgingMode { + LazyAuto, + Auto, + Disabled, +} + +pub(super) fn bridging_mode(entity_type: BridgeType, topic_name: &str) -> BridgingMode { + macro_rules! calcmode { + ($custom_modes:expr, $default_mode:expr, $topic_name:expr) => { + $custom_modes + .get() + .modes + .get($topic_name) + .map_or_else(|| $default_mode.get(), |v| v.clone()) + }; + } + + match entity_type { + BridgeType::Publisher => { + calcmode!( + Environment::publisher_topic_custom_bridging_mode(), + Environment::publisher_bridging_mode(), + topic_name + ) + } + BridgeType::Subscriber => { + calcmode!( + Environment::subscriber_topic_custom_bridging_mode(), + Environment::subscriber_bridging_mode(), + topic_name + ) + } + BridgeType::Service => { + calcmode!( + Environment::service_topic_custom_bridging_mode(), + Environment::service_bridging_mode(), + topic_name + ) + } + BridgeType::Client => { + calcmode!( + Environment::client_topic_custom_bridging_mode(), + Environment::client_bridging_mode(), + topic_name + ) + } + } +} diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs index 1d4d897..5ef56e5 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -13,12 +13,15 @@ // use duration_string::DurationString; +use log::error; use rosrust::api::resolve::*; +use serde_json::Value; +use std::collections::HashMap; use std::convert::From; use std::time::Duration; use std::{marker::PhantomData, str::FromStr}; -use super::topic_bridge::BridgingMode; +use super::bridging_mode::BridgingMode; #[derive(Clone)] pub struct Entry<'a, Tvar> @@ -73,6 +76,56 @@ impl<'a> From> for Entry<'a, String> { } } +impl<'a> From> for Entry<'a, String> { + fn from(item: Entry<'a, CustomBridgingModes>) -> Entry<'a, String> { + Entry::new(item.name, item.default.to_string()) + } +} + +#[derive(Clone, Default)] +pub struct CustomBridgingModes { + pub modes: HashMap, +} + +impl ToString for CustomBridgingModes { + fn to_string(&self) -> String { + todo!() + } +} + +impl FromStr for CustomBridgingModes { + type Err = serde_json::Error; + + fn from_str(s: &str) -> Result { + let mut result = CustomBridgingModes::default(); + let parsed: Value = serde_json::from_str(s)?; + match parsed.as_object() { + Some(v) => { + for (topic, mode) in v { + match mode.as_str() { + Some(v) => match BridgingMode::from_str(v) { + Ok(mode) => { + result.modes.insert(topic.clone(), mode); + } + Err(e) => { + error!("Error reading value {v} as Bridging Mode: {e}"); + } + }, + None => { + error!("Error reading non-string as Bridging Mode"); + } + } + } + } + None => { + error!("Error reading custom Bridging Mode map: the value provided is not a map!"); + } + } + + Ok(result) + } +} + pub struct Environment; impl Environment { pub fn ros_master_uri() -> Entry<'static, String> { @@ -95,12 +148,44 @@ impl Environment { return Entry::new("WITH_ROSMASTER", false); } - pub fn remote_bridging_mode() -> Entry<'static, BridgingMode> { - return Entry::new("ROS_REMOTE_BRIDGING_MODE", BridgingMode::Auto); + pub fn subscriber_bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("SUBSCRIBER_BRIDGING_MODE", BridgingMode::Auto); + } + pub fn subscriber_topic_custom_bridging_mode() -> Entry<'static, CustomBridgingModes> { + return Entry::new( + "SUBSCRIBER_TOPIC_CUSTOM_BRIDGING_MODE", + CustomBridgingModes::default(), + ); + } + + pub fn publisher_bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("PUBLISHER_BRIDGING_MODE", BridgingMode::Auto); + } + pub fn publisher_topic_custom_bridging_mode() -> Entry<'static, CustomBridgingModes> { + return Entry::new( + "PUBLISHER_TOPIC_CUSTOM_BRIDGING_MODE", + CustomBridgingModes::default(), + ); } - pub fn local_bridging_mode() -> Entry<'static, BridgingMode> { - return Entry::new("ROS_LOCAL_BRIDGING_MODE", BridgingMode::Auto); + pub fn service_bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("SERVICE_BRIDGING_MODE", BridgingMode::Auto); + } + pub fn service_topic_custom_bridging_mode() -> Entry<'static, CustomBridgingModes> { + return Entry::new( + "SERVICE_TOPIC_CUSTOM_BRIDGING_MODE", + CustomBridgingModes::default(), + ); + } + + pub fn client_bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("CLIENT_BRIDGING_MODE", BridgingMode::Disabled); + } + pub fn client_topic_custom_bridging_mode() -> Entry<'static, CustomBridgingModes> { + return Entry::new( + "CLIENT_TOPIC_CUSTOM_BRIDGING_MODE", + CustomBridgingModes::default(), + ); } pub fn master_polling_interval() -> Entry<'static, DurationString> { @@ -116,8 +201,14 @@ impl Environment { Self::ros_hostname(), Self::ros_name(), Self::ros_namespace(), - Self::remote_bridging_mode().into(), - Self::local_bridging_mode().into(), + Self::subscriber_bridging_mode().into(), + Self::publisher_bridging_mode().into(), + Self::service_bridging_mode().into(), + Self::client_bridging_mode().into(), + Self::subscriber_topic_custom_bridging_mode().into(), + Self::publisher_topic_custom_bridging_mode().into(), + Self::service_topic_custom_bridging_mode().into(), + Self::client_topic_custom_bridging_mode().into(), Self::master_polling_interval().into(), Self::with_rosmaster().into(), ] diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs index 0fb6eee..c612001 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs @@ -19,7 +19,7 @@ use async_std::{ use atoi::atoi; use log::error; use zenoh::plugins::ZResult; -use zenoh_core::{bail, zerror}; +use zenoh_core::{bail, zasynclock, zerror}; use crate::ros_to_zenoh_bridge::environment::Environment; @@ -37,7 +37,7 @@ impl Ros1MasterCtrl { let port = atoi::(port_str.as_bytes()).ok_or("Unable to parse port from ros_master_uri!")?; - let mut locked = ROSMASTER.lock().await; + let mut locked = zasynclock!(ROSMASTER); assert!(locked.is_none()); let child = Command::new("rosmaster") .arg(format!("-p {}", port).as_str()) @@ -50,7 +50,7 @@ impl Ros1MasterCtrl { } pub async fn without_ros1_master() { - let mut locked = ROSMASTER.lock().await; + let mut locked = zasynclock!(ROSMASTER); assert!(locked.is_some()); match locked.take() { Some(mut child) => match child.kill() { diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index 7c90913..a20fa1e 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -15,7 +15,7 @@ use async_std::sync::Mutex; use zenoh; -use zenoh_core::zresult::ZResult; +use zenoh_core::{zasynclock, zresult::ZResult}; use std::{ sync::{ @@ -99,9 +99,7 @@ async fn make_discovery<'a>( .on_discovered(move |b_type, topic| { let bridges = bridges.clone(); Box::new(Box::pin(async move { - bridges - .lock() - .await + zasynclock!(bridges) .bridges() .complementary_for(b_type) .complementary_entity_discovered(topic) @@ -111,9 +109,7 @@ async fn make_discovery<'a>( .on_lost(move |b_type, topic| { let bridges = bridges2.clone(); Box::new(Box::pin(async move { - bridges - .lock() - .await + zasynclock!(bridges) .bridges() .complementary_for(b_type) .complementary_entity_lost(topic) @@ -177,7 +173,7 @@ where let smth_changed; { - let mut locked = bridges.lock().await; + let mut locked = zasynclock!(bridges); smth_changed = locked.receive_ros1_state(&mut ros1_state_val).await; self.report_bridge_statistics(&locked); } @@ -196,7 +192,7 @@ where self.transit_ros_status(RosStatus::Error); { - let mut locked = bridges.lock().await; + let mut locked = zasynclock!(bridges); Self::cleanup(&mut locked); self.report_bridge_statistics(&locked); } diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs index 3895bc5..f6d7458 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -20,6 +20,7 @@ use std::sync::{Arc, Mutex, RwLock}; use std::{net::SocketAddr, str::FromStr, sync::atomic::AtomicU16}; use zenoh::config::ModeDependentValue; use zenoh::sample::Sample; +use zenoh_core::zlock; use zenoh_core::{AsyncResolve, SyncResolve}; use super::discovery::LocalResources; @@ -127,11 +128,11 @@ impl RunningBridge { session, flag, move |v| { - let mut val = ros_status.lock().unwrap(); + let mut val = zlock!(ros_status); *val = v; }, move |status| { - let mut my_status = bridge_status.lock().unwrap(); + let mut my_status = zlock!(bridge_status); *my_status = status; }, ) diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs index cc1b8c4..6efd60c 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -15,19 +15,12 @@ use super::{ abstract_bridge::AbstractBridge, bridge_type::BridgeType, + bridging_mode::BridgingMode, discovery::{LocalResource, LocalResources}, ros1_client, zenoh_client, }; use log::error; use std::{fmt::Display, sync::Arc}; -use strum_macros::{Display, EnumString}; - -#[derive(PartialEq, Eq, EnumString, Clone, Display)] -#[strum(serialize_all = "snake_case")] -pub enum BridgingMode { - Lazy, - Auto, -} pub struct TopicBridge { topic: rosrust::api::Topic, @@ -35,8 +28,7 @@ pub struct TopicBridge { ros1_client: Arc, zenoh_client: Arc, - remote_bridging_mode: BridgingMode, - local_bridging_mode: BridgingMode, + bridging_mode: BridgingMode, required_on_ros1_side: bool, required_on_zenoh_side: bool, @@ -59,16 +51,14 @@ impl TopicBridge { declaration_interface: Arc, ros1_client: Arc, zenoh_client: Arc, - remote_bridging_mode: BridgingMode, - local_bridging_mode: BridgingMode, + bridging_mode: BridgingMode, ) -> Self { Self { topic, b_type, ros1_client, zenoh_client, - remote_bridging_mode, - local_bridging_mode, + bridging_mode, required_on_ros1_side: false, required_on_zenoh_side: false, declaration_interface, @@ -134,9 +124,8 @@ impl TopicBridge { let is_required = is_discovered_client || match (self.required_on_zenoh_side, self.required_on_ros1_side) { (true, true) => true, - (true, false) => self.remote_bridging_mode == BridgingMode::Auto, - (false, true) => self.local_bridging_mode == BridgingMode::Auto, (false, false) => false, + (_, _) => self.bridging_mode == BridgingMode::Auto, }; match (is_required, self.bridge.as_ref()) { diff --git a/zenoh-plugin-ros1/tests/bridge_to_bridge.rs b/zenoh-plugin-ros1/tests/bridge_to_bridge.rs index 2e24ed0..6fb0db3 100644 --- a/zenoh-plugin-ros1/tests/bridge_to_bridge.rs +++ b/zenoh-plugin-ros1/tests/bridge_to_bridge.rs @@ -18,9 +18,13 @@ use rosrust::{Client, Publisher, RawMessage, Service, Subscriber}; use std::sync::atomic::Ordering::*; use strum_macros::Display; use zenoh::prelude::KeyExpr; -use zenoh_plugin_ros1::ros_to_zenoh_bridge::test_helpers::{ - BridgeChecker, IsolatedConfig, IsolatedROSMaster, RAIICounter, ROSEnvironment, RunningBridge, - TestParams, +use zenoh_plugin_ros1::ros_to_zenoh_bridge::{ + bridging_mode::BridgingMode, + environment::Environment, + test_helpers::{ + BridgeChecker, IsolatedConfig, IsolatedROSMaster, RAIICounter, ROSEnvironment, + RunningBridge, TestParams, + }, }; struct ROSWithChecker { @@ -233,6 +237,9 @@ async fn async_bridge_2_bridge(instances: u32, mode: std::collections::HashSet Date: Mon, 11 Sep 2023 09:34:41 +0300 Subject: [PATCH 4/7] Fix config and command line arguments --- zenoh-bridge-ros1/src/main.rs | 52 ++++++++++++++++--- .../src/ros_to_zenoh_bridge/environment.rs | 6 ++- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/zenoh-bridge-ros1/src/main.rs b/zenoh-bridge-ros1/src/main.rs index fcd5933..a956421 100644 --- a/zenoh-bridge-ros1/src/main.rs +++ b/zenoh-bridge-ros1/src/main.rs @@ -94,22 +94,28 @@ r#"--ros_hostname=[String] 'A hostname to send to ROS1 Master, the default is r#"--ros_name=[String] 'A bridge node's name for ROS1, the default is "ros1_to_zenoh_bridge"'"# )) .arg(Arg::from_usage( +r#"--ros_namespace=[String] 'A bridge's namespace in terms of ROS1, the default is empty'"# + )) + .arg(Arg::from_usage( +r#"--with_rosmaster=[bool] 'Start rosmaster with the bridge, the default is "false"'"# + )) + .arg(Arg::from_usage( r#"--subscriber_bridging_mode=[String] \ -'Mode of subscriber's topic bridging. Accepted values:' +'Global subscriber's topic bridging mode. Accepted values:' - "auto"(default) - bridge topics once they are declared locally or discovered remotely - "lazy_auto" - bridge topics once they are both declared locally and discovered remotely - "disabled" - never bridge topics. This setting will also suppress the topic discovery."# )) .arg(Arg::from_usage( r#"--publisher_bridging_mode=[String] \ -'Mode of publisher's topic bridging. Accepted values:' +'Global publisher's topic bridging mode. Accepted values:' - "auto"(default) - bridge topics once they are declared locally or discovered remotely - "lazy_auto" - bridge topics once they are both declared locally and discovered remotely - "disabled" - never bridge topics. This setting will also suppress the topic discovery."# )) .arg(Arg::from_usage( r#"--service_bridging_mode=[String] \ -'Mode of service's topic bridging. Accepted values:' +'Global service's topic bridging mode. Accepted values:' - "auto"(default) - bridge topics once they are declared locally or discovered remotely - "lazy_auto" - bridge topics once they are both declared locally and discovered remotely - "disabled" - never bridge topics. This setting will also suppress the topic discovery."# @@ -129,7 +135,42 @@ r#"--client_bridging_mode=[String] \ - globally select auto bridging mode (with caution!) with this option - bridge specific topics using 'client_topic_custom_bridging_mode' option (with a little bit less caution!)"# )) - + .arg(Arg::from_usage( +r#"--subscriber_topic_custom_bridging_mode=[JSON] 'A JSON Map describing custom bridging modes for particular topics. +Custom bridging mode overrides the global one. +Format: {[topic, mode]} +where +- topic: ROS1 topic name +- mode (auto/lazy_auto/disabled) as described above +The default is empty'"# + )) + .arg(Arg::from_usage( +r#"--publisher_topic_custom_bridging_mode=[JSON] 'A JSON Map describing custom bridging modes for particular topics. +Custom bridging mode overrides the global one. +Format: {[topic, mode]} +where +- topic: ROS1 topic name +- mode (auto/lazy_auto/disabled) as described above +The default is empty'"# + )) + .arg(Arg::from_usage( +r#"--service_topic_custom_bridging_mode=[JSON] 'A JSON Map describing custom bridging modes for particular topics. +Custom bridging mode overrides the global one. +Format: {[topic, mode]} +where +- topic: ROS1 topic name +- mode (auto/lazy_auto/disabled) as described above +The default is empty'"# + )) + .arg(Arg::from_usage( +r#"--client_topic_custom_bridging_mode=[JSON] 'A JSON Map describing custom bridging modes for particular topics. +Custom bridging mode overrides the global one. +Format: {[topic, mode]} +where +- topic: ROS1 topic name +- mode (auto/lazy_auto/disabled) as described above +The default is empty'"# + )) .arg(Arg::from_usage( r#"--ros_master_polling_interval=[String] \ 'An interval to poll the ROS1 master for status @@ -137,9 +178,6 @@ Bridge polls ROS1 master to get information on local topics. This option is the Accepted value:' A string such as 100ms, 2s, 5m The string format is [0-9]+(ns|us|ms|[smhdwy])"# - )) - .arg(Arg::from_usage( -r#"--with_rosmaster=[bool] 'An option wether the bridge should run it's own rosmaster process, the default is "false"'"# )); let args = app.get_matches(); diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs index 5ef56e5..23f8188 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -89,7 +89,11 @@ pub struct CustomBridgingModes { impl ToString for CustomBridgingModes { fn to_string(&self) -> String { - todo!() + let mut json_map = serde_json::Map::new(); + for (k, v) in &self.modes { + json_map.insert(k.clone(), Value::String(v.to_string())); + } + serde_json::Value::Object(json_map).to_string() } } From 2dde46d2d4582c2ddaee088e66276293965ed167 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 11 Sep 2023 09:57:28 +0300 Subject: [PATCH 5/7] Enable previously hidden release target platforms --- .github/workflows/release.yml | 61 ++++++++++++++++++----------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 8e42b03..34d2cc7 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -93,42 +93,43 @@ jobs: - { target: x86_64-unknown-linux-gnu, arch: amd64, os: ubuntu-20.04 } - { target: x86_64-apple-darwin, arch: darwin, os: macos-latest } - { target: aarch64-apple-darwin, arch: darwin, os: macos-latest } - # - { - # target: x86_64-unknown-linux-musl, - # arch: amd64, - # os: ubuntu-20.04, - # use-cross: true, - # } - # - { - # target: arm-unknown-linux-gnueabi, - # arch: armel, - # os: ubuntu-20.04, - # use-cross: true, - # } + - { + target: x86_64-unknown-linux-musl, + arch: amd64, + os: ubuntu-20.04, + use-cross: true, + } + - { + target: arm-unknown-linux-gnueabi, + arch: armel, + os: ubuntu-20.04, + use-cross: true, + } - { target: arm-unknown-linux-gnueabihf, arch: armhf, os: ubuntu-20.04, use-cross: true, } - # - { - # target: armv7-unknown-linux-gnueabihf, - # arch: armhf, - # os: ubuntu-20.04, - # use-cross: true, - # } - # - { - # target: aarch64-unknown-linux-gnu, - # arch: arm64, - # os: ubuntu-20.04, - # use-cross: true, - # } - # - { - # target: aarch64-unknown-linux-musl, - # arch: arm64, - # os: ubuntu-20.04, - # use-cross: true, - # } + - { + target: armv7-unknown-linux-gnueabihf, + arch: armhf, + os: ubuntu-20.04, + use-cross: true, + } + - { + target: aarch64-unknown-linux-gnu, + arch: arm64, + os: ubuntu-20.04, + use-cross: true, + } + - { + target: aarch64-unknown-linux-musl, + arch: arm64, + os: ubuntu-20.04, + use-cross: true, + } + # win is possible to build on, but we don't target it for now # - { target: x86_64-pc-windows-msvc, arch: win64, os: windows-2019 } # - { target: x86_64-pc-windows-gnu , arch: win64 , os: windows-2019 } steps: From 2c7b707e5733b2ee9aeaac649b51104ab3df8f58 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 11 Sep 2023 10:40:11 +0300 Subject: [PATCH 6/7] Update Cargo.lock to use proper rosrust's for commit --- Cargo.lock | 216 ++++++++++++++++++++++++++++------------------------- 1 file changed, 113 insertions(+), 103 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index f28e15d..61d2c8a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -56,9 +56,9 @@ dependencies = [ [[package]] name = "aho-corasick" -version = "1.0.4" +version = "1.0.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6748e8def348ed4d14996fa801f4122cd763fff530258cdc03f64b25f89d3a5a" +checksum = "0c378d78423fdad8089616f827526ee33c19f2fddbd5de1629152c9593ba4783" dependencies = [ "memchr", ] @@ -261,7 +261,7 @@ checksum = "bc00ceb34980c03614e35a3a4e218276a0a824e911d07651cd0d858a51e8c0f0" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] @@ -339,9 +339,9 @@ checksum = "9e1b586273c5702936fe7b7d6896644d8be71e6314cfe09d3167c95f712589e8" [[package]] name = "base64" -version = "0.21.2" +version = "0.21.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "604178f6c5c21f02dc555784810edfb88d34ac2c73b2eae109655649ee73ce3d" +checksum = "9ba43ea6f343b788c8764558649e08df62f86c6ef251fdaeb1ffd010a9ae50a2" [[package]] name = "base64ct" @@ -460,9 +460,9 @@ checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" [[package]] name = "bytes" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "89b2fd2a0dcf38d7971e2194b6b6eebab45ae01067456a7fd93d5547a61b70be" +checksum = "a2bd12c1caf447e69cd4528f47f94d203fd2582878ecb9e9465484c4148a8223" [[package]] name = "cache-padded" @@ -487,14 +487,14 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "chrono" -version = "0.4.26" +version = "0.4.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec837a71355b28f6556dbd569b37b3f363091c0bd4b2e735674521b4c5fd9bc5" +checksum = "defd4e7873dbddba6c7c91e199c7fcb946abc4a6a4ac3195400bcfb01b5de877" dependencies = [ "android-tzdata", "iana-time-zone", "num-traits", - "winapi", + "windows-targets", ] [[package]] @@ -696,19 +696,19 @@ dependencies = [ [[package]] name = "ctrlc" -version = "3.4.0" +version = "3.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2a011bbe2c35ce9c1f143b7af6f94f29a167beb4cd1d29e6740ce836f723120e" +checksum = "82e95fbd621905b854affdc67943b043a0fbb6ed7385fd5a25650d19a8a6cfdf" dependencies = [ - "nix", + "nix 0.27.1", "windows-sys", ] [[package]] name = "dashmap" -version = "5.5.1" +version = "5.5.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "edd72493923899c6f10c641bdbdeddc7183d6396641d99c1a0d1597f37f92e28" +checksum = "978747c1d849a7d2ee5e8adc0159961c48fb7e5db2f06af6723b80123bb53856" dependencies = [ "cfg-if", "hashbrown 0.14.0", @@ -813,7 +813,7 @@ dependencies = [ "atty", "humantime", "log 0.4.20", - "regex 1.9.3", + "regex 1.9.5", "termcolor", ] @@ -826,7 +826,7 @@ dependencies = [ "humantime", "is-terminal", "log 0.4.20", - "regex 1.9.3", + "regex 1.9.5", "termcolor", ] @@ -838,9 +838,9 @@ checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" [[package]] name = "errno" -version = "0.3.2" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6b30f669a7961ef1631673d2766cc92f52d64f7ef354d4fe0ddfd30ed52f0f4f" +checksum = "136526188508e25c6fef639d7927dfb3e0e3084488bf202267829cf7fc23dbdd" dependencies = [ "errno-dragonfly", "libc", @@ -1020,7 +1020,7 @@ checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] @@ -1365,7 +1365,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cb0889898416213fab133e1d33a0e5858a48177452750691bde3666d0fdbaf8b" dependencies = [ "hermit-abi 0.3.2", - "rustix 0.38.8", + "rustix 0.38.13", "windows-sys", ] @@ -1482,9 +1482,9 @@ checksum = "ef53942eb7bf7ff43a617b3e2c1c4a5ecf5944a7c1bc12d7ee39bbb15e5c1519" [[package]] name = "linux-raw-sys" -version = "0.4.5" +version = "0.4.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57bcfdad1b858c2db7c38303a6d2ad4dfaf5eb53dfeb0910128b2c26d6158503" +checksum = "1a9bad9f94746442c783ca431b22403b519cd7fbeed0533fdd6328b2f2212128" [[package]] name = "lock_api" @@ -1548,9 +1548,9 @@ dependencies = [ [[package]] name = "memchr" -version = "2.5.0" +version = "2.6.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" +checksum = "8f232d6ef707e1956a43342693d2a31e72989554d58299d7a88738cc95b0d35c" [[package]] name = "memoffset" @@ -1650,16 +1650,26 @@ dependencies = [ [[package]] name = "nix" -version = "0.26.2" +version = "0.26.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bfdda3d196821d6af13126e40375cdf7da646a96114af134d5f417a9a1dc8e1a" +checksum = "598beaf3cc6fdd9a5dfb1630c2800c7acd31df7aaf0f565796fba2b53ca1af1b" dependencies = [ "bitflags 1.3.2", "cfg-if", "libc", "memoffset 0.7.1", "pin-utils", - "static_assertions", +] + +[[package]] +name = "nix" +version = "0.27.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2eb04e9c688eff1c89d72b407f168cf79bb9e867a9d3323ed6c01519eb9cc053" +dependencies = [ + "bitflags 2.4.0", + "cfg-if", + "libc", ] [[package]] @@ -1737,9 +1747,9 @@ dependencies = [ [[package]] name = "object" -version = "0.32.0" +version = "0.32.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "77ac5bbd07aea88c60a577a1ce218075ffd59208b2d7ca97adf9bfc5aeb21ebe" +checksum = "9cf5f9dd3933bd50a9e1f149ec995f39ae2c496d31fd772c1fd45ebc27e902b0" dependencies = [ "memchr", ] @@ -1841,19 +1851,20 @@ checksum = "9b2a4787296e9989611394c33f193f676704af1686e70b8f8033ab5ba9a35a94" [[package]] name = "pest" -version = "2.7.2" +version = "2.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1acb4a4365a13f749a93f1a094a7805e5cfa0955373a9de860d962eaa3a5fe5a" +checksum = "d7a4d085fd991ac8d5b05a147b437791b4260b76326baf0fc60cf7c9c27ecd33" dependencies = [ + "memchr", "thiserror", "ucd-trie", ] [[package]] name = "pest_derive" -version = "2.7.2" +version = "2.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "666d00490d4ac815001da55838c500eafb0320019bbaa44444137c48b443a853" +checksum = "a2bee7be22ce7918f641a33f08e3f43388c7656772244e2bbb2477f44cc9021a" dependencies = [ "pest", "pest_generator", @@ -1861,22 +1872,22 @@ dependencies = [ [[package]] name = "pest_generator" -version = "2.7.2" +version = "2.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "68ca01446f50dbda87c1786af8770d535423fa8a53aec03b8f4e3d7eb10e0929" +checksum = "d1511785c5e98d79a05e8a6bc34b4ac2168a0e3e92161862030ad84daa223141" dependencies = [ "pest", "pest_meta", "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] name = "pest_meta" -version = "2.7.2" +version = "2.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "56af0a30af74d0445c0bf6d9d051c979b516a1a5af790d251daee76005420a48" +checksum = "b42f0394d3123e33353ca5e1e89092e533d2cc490389f2bd6131c43c634ebc5f" dependencies = [ "once_cell", "pest", @@ -1910,14 +1921,14 @@ checksum = "4359fd9c9171ec6e8c62926d6faaf553a8dc3f64e1507e76da7911b4f6a04405" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] name = "pin-project-lite" -version = "0.2.12" +version = "0.2.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "12cc1b0bf1727a77a54b6654e7b5f1af8604923edc8b81885f8ec92f9e3f0a05" +checksum = "8afb450f006bf6385ca15ef45d71d2288452bc3683ce2e2cacc0d18e4be60b58" [[package]] name = "pin-utils" @@ -1991,7 +2002,7 @@ checksum = "2a780e80005c2e463ec25a6e9f928630049a10b43945fea83207207d4a7606f4" dependencies = [ "proc-macro2", "quote", - "regex 1.9.3", + "regex 1.9.5", "syn 1.0.109", ] @@ -2212,25 +2223,25 @@ dependencies = [ [[package]] name = "regex" -version = "1.9.3" +version = "1.9.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "81bc1d4caf89fac26a70747fe603c130093b53c773888797a6329091246d651a" +checksum = "697061221ea1b4a94a624f67d0ae2bfe4e22b8a17b6a192afb11046542cc8c47" dependencies = [ - "aho-corasick 1.0.4", + "aho-corasick 1.0.5", "memchr", "regex-automata", - "regex-syntax 0.7.4", + "regex-syntax 0.7.5", ] [[package]] name = "regex-automata" -version = "0.3.6" +version = "0.3.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fed1ceff11a1dddaee50c9dc8e4938bd106e9d89ae372f192311e7da498e3b69" +checksum = "c2f401f4955220693b56f8ec66ee9c78abffd8d1c4f23dc41a23839eb88f0795" dependencies = [ - "aho-corasick 1.0.4", + "aho-corasick 1.0.5", "memchr", - "regex-syntax 0.7.4", + "regex-syntax 0.7.5", ] [[package]] @@ -2244,9 +2255,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.7.4" +version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e5ea92a5b6195c6ef2a0295ea818b312502c6fc94dde986c5553242e18fd4ce2" +checksum = "dbb5fb1acd8a1a18b3dd5be62d25485eb770e05afb408a9627d14d451bae12da" [[package]] name = "ring" @@ -2275,15 +2286,15 @@ dependencies = [ [[package]] name = "ros_message" -version = "0.1.0" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#3ff335ae909e494578d2c9b5d020c5ade58f39cf" +version = "0.1.1" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#e21ec5b0b6c50e5fe8dc032e12f60142056643bf" dependencies = [ "array-init", "hex", "itertools", "lazy_static", "md-5", - "regex 1.9.3", + "regex 1.9.5", "serde", "serde_derive", "thiserror", @@ -2291,8 +2302,8 @@ dependencies = [ [[package]] name = "rosrust" -version = "0.9.10" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#3ff335ae909e494578d2c9b5d020c5ade58f39cf" +version = "0.9.11" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#e21ec5b0b6c50e5fe8dc032e12f60142056643bf" dependencies = [ "byteorder", "colored", @@ -2302,7 +2313,7 @@ dependencies = [ "hostname", "lazy_static", "log 0.4.20", - "regex 1.9.3", + "regex 1.9.5", "ros_message", "rosrust_codegen", "serde", @@ -2315,7 +2326,7 @@ dependencies = [ [[package]] name = "rosrust_codegen" version = "0.9.6" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#3ff335ae909e494578d2c9b5d020c5ade58f39cf" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#e21ec5b0b6c50e5fe8dc032e12f60142056643bf" dependencies = [ "error-chain 0.12.4", "hex", @@ -2346,9 +2357,9 @@ dependencies = [ "serde_json", "sha1_smol", "threadpool", - "time 0.3.27", + "time 0.3.28", "tiny_http", - "url 2.4.0", + "url 2.4.1", ] [[package]] @@ -2408,22 +2419,22 @@ dependencies = [ [[package]] name = "rustix" -version = "0.38.8" +version = "0.38.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "19ed4fa021d81c8392ce04db050a3da9a60299050b7ae1cf482d862b54a7218f" +checksum = "d7db8590df6dfcd144d22afd1b83b36c21a18d7cbc1dc4bb5295a8712e9eb662" dependencies = [ "bitflags 2.4.0", "errno", "libc", - "linux-raw-sys 0.4.5", + "linux-raw-sys 0.4.7", "windows-sys", ] [[package]] name = "rustls" -version = "0.21.6" +version = "0.21.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d1feddffcfcc0b33f5c6ce9a29e341e4cd59c3f78e7ee45f4a40c038b1d6cbb" +checksum = "cd8d6c9f025a446bc4d18ad9632e69aec8f287aa84499ee335599fabd20c3fd8" dependencies = [ "log 0.4.20", "ring", @@ -2449,7 +2460,7 @@ version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2d3987094b1d07b653b7dfdc3f70ce9a1da9c51ac18c1b06b662e4f9a0e9f4b2" dependencies = [ - "base64 0.21.2", + "base64 0.21.4", ] [[package]] @@ -2497,9 +2508,9 @@ dependencies = [ [[package]] name = "schemars" -version = "0.8.12" +version = "0.8.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "02c613288622e5f0c3fdc5dbd4db1c5fbe752746b1d1a56a0630b78fd00de44f" +checksum = "763f8cd0d4c71ed8389c90cb8100cba87e763bd01a8e614d4f0af97bcd50a161" dependencies = [ "dyn-clone", "schemars_derive", @@ -2509,9 +2520,9 @@ dependencies = [ [[package]] name = "schemars_derive" -version = "0.8.12" +version = "0.8.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "109da1e6b197438deb6db99952990c7f959572794b80ff93707d55a232545e7c" +checksum = "ec0f696e21e10fa546b7ffb1c9672c6de8fbc7a81acf59524386d8639bf12737" dependencies = [ "proc-macro2", "quote", @@ -2566,9 +2577,9 @@ checksum = "b0293b4b29daaf487284529cc2f5675b8e57c61f70167ba415a463651fd6a918" [[package]] name = "serde" -version = "1.0.186" +version = "1.0.188" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9f5db24220c009de9bd45e69fb2938f4b6d2df856aa9304ce377b3180f83b7c1" +checksum = "cf9e0fcba69a370eed61bcf2b728575f726b50b55cba78064753d708ddc7549e" dependencies = [ "serde_derive", ] @@ -2595,13 +2606,13 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.186" +version = "1.0.188" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5ad697f7e0b65af4983a4ce8f56ed5b357e8d3c36651bf6a7e13639c17b8e670" +checksum = "4eca7ac642d82aa35b60049a6eccb4be6be75e599bd2e9adb5f875a737654af2" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] @@ -2617,9 +2628,9 @@ dependencies = [ [[package]] name = "serde_json" -version = "1.0.105" +version = "1.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "693151e1ac27563d6dbcec9dee9fbd5da8539b20fa14ad3752b2e6d363ace360" +checksum = "2cc66a619ed80bf7a0f6b17dd063a84b88f6dea1813737cf469aef1d081142c2" dependencies = [ "itoa", "ryu", @@ -2862,9 +2873,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.29" +version = "2.0.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c324c494eba9d92503e6f1ef2e6df781e78f6a7705a0202d9801b198807d518a" +checksum = "239814284fd6f1a4ffe4ca893952cdd93c224b6a1571c9a9eadd670295c0c9e2" dependencies = [ "proc-macro2", "quote", @@ -2880,7 +2891,7 @@ dependencies = [ "cfg-if", "fastrand 2.0.0", "redox_syscall 0.3.5", - "rustix 0.38.8", + "rustix 0.38.13", "windows-sys", ] @@ -2901,22 +2912,22 @@ checksum = "222a222a5bfe1bba4a77b45ec488a741b3cb8872e5e499451fd7d0129c9c7c3d" [[package]] name = "thiserror" -version = "1.0.47" +version = "1.0.48" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "97a802ec30afc17eee47b2855fc72e0c4cd62be9b4efe6591edde0ec5bd68d8f" +checksum = "9d6d7a740b8a666a7e828dd00da9c0dc290dff53154ea77ac109281de90589b7" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.47" +version = "1.0.48" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6bb623b56e39ab7dcd4b1b98bb6c8f8d907ed255b18de254088016b27a8ee19b" +checksum = "49922ecae66cc8a249b77e68d1d0623c1b2c514f0060c27cdc68bd62a1219d35" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] @@ -2950,9 +2961,9 @@ dependencies = [ [[package]] name = "time" -version = "0.3.27" +version = "0.3.28" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bb39ee79a6d8de55f48f2293a830e040392f1c5f16e336bdd1788cd0aadce07" +checksum = "17f6bb557fd245c28e6411aa56b6403c689ad95061f50e4be16c274e70a17e48" dependencies = [ "deranged", "libc", @@ -3028,7 +3039,7 @@ checksum = "630bdcf245f78637c13ec01ffae6187cca34625e8c63150d424b59e55af2675e" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] @@ -3064,7 +3075,7 @@ checksum = "5f4f31f56159e98206da9efd823404b79b6ef3143b4a7ab76e67b1751b25a4ab" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", ] [[package]] @@ -3097,7 +3108,7 @@ dependencies = [ "rand", "sha1", "thiserror", - "url 2.4.0", + "url 2.4.1", "utf-8", ] @@ -3146,11 +3157,10 @@ checksum = "abd2fc5d32b590614af8b0a20d837f32eca055edd0bbead59a9cfe80858be003" [[package]] name = "uhlc" -version = "0.6.0" +version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "af1438496174a601a35fb41bf9bc408e47384b1df52ddc1252e75ef0ab811322" +checksum = "91fd883bd1822917d4fcdff9bc1086f7fcf97c84ef6736185f6e1ff4c15da884" dependencies = [ - "hex", "humantime", "lazy_static", "log 0.4.20", @@ -3240,9 +3250,9 @@ dependencies = [ [[package]] name = "url" -version = "2.4.0" +version = "2.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "50bff7831e19200a85b17131d085c25d7811bc4e186efdaf54bbd132994a88cb" +checksum = "143b538f18257fac9cad154828a57c6bf5157e1aa604d4816b5995bf6de87ae5" dependencies = [ "form_urlencoded", "idna 0.4.0", @@ -3357,7 +3367,7 @@ dependencies = [ "once_cell", "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", "wasm-bindgen-shared", ] @@ -3391,7 +3401,7 @@ checksum = "54681b18a46765f095758388f2d0cf16eb8d4169b639ab575a8f5693af210c7b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.29", + "syn 2.0.32", "wasm-bindgen-backend", "wasm-bindgen-shared", ] @@ -3579,7 +3589,7 @@ dependencies = [ "async-global-executor", "async-std", "async-trait", - "base64 0.21.2", + "base64 0.21.4", "env_logger 0.10.0", "event-listener", "flume", @@ -3593,7 +3603,7 @@ dependencies = [ "paste", "petgraph", "rand", - "regex 1.9.3", + "regex 1.9.5", "rustc_version", "serde", "serde_json", @@ -3860,7 +3870,7 @@ dependencies = [ "async-trait", "futures 0.3.28", "log 0.4.20", - "nix", + "nix 0.26.4", "uuid", "zenoh-core", "zenoh-link-commons", @@ -3880,7 +3890,7 @@ dependencies = [ "log 0.4.20", "tokio", "tokio-tungstenite", - "url 2.4.0", + "url 2.4.1", "zenoh-core", "zenoh-link-commons", "zenoh-protocol", From 1c31ebd3b2d3c87420f64235f235cc7bdbfd9a8c Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 13 Sep 2023 21:00:20 +0400 Subject: [PATCH 7/7] - preserve datatype for all entities - many additional tests, especially for ros behaviour --- Cargo.lock | 78 ++++---- zenoh-bridge-ros1/Cargo.toml | 3 - zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs | 6 + .../ros_to_zenoh_bridge/abstract_bridge.rs | 24 ++- .../src/ros_to_zenoh_bridge/discovery.rs | 23 +-- .../src/ros_to_zenoh_bridge/ros1_client.rs | 31 ++- .../ros1_to_zenoh_bridge_impl.rs | 25 ++- .../rosclient_test_helpers.rs | 40 ++++ .../src/ros_to_zenoh_bridge/service_cache.rs | 90 +++++++++ .../src/ros_to_zenoh_bridge/test_helpers.rs | 2 +- .../src/ros_to_zenoh_bridge/topic_mapping.rs | 47 +++-- zenoh-plugin-ros1/tests/discovery_test.rs | 24 --- zenoh-plugin-ros1/tests/ros_test.rs | 121 ++++++++---- zenoh-plugin-ros1/tests/service_cache_test.rs | 181 ++++++++++++++++++ 14 files changed, 550 insertions(+), 145 deletions(-) create mode 100644 zenoh-plugin-ros1/src/ros_to_zenoh_bridge/rosclient_test_helpers.rs create mode 100644 zenoh-plugin-ros1/src/ros_to_zenoh_bridge/service_cache.rs create mode 100644 zenoh-plugin-ros1/tests/service_cache_test.rs diff --git a/Cargo.lock b/Cargo.lock index 61d2c8a..87c075b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1448,9 +1448,9 @@ dependencies = [ [[package]] name = "libc" -version = "0.2.147" +version = "0.2.148" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4668fb0ea861c1df094127ac5f1da3409a82116a4ba74fca2e58ef927159bb3" +checksum = "9cdc71e17332e86d2e1d38c1f99edcb6288ee11b815fb1a4b049eaa2114d369b" [[package]] name = "libloading" @@ -2135,7 +2135,7 @@ checksum = "055b4e778e8feb9f93c4e439f71dc2156ef13360b432b799e179a8c4cdf0b1d7" dependencies = [ "bytes", "libc", - "socket2 0.5.3", + "socket2 0.5.4", "tracing", "windows-sys", ] @@ -2287,7 +2287,7 @@ dependencies = [ [[package]] name = "ros_message" version = "0.1.1" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#e21ec5b0b6c50e5fe8dc032e12f60142056643bf" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#de0c837033ac1d442403141e0ff2fda5087f119a" dependencies = [ "array-init", "hex", @@ -2303,7 +2303,7 @@ dependencies = [ [[package]] name = "rosrust" version = "0.9.11" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#e21ec5b0b6c50e5fe8dc032e12f60142056643bf" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#de0c837033ac1d442403141e0ff2fda5087f119a" dependencies = [ "byteorder", "colored", @@ -2326,7 +2326,7 @@ dependencies = [ [[package]] name = "rosrust_codegen" version = "0.9.6" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#e21ec5b0b6c50e5fe8dc032e12f60142056643bf" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#de0c837033ac1d442403141e0ff2fda5087f119a" dependencies = [ "error-chain 0.12.4", "hex", @@ -2465,9 +2465,9 @@ dependencies = [ [[package]] name = "rustls-webpki" -version = "0.101.4" +version = "0.101.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7d93931baf2d282fff8d3a532bbfd7653f734643161b87e3e01e59a04439bf0d" +checksum = "45a27e3b59326c16e23d30aeb7a36a24cc0d29e71d68ff611cdfb4a01d013bed" dependencies = [ "ring", "untrusted", @@ -2778,9 +2778,9 @@ dependencies = [ [[package]] name = "socket2" -version = "0.5.3" +version = "0.5.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2538b18701741680e0322a2302176d3253a35388e2e62f172f64f4f16605f877" +checksum = "4031e820eb552adee9295814c0ced9e5cf38ddf1e8b7d566d6de8e2538ea989e" dependencies = [ "libc", "windows-sys", @@ -3026,7 +3026,7 @@ dependencies = [ "mio", "num_cpus", "pin-project-lite", - "socket2 0.5.3", + "socket2 0.5.4", "tokio-macros", "windows-sys", ] @@ -3195,9 +3195,9 @@ checksum = "92888ba5573ff080736b3648696b70cafad7d250551175acbaa4e0385b3e1460" [[package]] name = "unicode-ident" -version = "1.0.11" +version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "301abaae475aa91687eb82514b328ab47a211a533026cb25fc3e519b86adfc3c" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" [[package]] name = "unicode-normalization" @@ -3584,7 +3584,7 @@ dependencies = [ [[package]] name = "zenoh" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-global-executor", "async-std", @@ -3607,7 +3607,7 @@ dependencies = [ "rustc_version", "serde", "serde_json", - "socket2 0.5.3", + "socket2 0.5.4", "stop-token", "uhlc", "uuid", @@ -3647,7 +3647,7 @@ dependencies = [ [[package]] name = "zenoh-buffers" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "zenoh-collections", ] @@ -3655,7 +3655,7 @@ dependencies = [ [[package]] name = "zenoh-codec" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "log 0.4.20", "serde", @@ -3667,12 +3667,12 @@ dependencies = [ [[package]] name = "zenoh-collections" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" [[package]] name = "zenoh-config" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "flume", "json5", @@ -3690,7 +3690,7 @@ dependencies = [ [[package]] name = "zenoh-core" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "lazy_static", @@ -3700,7 +3700,7 @@ dependencies = [ [[package]] name = "zenoh-crypto" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "aes", "hmac", @@ -3713,7 +3713,7 @@ dependencies = [ [[package]] name = "zenoh-ext" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "bincode", @@ -3733,7 +3733,7 @@ dependencies = [ [[package]] name = "zenoh-keyexpr" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "hashbrown 0.13.2", "keyed-set", @@ -3747,7 +3747,7 @@ dependencies = [ [[package]] name = "zenoh-link" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", @@ -3766,7 +3766,7 @@ dependencies = [ [[package]] name = "zenoh-link-commons" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", @@ -3782,7 +3782,7 @@ dependencies = [ [[package]] name = "zenoh-link-quic" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-rustls", "async-std", @@ -3806,7 +3806,7 @@ dependencies = [ [[package]] name = "zenoh-link-tcp" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", @@ -3822,7 +3822,7 @@ dependencies = [ [[package]] name = "zenoh-link-tls" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-rustls", "async-std", @@ -3845,12 +3845,12 @@ dependencies = [ [[package]] name = "zenoh-link-udp" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", "log 0.4.20", - "socket2 0.5.3", + "socket2 0.5.4", "zenoh-buffers", "zenoh-collections", "zenoh-core", @@ -3864,7 +3864,7 @@ dependencies = [ [[package]] name = "zenoh-link-unixsock_stream" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", @@ -3882,7 +3882,7 @@ dependencies = [ [[package]] name = "zenoh-link-ws" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", @@ -3902,7 +3902,7 @@ dependencies = [ [[package]] name = "zenoh-macros" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "proc-macro2", "quote", @@ -3947,7 +3947,7 @@ dependencies = [ [[package]] name = "zenoh-plugin-trait" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "libloading", "log 0.4.20", @@ -3960,7 +3960,7 @@ dependencies = [ [[package]] name = "zenoh-protocol" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "const_format", "hex", @@ -3976,7 +3976,7 @@ dependencies = [ [[package]] name = "zenoh-result" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "anyhow", ] @@ -3984,7 +3984,7 @@ dependencies = [ [[package]] name = "zenoh-sync" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "event-listener", @@ -3999,7 +3999,7 @@ dependencies = [ [[package]] name = "zenoh-transport" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-executor", "async-global-executor", @@ -4030,7 +4030,7 @@ dependencies = [ [[package]] name = "zenoh-util" version = "0.10.0-dev" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#108dc59c18406cae1160e16aa51fd19f327a2377" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#a341d7d72acbcdf7e7650f1ff1444a113ddf5314" dependencies = [ "async-std", "async-trait", diff --git a/zenoh-bridge-ros1/Cargo.toml b/zenoh-bridge-ros1/Cargo.toml index 6ac8e00..b5787f7 100644 --- a/zenoh-bridge-ros1/Cargo.toml +++ b/zenoh-bridge-ros1/Cargo.toml @@ -34,9 +34,6 @@ zenoh = { workspace = true } zenoh-plugin-trait = { workspace = true } zenoh-plugin-ros1 = { path = "../zenoh-plugin-ros1/", default-features = false } -[features] -preserve_topic_metadata = [] # development feature, do not use it - [[bin]] name = "zenoh-bridge-ros1" path = "src/main.rs" diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs index df88c84..3a984f9 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge.rs @@ -40,6 +40,10 @@ pub mod ros1_client; #[cfg(feature = "test")] pub mod ros1_to_zenoh_bridge_impl; #[cfg(feature = "test")] +pub mod rosclient_test_helpers; +#[cfg(feature = "test")] +pub mod service_cache; +#[cfg(feature = "test")] pub mod test_helpers; #[cfg(feature = "test")] pub mod topic_utilities; @@ -61,6 +65,8 @@ mod ros1_client; #[cfg(not(feature = "test"))] mod ros1_to_zenoh_bridge_impl; #[cfg(not(feature = "test"))] +mod service_cache; +#[cfg(not(feature = "test"))] mod topic_utilities; #[cfg(not(feature = "test"))] mod zenoh_client; diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs index 7aacb69..a202de6 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs @@ -16,6 +16,7 @@ use std::sync::Arc; use log::{debug, error, info}; +use rosrust::RawMessageDescription; use zenoh::{plugins::ZResult, prelude::SplitBuffer}; use zenoh_core::{AsyncResolve, SyncResolve}; @@ -156,9 +157,14 @@ impl Ros1ToZenohService { match ros1_client.client(topic) { Ok(client) => { let client_in_arc = Arc::new(client); + let topic_in_arc = Arc::new(topic.clone()); let queryable = zenoh_client .make_queryable(make_zenoh_key(topic), move |query| { - async_std::task::spawn(Self::on_query(client_in_arc.clone(), query)); + async_std::task::spawn(Self::on_query( + client_in_arc.clone(), + query, + topic_in_arc.clone(), + )); }) .await?; Ok(Ros1ToZenohService { @@ -175,6 +181,7 @@ impl Ros1ToZenohService { async fn on_query( ros1_client: Arc>, query: zenoh::queryable::Query, + topic: Arc, ) { match query.value() { Some(val) => { @@ -183,7 +190,7 @@ impl Ros1ToZenohService { "ROS1 -> Zenoh Service: got query of {} bytes!", payload.len() ); - Self::process_query(ros1_client, query, payload).await; + Self::process_query(ros1_client, query, payload, topic).await; } None => { error!("ROS1 -> Zenoh Service: got query without value!"); @@ -195,12 +202,19 @@ impl Ros1ToZenohService { ros1_client: Arc>, query: zenoh::queryable::Query, payload: Vec, + topic: Arc, ) { // rosrust is synchronous, so we will use spawn_blocking. If there will be an async mode some day for the rosrust, // than reply_to_query can be refactored to async very easily - let res = - async_std::task::spawn_blocking(move || ros1_client.req(&rosrust::RawMessage(payload))) - .await; + let res = async_std::task::spawn_blocking(move || { + let description = RawMessageDescription { + msg_definition: String::from("*"), + md5sum: String::from("*"), + msg_type: topic.datatype.clone(), + }; + ros1_client.req_with_description(&rosrust::RawMessage(payload), description) + }) + .await; match Self::reply_to_query(res, &query).await { Ok(_) => {} Err(e) => { diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs index 7cb8bb7..0c5febf 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/discovery.rs @@ -144,22 +144,13 @@ impl RemoteResources { F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send>, { //let discovery_namespace = discovery.discovery_namespace().ok_or("No discovery_namespace present!")?; - let datatype; - #[cfg(feature = "preserve_topic_metadata")] - { - let datatype_bytes = hex::decode( - discovery - .data_type() - .ok_or("No data_type present!")? - .as_str(), - )?; - datatype = std::str::from_utf8(&datatype_bytes)?; - } - - #[cfg(not(feature = "preserve_topic_metadata"))] - { - datatype = "*"; - } + let datatype_bytes = hex::decode( + discovery + .data_type() + .ok_or("No data_type present!")? + .as_str(), + )?; + let datatype = std::str::from_utf8(&datatype_bytes)?; let resource_class = discovery .resource_class() diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs index a368cd7..e432fe9 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs @@ -13,7 +13,7 @@ // use log::debug; -use rosrust; +use rosrust::{self, RawMessageDescription}; use zenoh_core::{zerror, zresult::ZResult}; pub struct Ros1Client { @@ -43,14 +43,31 @@ impl Ros1Client { T: rosrust::Message, F: Fn(T) + Send + 'static, { - self.ros.subscribe(&topic.name, 0, callback) + let description = RawMessageDescription { + msg_definition: "*".to_string(), + md5sum: "*".to_string(), + msg_type: topic.datatype.clone(), + }; + self.ros.subscribe_with_ids_and_headers( + &topic.name, + 0, + move |data, _| callback(data), + |_| (), + Some(description), + ) } pub fn publish( &self, topic: &rosrust::api::Topic, ) -> rosrust::api::error::Result> { - self.ros.publish(&topic.name, 0) + let description = RawMessageDescription { + msg_definition: "*".to_string(), + md5sum: "*".to_string(), + msg_type: topic.datatype.clone(), + }; + self.ros + .publish_with_description(&topic.name, 0, description) } pub fn client( @@ -69,7 +86,13 @@ impl Ros1Client { T: rosrust::ServicePair, F: Fn(T::Request) -> rosrust::ServiceResult + Send + Sync + 'static, { - self.ros.service::(&topic.name, handler) + let description = RawMessageDescription { + msg_definition: "*".to_string(), + md5sum: "*".to_string(), + msg_type: topic.datatype.clone(), + }; + self.ros + .service_with_description::(&topic.name, handler, description) } pub fn state(&self) -> rosrust::api::error::Response { diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index a20fa1e..0270846 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -35,6 +35,7 @@ use crate::ros_to_zenoh_bridge::{ use super::{ discovery::{Discovery, DiscoveryBuilder}, ros1_client::Ros1Client, + service_cache::Ros1ServiceCache, }; #[derive(PartialEq, Clone, Copy)] @@ -63,6 +64,14 @@ where RosStatusCallback: Fn(RosStatus), BridgeStatisticsCallback: Fn(BridgeStatus), { + let ros1_service_cache = Ros1ServiceCache::new( + format!( + "{}_service_cache_node", + Environment::ros_name().get().as_str() + ) + .as_str(), + ros_master_uri, + )?; let ros1_client = Arc::new(ros1_client::Ros1Client::new( Environment::ros_name().get().as_str(), ros_master_uri, @@ -84,7 +93,9 @@ where let _discovery = make_discovery(session.clone(), bridges.clone()).await; let mut bridge = RosToZenohBridge::new(ros_status_callback, statistics_callback); - bridge.run(ros1_client, bridges, flag).await; + bridge + .run(ros1_client, ros1_service_cache, bridges, flag) + .await; Ok(()) } @@ -153,6 +164,7 @@ where pub async fn run( &mut self, ros1_client: Arc, + mut ros1_service_cache: Ros1ServiceCache, bridges: Arc>, flag: Arc, ) { @@ -160,10 +172,17 @@ where while flag.load(Relaxed) { let cl = ros1_client.clone(); - let ros1_state = async_std::task::spawn_blocking(move || { - topic_mapping::Ros1TopicMapping::topic_mapping(cl.as_ref()) + let (ros1_state, returned_cache) = async_std::task::spawn_blocking(move || { + ( + topic_mapping::Ros1TopicMapping::topic_mapping( + cl.as_ref(), + &mut ros1_service_cache, + ), + ros1_service_cache, + ) }) .await; + ros1_service_cache = returned_cache; debug!("ros state: {:#?}", ros1_state); diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/rosclient_test_helpers.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/rosclient_test_helpers.rs new file mode 100644 index 0000000..7c2bcc3 --- /dev/null +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/rosclient_test_helpers.rs @@ -0,0 +1,40 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::time::Duration; + +use rosrust::{Publisher, RawMessage, Subscriber}; + +use super::{ros1_client::Ros1Client, test_helpers::wait}; + +pub fn wait_for_rosclient_to_connect(rosclient: &Ros1Client) -> bool { + async_std::task::block_on(wait( + || rosclient.topic_types().is_ok(), + Duration::from_secs(10), + )) +} + +pub fn wait_for_publishers(subscriber: &Subscriber, count: usize) -> bool { + async_std::task::block_on(wait( + || subscriber.publisher_count() == count, + Duration::from_secs(10), + )) +} + +pub fn wait_for_subscribers(publisher: &Publisher, count: usize) -> bool { + async_std::task::block_on(wait( + || publisher.subscriber_count() == count, + Duration::from_secs(10), + )) +} diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/service_cache.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/service_cache.rs new file mode 100644 index 0000000..bdc0aa2 --- /dev/null +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/service_cache.rs @@ -0,0 +1,90 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use rosrust::api::Topic; +use std::{ + collections::{hash_map::Entry, HashMap}, + time::Duration, +}; +use zenoh_core::{zerror, zresult::ZResult}; + +use super::ros1_client::Ros1Client; + +type ServiceName = String; +type NodeName = String; +type DataType = String; + +/** + * Ros1ServiceCache - caching resolver for service's data type + */ +pub struct Ros1ServiceCache { + aux_node: Ros1Client, + cache: HashMap>, +} + +impl Ros1ServiceCache { + pub fn new(aux_ros_node_name: &str, ros_master_uri: &str) -> ZResult { + let aux_node = Ros1Client::new(aux_ros_node_name, ros_master_uri)?; + Ok(Ros1ServiceCache { + aux_node, + cache: HashMap::default(), + }) + } + + pub fn resolve_datatype(&mut self, service: ServiceName, node: NodeName) -> ZResult { + match self.cache.entry(service.clone()) { + Entry::Occupied(mut occupied) => { + match occupied.get_mut().entry(node) { + Entry::Occupied(occupied) => { + // return already resolved datatype + Ok(occupied.get().clone()) + } + Entry::Vacant(vacant) => { + // actively resolve datatype through network + let datatype = Self::resolve(&self.aux_node, service)?; + + // save in cache and return datatype + Ok(vacant.insert(datatype).clone()) + } + } + } + Entry::Vacant(vacant) => { + // actively resolve datatype through network + let datatype = Self::resolve(&self.aux_node, service)?; + + // save in cache and return datatype + let mut node_map = HashMap::new(); + node_map.insert(node, datatype.clone()); + vacant.insert(node_map); + Ok(datatype) + } + } + } + + fn resolve(aux_node: &Ros1Client, service: ServiceName) -> ZResult { + let resolving_client = aux_node + .client(&Topic { + name: service, + datatype: "*".to_string(), + }) + .map_err(|e| zerror!("{e}"))?; + let mut probe = resolving_client + .probe(Duration::from_secs(1)) + .map_err(|e| zerror!("{e}"))?; + let datatype = probe + .remove("type") + .ok_or("no data_type field in service's responce!")?; + Ok(datatype) + } +} diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs index f6d7458..1ba7fa8 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -390,7 +390,7 @@ impl BridgeChecker { } pub fn make_topic(name: &str) -> rosrust::api::Topic { - topic_utilities::make_topic("some/very.complicated/datatype//", name.try_into().unwrap()) + topic_utilities::make_topic("some/testdatatype", name.try_into().unwrap()) } pub fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { diff --git a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs index 5c55090..5643987 100644 --- a/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs +++ b/zenoh-plugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs @@ -17,6 +17,8 @@ use std::collections::HashSet; use crate::ros_to_zenoh_bridge::ros1_client; use log::debug; +use super::service_cache::Ros1ServiceCache; + #[derive(Debug)] pub struct Ros1TopicMapping { pub published: HashSet, @@ -26,12 +28,17 @@ pub struct Ros1TopicMapping { impl Ros1TopicMapping { pub fn topic_mapping( ros1_client: &ros1_client::Ros1Client, + ros1_service_cache: &mut Ros1ServiceCache, ) -> rosrust::api::error::Response { match ros1_client.state() { Ok(state_val) => match ros1_client.topic_types() { Ok(topics_val) => { debug!("topics: {:#?}", topics_val); - Ok(Ros1TopicMapping::new(&state_val, &topics_val)) + Ok(Ros1TopicMapping::new( + state_val, + &topics_val, + ros1_service_cache, + )) } Err(e) => Err(e), }, @@ -40,7 +47,11 @@ impl Ros1TopicMapping { } // PRIVATE: - fn new(state: &rosrust::api::SystemState, topics: &[rosrust::api::Topic]) -> Ros1TopicMapping { + fn new( + state: rosrust::api::SystemState, + topics: &[rosrust::api::Topic], + ros1_service_cache: &mut Ros1ServiceCache, + ) -> Ros1TopicMapping { let mut result = Ros1TopicMapping { published: HashSet::new(), subscribed: HashSet::new(), @@ -49,12 +60,11 @@ impl Ros1TopicMapping { Ros1TopicMapping::fill(&mut result.subscribed, &state.subscribers, topics); Ros1TopicMapping::fill(&mut result.published, &state.publishers, topics); - Ros1TopicMapping::fill(&mut result.serviced, &state.services, topics); + Ros1TopicMapping::fill_services(&mut result.serviced, state.services, ros1_service_cache); result } - #[cfg(feature = "preserve_topic_metadata")] fn fill( dst: &mut HashSet, data: &[rosrust::api::TopicData], @@ -77,17 +87,28 @@ impl Ros1TopicMapping { } } - #[cfg(not(feature = "preserve_topic_metadata"))] - fn fill( + fn fill_services( dst: &mut HashSet, - data: &[rosrust::api::TopicData], - _topics: &[rosrust::api::Topic], + data: Vec, + ros1_service_cache: &mut Ros1ServiceCache, ) { - for item in data.iter() { - dst.insert(rosrust::api::Topic { - name: item.name.clone(), - datatype: "*".to_string(), - }); + for service in data { + for node in service.connections { + match ros1_service_cache.resolve_datatype(service.name.clone(), node) { + Ok(datatype) => { + dst.insert(rosrust::api::Topic { + name: service.name.clone(), + datatype, + }); + } + Err(e) => { + debug!( + "Error finding datatype for service topic {}: {e}", + service.name.clone() + ); + } + } + } } } } diff --git a/zenoh-plugin-ros1/tests/discovery_test.rs b/zenoh-plugin-ros1/tests/discovery_test.rs index 67c4016..ee0d01e 100644 --- a/zenoh-plugin-ros1/tests/discovery_test.rs +++ b/zenoh-plugin-ros1/tests/discovery_test.rs @@ -19,7 +19,6 @@ use std::{ use async_std::prelude::FutureExt; use multiset::HashMultiSet; -use rosrust::api::Topic; use zenoh::{prelude::keyexpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; use zenoh_plugin_ros1::ros_to_zenoh_bridge::{ @@ -150,32 +149,9 @@ impl DiscoveryCollector { container: &Arc>>, expected: HashMultiSet, ) { - #[cfg(feature = "preserve_topic_metadata")] while expected != *container.lock().unwrap() { async_std::task::sleep(core::time::Duration::from_millis(10)).await; } - #[cfg(not(feature = "preserve_topic_metadata"))] - { - let comparator = || { - let locked = container.lock().unwrap(); - if locked.len() != expected.len() { - return false; - } - for e in expected.iter() { - if !locked.contains(&Topic { - name: e.name.clone(), - datatype: "*".to_string(), - }) { - return false; - } - } - - true - }; - while !comparator() { - async_std::task::sleep(core::time::Duration::from_millis(10)).await; - } - } } } diff --git a/zenoh-plugin-ros1/tests/ros_test.rs b/zenoh-plugin-ros1/tests/ros_test.rs index 8ba0e2b..36e3f86 100644 --- a/zenoh-plugin-ros1/tests/ros_test.rs +++ b/zenoh-plugin-ros1/tests/ros_test.rs @@ -14,33 +14,15 @@ use std::time::Duration; -use rosrust::{Publisher, RawMessage, Subscriber}; +use rosrust::{RawMessage, RawMessageDescription}; use zenoh_plugin_ros1::ros_to_zenoh_bridge::{ ros1_client::Ros1Client, - test_helpers::{wait, BridgeChecker, IsolatedROSMaster, ROSEnvironment}, + rosclient_test_helpers::{ + wait_for_publishers, wait_for_rosclient_to_connect, wait_for_subscribers, + }, + test_helpers::{BridgeChecker, IsolatedROSMaster, ROSEnvironment}, }; -fn wait_for_rosclient_to_connect(rosclient: &Ros1Client) -> bool { - async_std::task::block_on(wait( - || rosclient.topic_types().is_ok(), - Duration::from_secs(10), - )) -} - -fn wait_for_publishers(subscriber: &Subscriber, count: usize) -> bool { - async_std::task::block_on(wait( - || subscriber.publisher_count() == count, - Duration::from_secs(10), - )) -} - -fn wait_for_subscribers(publisher: &Publisher, count: usize) -> bool { - async_std::task::block_on(wait( - || publisher.subscriber_count() == count, - Duration::from_secs(10), - )) -} - #[test] fn check_rosclient_connectivity_ros_then_client() { // init and start isolated ros @@ -312,14 +294,29 @@ fn prove_rosclient_service_non_isolation_service_then_client() { .client(&shared_topic) .expect("error creating client!"); - // check that they are isolated - assert!(client.probe(Duration::from_secs(1)).is_ok()); - assert!(client.req(&RawMessage::default()).is_ok()); - - // wait and check that they are still isolated + // datatype extended description + let description = RawMessageDescription { + msg_definition: "*".to_string(), + md5sum: "*".to_string(), + msg_type: shared_topic.datatype.clone(), + }; + + // check that they are not isolated + assert!(client + .probe_with_description(Duration::from_secs(1), description.clone()) + .is_ok()); + assert!(client + .req_with_description(&RawMessage::default(), description.clone()) + .is_ok()); + + // wait and check that they are still not isolated std::thread::sleep(Duration::from_secs(5)); - assert!(client.probe(Duration::from_secs(1)).is_ok()); - assert!(client.req(&RawMessage::default()).is_ok()); + assert!(client + .probe_with_description(Duration::from_secs(1), description.clone()) + .is_ok()); + assert!(client + .req_with_description(&RawMessage::default(), description.clone()) + .is_ok()); } #[test] @@ -344,12 +341,62 @@ fn prove_rosclient_service_non_isolation_client_then_service() { .service::(&shared_topic, Ok) .expect("error creating service!"); - // check that they are isolated - assert!(client.probe(Duration::from_secs(1)).is_ok()); - assert!(client.req(&RawMessage::default()).is_ok()); - - // wait and check that they are still isolated + // datatype extended description + let description = RawMessageDescription { + msg_definition: "*".to_string(), + md5sum: "*".to_string(), + msg_type: shared_topic.datatype.clone(), + }; + + // check that they are not isolated + assert!(client + .probe_with_description(Duration::from_secs(1), description.clone()) + .is_ok()); + assert!(client + .req_with_description(&RawMessage::default(), description.clone()) + .is_ok()); + + // wait and check that they are still not isolated std::thread::sleep(Duration::from_secs(5)); - assert!(client.probe(Duration::from_secs(1)).is_ok()); - assert!(client.req(&RawMessage::default()).is_ok()); + assert!(client + .probe_with_description(Duration::from_secs(1), description.clone()) + .is_ok()); + assert!(client + .req_with_description(&RawMessage::default(), description.clone()) + .is_ok()); +} + +#[test] +fn service_preserve_datatype_incorrect_datatype() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let rosclient = + Ros1Client::new("test_client", &roscfg.master_uri()).expect("error creating Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&rosclient)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("prove_rosclient_service_non_isolation"); + + // create client and service + let client = rosclient + .client(&shared_topic) + .expect("error creating client!"); + let _service = rosclient + .service::(&shared_topic, Ok) + .expect("error creating service!"); + + // datatype extended description + let description = RawMessageDescription { + msg_definition: "*".to_string(), + md5sum: "deadbeeef".to_string(), // only this makes the request to fail + msg_type: "incorrect/datatype".to_string(), + }; + + // check that datatype is preserved + assert!(client + .req_with_description(&RawMessage::default(), description) + .is_err()); } diff --git a/zenoh-plugin-ros1/tests/service_cache_test.rs b/zenoh-plugin-ros1/tests/service_cache_test.rs new file mode 100644 index 0000000..768e75b --- /dev/null +++ b/zenoh-plugin-ros1/tests/service_cache_test.rs @@ -0,0 +1,181 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use rosrust::api::Topic; +use zenoh_plugin_ros1::ros_to_zenoh_bridge::{ + ros1_client::Ros1Client, + rosclient_test_helpers::wait_for_rosclient_to_connect, + service_cache::Ros1ServiceCache, + test_helpers::{BridgeChecker, IsolatedROSMaster, ROSEnvironment}, +}; + +#[test] +fn check_service_cache_single_node() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclient + let service_node_name = "test_service_node"; + let service_rosclient = Ros1Client::new(service_node_name, &roscfg.master_uri()) + .expect("error creating service Ros1Client!"); + let watching_rosclient = Ros1Client::new("watching_node", &roscfg.master_uri()) + .expect("error creating watching Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&service_rosclient)); + assert!(wait_for_rosclient_to_connect(&watching_rosclient)); + + // create shared topic + let shared_topic = BridgeChecker::make_topic("check_service_cache_single_node"); + + // create service + let _service = service_rosclient + .service::(&shared_topic, Ok) + .expect("error creating service!"); + + // create cache + let mut cache = Ros1ServiceCache::new("ros_service_cache_node", &roscfg.master_uri()) + .expect("error creating Ros1ServiceCache"); + + // check that there is a proper service with one proper node in rosmaster's system state + let state = watching_rosclient + .state() + .expect("error getting ROS state!"); + + assert_eq!(state.services.len(), 1); + assert_eq!(state.services[0].connections.len(), 1); + assert_eq!( + state.services[0].connections[0], + format!("/{service_node_name}") + ); + + // resolve datatype + let resolved_datatype = cache + .resolve_datatype(shared_topic.name, service_node_name.to_string()) + .unwrap(); + + assert_eq!(resolved_datatype, shared_topic.datatype); +} + +#[test] +fn check_service_cache_single_node_many_resolutions() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclients + let service_node_name = "test_service_node"; + let service_rosclient = Ros1Client::new(service_node_name, &roscfg.master_uri()) + .expect("error creating service Ros1Client!"); + let watching_rosclient = Ros1Client::new("watching_node", &roscfg.master_uri()) + .expect("error creating watching Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&service_rosclient)); + assert!(wait_for_rosclient_to_connect(&watching_rosclient)); + + // create shared topic + let shared_topic = + BridgeChecker::make_topic("check_service_cache_single_node_many_resolutions"); + + // create service + let _service = service_rosclient + .service::(&shared_topic, Ok) + .expect("error creating service!"); + + // create cache + let mut cache = Ros1ServiceCache::new("ros_service_cache_node", &roscfg.master_uri()) + .expect("error creating Ros1ServiceCache"); + + // check that there is a proper service with one proper node in rosmaster's system state + let state = watching_rosclient + .state() + .expect("error getting ROS state!"); + + assert_eq!(state.services.len(), 1); + assert_eq!(state.services[0].connections.len(), 1); + assert_eq!( + state.services[0].connections[0], + format!("/{service_node_name}") + ); + + // resolve datatype many times + for _ in 0..10 { + let resolved_datatype = cache + .resolve_datatype(shared_topic.name.clone(), service_node_name.to_string()) + .unwrap(); + assert_eq!(resolved_datatype, shared_topic.datatype); + } +} + +#[test] +fn check_service_cache_many_nodes() { + // init and start isolated ros + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + + // start rosclients + let service_node_name1 = "test_service_node1"; + let service_node_name2 = "test_service_node2"; + let service_rosclient1 = Ros1Client::new(service_node_name1, &roscfg.master_uri()) + .expect("error creating service Ros1Client 1!"); + let service_rosclient2 = Ros1Client::new(service_node_name2, &roscfg.master_uri()) + .expect("error creating service Ros1Client 2!"); + let watching_rosclient = Ros1Client::new("watching_node", &roscfg.master_uri()) + .expect("error creating watching Ros1Client!"); + assert!(wait_for_rosclient_to_connect(&service_rosclient1)); + assert!(wait_for_rosclient_to_connect(&service_rosclient2)); + assert!(wait_for_rosclient_to_connect(&watching_rosclient)); + + // create shared topics + let shared_topic1 = BridgeChecker::make_topic("check_service_cache_many_nodes"); + let shared_topic2 = Topic { + name: shared_topic1.name.clone(), + datatype: "some/other".to_string(), + }; + + // create services + let _service1 = service_rosclient1 + .service::(&shared_topic1, Ok) + .expect("error creating service 1!"); + let _service2 = service_rosclient2 + .service::(&shared_topic2, Ok) + .expect("error creating service 2!"); + + // create cache + let mut cache = Ros1ServiceCache::new("ros_service_cache_node", &roscfg.master_uri()) + .expect("error creating Ros1ServiceCache"); + + // check that there is a proper service with one proper node in rosmaster's system state + // note: currently, ROS1 intends to have not more than one node for each service topic, + // so for current test case service2 should hide service1 + let state = watching_rosclient + .state() + .expect("error getting ROS state!"); + assert_eq!(state.services.len(), 1); + assert_eq!(state.services[0].connections.len(), 1); + assert_eq!( + state.services[0].connections[0], + format!("/{service_node_name2}") + ); + + // resolve datatype for service 1 + let resolved_datatype = cache + .resolve_datatype(shared_topic1.name, service_node_name1.to_string()) + .unwrap(); + assert_eq!(resolved_datatype, shared_topic2.datatype); + + // resolve datatype for service 2 + let resolved_datatype = cache + .resolve_datatype(shared_topic2.name, service_node_name2.to_string()) + .unwrap(); + assert_eq!(resolved_datatype, shared_topic2.datatype); +}