diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5649d5c9..0f349715 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -106,7 +106,7 @@ jobs: - name: Install dependencies (macOS) if: runner.os == 'macOS' run: | - brew install nushell protobuf + brew install nushell protobuf || brew link nushell chmod +x scripts/test-pure-rust.nu - name: Install cargo-nextest (macOS) @@ -308,7 +308,7 @@ jobs: - name: Install dependencies (macOS) if: runner.os == 'macOS' run: | - brew install nushell protobuf + brew install nushell protobuf || brew link nushell chmod +x scripts/test-pure-rust.nu # Common setup diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index f3addd7c..34d1a608 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -116,3 +116,56 @@ jobs: --features ros-interop --release \ -E 'test(dynamic_subscriber)' fi + + ros2dds_bridge_test: + name: ros-z-bridge-ros2dds integration tests (Jazzy / CycloneDDS) + runs-on: ubuntu-latest + if: | + github.event_name == 'push' || + github.event.pull_request.draft == false + container: + image: ros:jazzy-ros-base + steps: + - name: Cache apt packages + uses: actions/cache@v4 + with: + path: /var/cache/apt/archives + key: apt-jazzy-bridge-v1-${{ hashFiles('.github/workflows/test.yml') }} + + - name: Install system dependencies + run: apt-get update && apt-get install -y nodejs cmake clang mold protobuf-compiler + + - name: Install ROS dependencies + run: | + apt-get install -y \ + ros-jazzy-rmw-cyclonedds-cpp \ + ros-jazzy-demo-nodes-cpp + + - uses: actions/checkout@v4 + + - uses: actions-rust-lang/setup-rust-toolchain@v1 + + - name: Setup Rust cache + uses: Swatinem/rust-cache@v2 + with: + shared-key: jazzy-bridge-interop + + - name: Install cargo-nextest + uses: taiki-e/install-action@v2 + with: + tool: cargo-nextest + + - name: Build ros-z-bridge-ros2dds + run: | + RUSTFLAGS="-C linker=clang -C link-arg=-fuse-ld=mold" \ + cargo build -p ros-z-bridge-dds --features jazzy --release -j4 + + - name: Run bridge integration tests + shell: bash + run: | + mkdir -p _tmp + source /opt/ros/jazzy/setup.bash + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + export RUST_LOG=info + cargo nextest run -p ros-z-tests --profile interop \ + --features "dds-bridge-interop,jazzy" --release -j4 diff --git a/Cargo.lock b/Cargo.lock index a6d3a2af..df37e407 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -211,6 +211,29 @@ dependencies = [ "serde", ] +[[package]] +name = "bindgen" +version = "0.69.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "271383c67ccabffb7381723dea0672a673f292304fcb45c01cc648c7a8d58088" +dependencies = [ + "bitflags 2.10.0", + "cexpr", + "clang-sys", + "itertools 0.12.1", + "lazy_static", + "lazycell", + "log", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "rustc-hash 1.1.0", + "shlex", + "syn 2.0.110", + "which", +] + [[package]] name = "bindgen" version = "0.72.1" @@ -226,7 +249,7 @@ dependencies = [ "proc-macro2", "quote", "regex", - "rustc-hash", + "rustc-hash 2.1.1", "shlex", "syn 2.0.110", ] @@ -342,6 +365,16 @@ dependencies = [ "shlex", ] +[[package]] +name = "cdr" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9617422bf43fde9280707a7e90f8f7494389c182f5c70b0f67592d0f06d41dfa" +dependencies = [ + "byteorder", + "serde", +] + [[package]] name = "cesu8" version = "1.1.0" @@ -450,6 +483,15 @@ version = "0.7.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1d728cc89cf3aee9ff92b05e62b19ee65a02b5702cff7d5a377e32c6ae29d8d" +[[package]] +name = "cmake" +version = "0.1.54" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7caa3f9de89ddbe2c607f4101924c5abec803763ae9534e4f4d7d8f84aa81f0" +dependencies = [ + "cc", +] + [[package]] name = "cobs" version = "0.2.3" @@ -779,6 +821,22 @@ dependencies = [ "syn 2.0.110", ] +[[package]] +name = "cyclors" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c5ffb0648538c6d90a2848d4d622ce8e430788bfadfaaf9ebacfa785a2200134" +dependencies = [ + "bincode", + "bindgen 0.69.5", + "cmake", + "derivative", + "libc", + "log", + "serde", + "serde_json", +] + [[package]] name = "darling" version = "0.21.3" @@ -902,6 +960,17 @@ dependencies = [ "serde_core", ] +[[package]] +name = "derivative" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fcc3dd5e9e9c0b295d6e1e4d811fb6f157d5ffd784b8d202fc62eac8035a770b" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + [[package]] name = "derive_more" version = "2.1.1" @@ -1600,6 +1669,15 @@ version = "1.70.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" +[[package]] +name = "itertools" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba291022dbbd398a455acf126c1e341954079855bc60dfdda641363bd6922569" +dependencies = [ + "either", +] + [[package]] name = "itertools" version = "0.13.0" @@ -1694,6 +1772,12 @@ dependencies = [ "spin 0.9.8", ] +[[package]] +name = "lazycell" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" + [[package]] name = "leb128" version = "0.2.5" @@ -2551,7 +2635,7 @@ dependencies = [ "pin-project-lite", "quinn-proto", "quinn-udp", - "rustc-hash", + "rustc-hash 2.1.1", "rustls", "socket2 0.6.1", "thiserror 2.0.17", @@ -2572,7 +2656,7 @@ dependencies = [ "lru-slab", "rand 0.9.2", "ring", - "rustc-hash", + "rustc-hash 2.1.1", "rustls", "rustls-pki-types", "rustls-platform-verifier", @@ -2812,7 +2896,7 @@ dependencies = [ name = "rmw-zenoh-rs" version = "0.1.0" dependencies = [ - "bindgen", + "bindgen 0.72.1", "cxx", "cxx-build", "flume", @@ -2895,6 +2979,20 @@ dependencies = [ "zenoh", ] +[[package]] +name = "ros-z-bridge-dds" +version = "0.1.0" +dependencies = [ + "anyhow", + "clap", + "ros-z", + "ros-z-dds", + "ros-z-protocol", + "tokio", + "tracing", + "tracing-subscriber", +] + [[package]] name = "ros-z-cdr" version = "0.1.0" @@ -2953,6 +3051,27 @@ dependencies = [ "zenoh", ] +[[package]] +name = "ros-z-dds" +version = "0.1.0" +dependencies = [ + "anyhow", + "cdr", + "cyclors", + "flume", + "parking_lot", + "regex", + "ros-z", + "ros-z-msgs", + "ros-z-protocol", + "serde", + "tokio", + "tracing", + "tracing-subscriber", + "zenoh", + "zenoh-ext", +] + [[package]] name = "ros-z-derive" version = "0.1.0" @@ -3030,6 +3149,7 @@ dependencies = [ "protobuf_demo", "ros-z", "ros-z-cdr", + "ros-z-dds", "ros-z-msgs", "ros-z-schema", "serde", @@ -3074,6 +3194,12 @@ dependencies = [ "smallvec", ] +[[package]] +name = "rustc-hash" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" + [[package]] name = "rustc-hash" version = "2.1.1" @@ -4594,6 +4720,18 @@ dependencies = [ "rustls-pki-types", ] +[[package]] +name = "which" +version = "4.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" +dependencies = [ + "either", + "home", + "once_cell", + "rustix 0.38.44", +] + [[package]] name = "win-sys" version = "0.3.1" diff --git a/Cargo.toml b/Cargo.toml index b269cfb9..df63cd0c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,6 +19,8 @@ members = [ "crates/ros-z-tests", "crates/ros-z-console", "crates/ros-z-bridge", + "crates/ros-z-dds", + "crates/ros-z-bridge-dds", "crates/ros-z/examples/protobuf_demo", ] default-members = ["crates/ros-z", "crates/ros-z-codegen"] diff --git a/crates/ros-z-bridge-dds/Cargo.toml b/crates/ros-z-bridge-dds/Cargo.toml new file mode 100644 index 00000000..0b5a98ad --- /dev/null +++ b/crates/ros-z-bridge-dds/Cargo.toml @@ -0,0 +1,29 @@ +[package] +name = "ros-z-bridge-dds" +version.workspace = true +edition.workspace = true +description = "Bridge between DDS-based ROS 2 nodes and Zenoh via ros-z" + +[[bin]] +name = "ros-z-bridge-dds" +path = "src/main.rs" + +[dependencies] +ros-z = { path = "../ros-z" } +ros-z-dds = { path = "../ros-z-dds" } +ros-z-protocol = { path = "../ros-z-protocol", features = [ + "rmw-zenoh", + "ros2dds", +] } + +anyhow = { workspace = true } +clap = { workspace = true, features = ["derive"] } +tokio = { workspace = true, features = ["rt-multi-thread", "macros"] } +tracing = { workspace = true } +tracing-subscriber = { version = "0.3", features = ["env-filter"] } + +[features] +default = ["jazzy"] +jazzy = ["ros-z/jazzy"] +humble = ["ros-z/humble"] +kilted = ["ros-z/kilted"] diff --git a/crates/ros-z-bridge-dds/README.md b/crates/ros-z-bridge-dds/README.md new file mode 100644 index 00000000..adc2c9e4 --- /dev/null +++ b/crates/ros-z-bridge-dds/README.md @@ -0,0 +1,11 @@ + + +# ros-z-bridge-dds + +Standalone binary that bridges DDS-based ROS 2 nodes to a Zenoh/ros-z network. + +**[Full Documentation](https://zettascalelabs.github.io/ros-z/user-guide/dds-bridge.html)** diff --git a/crates/ros-z-bridge-dds/src/config.rs b/crates/ros-z-bridge-dds/src/config.rs new file mode 100644 index 00000000..6e6673b6 --- /dev/null +++ b/crates/ros-z-bridge-dds/src/config.rs @@ -0,0 +1,111 @@ +use clap::Parser; +use ros_z_protocol::KeyExprFormat; + +fn default_domain_id() -> u32 { + std::env::var("ROS_DOMAIN_ID") + .ok() + .and_then(|s| s.parse().ok()) + .unwrap_or(0) +} + +/// Zenoh key expression wire format. +#[derive(Debug, Clone, PartialEq, clap::ValueEnum)] +pub enum WireFormat { + /// rmw_zenoh_cpp compatible format (default). Interops with ros-z and rmw_zenoh_cpp. + #[value(name = "rmw-zenoh")] + RmwZenoh, + /// zenoh-plugin-ros2dds compatible format (legacy). Interops with zenoh-plugin-ros2dds. + #[value(name = "ros2dds")] + Ros2Dds, +} + +impl From for KeyExprFormat { + fn from(w: WireFormat) -> Self { + match w { + WireFormat::RmwZenoh => KeyExprFormat::RmwZenoh, + WireFormat::Ros2Dds => KeyExprFormat::Ros2Dds, + } + } +} + +/// Bridge between DDS-based ROS 2 nodes and a Zenoh/ros-z network. +#[derive(Parser, Debug, Clone)] +#[command(name = "ros-z-bridge-dds")] +pub struct Config { + /// Zenoh endpoint to connect to. + #[arg(short, long, default_value = "tcp/127.0.0.1:7447")] + pub zenoh_endpoint: String, + + /// ROS 2 domain ID. Defaults to ROS_DOMAIN_ID env var, or 0. + #[arg(short, long, default_value_t = default_domain_id())] + pub domain_id: u32, + + /// Namespace prefix applied to all bridged topics/services on the Zenoh side. + #[arg(short, long)] + pub namespace: Option, + + /// Node name used for liveliness tokens. + #[arg(long, default_value = "zenoh_bridge_dds")] + pub node_name: String, + + /// Topic allow-list regex. Only DDS topic names matching this pattern are bridged. + #[arg(long)] + pub allow: Option, + + /// Topic deny-list regex. DDS topic names matching this pattern are not bridged. + #[arg(long)] + pub deny: Option, + + /// Timeout in seconds for Zenoh get() calls on service routes. + #[arg(long, default_value_t = 10)] + pub service_timeout_secs: u64, + + /// Timeout in seconds for action get_result Zenoh get() calls. + #[arg(long, default_value_t = 300)] + pub action_get_result_timeout_secs: u64, + + /// TRANSIENT_LOCAL AdvancedPublisher cache depth multiplier. + #[arg(long, default_value_t = 10)] + pub transient_local_cache_multiplier: usize, + + /// Zenoh key expression wire format. + /// Use `rmw-zenoh` (default) to interop with ros-z/rmw_zenoh_cpp. + /// Use `ros2dds` for legacy zenoh-plugin-ros2dds compatibility. + #[arg(long, value_enum, default_value = "rmw-zenoh")] + pub wire_format: WireFormat, +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_default_domain_id_without_env() { + unsafe { std::env::remove_var("ROS_DOMAIN_ID") }; + assert_eq!(default_domain_id(), 0); + } + + #[test] + fn test_default_domain_id_with_env() { + unsafe { std::env::set_var("ROS_DOMAIN_ID", "42") }; + assert_eq!(default_domain_id(), 42); + unsafe { std::env::remove_var("ROS_DOMAIN_ID") }; + } + + #[test] + fn test_default_domain_id_invalid_env() { + unsafe { std::env::set_var("ROS_DOMAIN_ID", "not_a_number") }; + assert_eq!(default_domain_id(), 0); + unsafe { std::env::remove_var("ROS_DOMAIN_ID") }; + } + + #[test] + fn test_config_defaults() { + let cfg = Config::parse_from(["ros-z-bridge-dds"]); + assert_eq!(cfg.zenoh_endpoint, "tcp/127.0.0.1:7447"); + assert_eq!(cfg.node_name, "zenoh_bridge_dds"); + assert!(cfg.namespace.is_none()); + assert!(cfg.allow.is_none()); + assert!(cfg.deny.is_none()); + } +} diff --git a/crates/ros-z-bridge-dds/src/main.rs b/crates/ros-z-bridge-dds/src/main.rs new file mode 100644 index 00000000..67186821 --- /dev/null +++ b/crates/ros-z-bridge-dds/src/main.rs @@ -0,0 +1,44 @@ +mod config; + +use anyhow::{Result, anyhow}; +use clap::Parser; +use ros_z::{Builder, context::ZContextBuilder}; +use ros_z_dds::{CyclorsParticipant, DdsParticipant, ZDdsBridge}; + +use crate::config::Config; + +#[tokio::main] +async fn main() -> Result<()> { + tracing_subscriber::fmt() + .with_env_filter( + tracing_subscriber::EnvFilter::try_from_default_env().unwrap_or_else(|_| "info".into()), + ) + .init(); + + let cfg = Config::parse(); + + let ctx = ZContextBuilder::default() + .with_connect_endpoints([cfg.zenoh_endpoint.as_str()]) + .keyexpr_format(cfg.wire_format.into()) + .build() + .map_err(|e| anyhow!("{e}"))?; + + let node = ctx + .create_node(&cfg.node_name) + .with_namespace(cfg.namespace.as_deref().unwrap_or("/")) + .build() + .map_err(|e| anyhow!("{e}"))?; + + let participant = CyclorsParticipant::create(cfg.domain_id)?; + + ZDdsBridge::new(node, participant) + .allow_topics_regex(cfg.allow.as_deref()) + .deny_topics_regex(cfg.deny.as_deref()) + .service_timeout(std::time::Duration::from_secs(cfg.service_timeout_secs)) + .action_get_result_timeout(std::time::Duration::from_secs( + cfg.action_get_result_timeout_secs, + )) + .transient_local_cache_multiplier(cfg.transient_local_cache_multiplier) + .run() + .await +} diff --git a/crates/ros-z-dds/Cargo.toml b/crates/ros-z-dds/Cargo.toml new file mode 100644 index 00000000..8cbdaeb7 --- /dev/null +++ b/crates/ros-z-dds/Cargo.toml @@ -0,0 +1,38 @@ +[package] +name = "ros-z-dds" +version.workspace = true +edition.workspace = true +description = "DDS bridging as a first-class ros-z component" + +[dependencies] +cyclors = "=0.2.7" +ros-z = { path = "../ros-z" } +ros-z-protocol = { path = "../ros-z-protocol", features = [ + "std", + "rmw-zenoh", + "ros2dds", +] } +zenoh = { workspace = true, features = ["transport_tls"] } +zenoh-ext = { workspace = true } + +tokio = { workspace = true, features = ["rt-multi-thread", "macros", "sync"] } +anyhow = { workspace = true } +tracing = { workspace = true } +flume = { workspace = true } +parking_lot = { workspace = true } +regex = "1.10" +cdr = "0.2.4" +serde = { workspace = true, features = ["derive"] } + +[dev-dependencies] +tracing-subscriber = { version = "0.3", features = ["env-filter"] } +ros-z-msgs = { path = "../ros-z-msgs", default-features = false, features = [ + "std_msgs", +] } +tokio = { workspace = true, features = ["signal"] } + +[features] +default = ["jazzy"] +jazzy = ["ros-z/jazzy"] +humble = ["ros-z/humble"] +kilted = ["ros-z/kilted"] diff --git a/crates/ros-z-dds/README.md b/crates/ros-z-dds/README.md new file mode 100644 index 00000000..ac55be70 --- /dev/null +++ b/crates/ros-z-dds/README.md @@ -0,0 +1,11 @@ + + +# ros-z-dds + +DDS bridging library for ros-z — connects DDS-based ROS 2 nodes to a Zenoh/ros-z network. + +**[Full Documentation](https://zettascalelabs.github.io/ros-z/user-guide/dds-bridge.html)** diff --git a/crates/ros-z-dds/examples/auto_bridge.rs b/crates/ros-z-dds/examples/auto_bridge.rs new file mode 100644 index 00000000..cc03a8e9 --- /dev/null +++ b/crates/ros-z-dds/examples/auto_bridge.rs @@ -0,0 +1,46 @@ +//! Auto-discovery DDS↔Zenoh bridge. +//! +//! Connects to a Zenoh router and a CycloneDDS domain, then watches for DDS endpoints +//! and automatically creates routes for every publisher, subscriber, service server, and +//! service client that appears. Bridge-to-bridge federation is also handled automatically: +//! a second bridge on a different DDS domain routes traffic across domains without any +//! manual configuration. +//! +//! Run with: +//! ```bash +//! cargo run --example auto_bridge -p ros-z-dds +//! ``` + +use anyhow::{Result, anyhow}; +use ros_z::{Builder, context::ZContextBuilder}; +use ros_z_dds::{CyclorsParticipant, DdsParticipant, ZDdsBridge}; + +#[tokio::main] +async fn main() -> Result<()> { + tracing_subscriber::fmt() + .with_env_filter( + tracing_subscriber::EnvFilter::try_from_default_env().unwrap_or_else(|_| "info".into()), + ) + .init(); + + let domain_id: u32 = std::env::var("ROS_DOMAIN_ID") + .ok() + .and_then(|s| s.parse().ok()) + .unwrap_or(0); + + let ctx = ZContextBuilder::default() + .with_connect_endpoints(["tcp/127.0.0.1:7447"]) + .build() + .map_err(|e| anyhow!("{e}"))?; + + let node = ctx + .create_node("zenoh_bridge_dds") + .build() + .map_err(|e| anyhow!("{e}"))?; + + let participant = CyclorsParticipant::create(domain_id)?; + + tracing::info!("bridge started on DDS domain {domain_id}"); + + ZDdsBridge::new(node, participant).run().await +} diff --git a/crates/ros-z-dds/examples/custom_bridge.rs b/crates/ros-z-dds/examples/custom_bridge.rs new file mode 100644 index 00000000..d202d309 --- /dev/null +++ b/crates/ros-z-dds/examples/custom_bridge.rs @@ -0,0 +1,66 @@ +//! Typed DDS↔Zenoh bridge using `DdsBridgeExt`. +//! +//! Demonstrates bridging specific topics with compile-time type information. The +//! type hash is embedded in the Zenoh liveliness token, which enables remote bridges +//! and ros-z nodes to reconstruct the exact DDS type without any runtime negotiation. +//! +//! This approach is suitable when you know the message types upfront and want: +//! - Compile-time type checking +//! - Full type-hash embedding in liveliness tokens +//! - No unintended topics being bridged +//! +//! For a bridge that automatically discovers and routes everything, see `auto_bridge.rs`. +//! +//! Run with: +//! ```bash +//! cargo run --example custom_bridge -p ros-z-dds +//! ``` + +use anyhow::{Result, anyhow}; +use ros_z::{Builder, context::ZContextBuilder}; +use ros_z_dds::{CyclorsParticipant, DdsBridgeExt, DdsParticipant}; +use ros_z_msgs::std_msgs::String as RosString; + +#[tokio::main] +async fn main() -> Result<()> { + tracing_subscriber::fmt() + .with_env_filter( + tracing_subscriber::EnvFilter::try_from_default_env().unwrap_or_else(|_| "info".into()), + ) + .init(); + + let domain_id: u32 = std::env::var("ROS_DOMAIN_ID") + .ok() + .and_then(|s| s.parse().ok()) + .unwrap_or(0); + + let ctx = ZContextBuilder::default() + .with_connect_endpoints(["tcp/127.0.0.1:7447"]) + .build() + .map_err(|e| anyhow!("{e}"))?; + + let node = ctx + .create_node("custom_bridge") + .build() + .map_err(|e| anyhow!("{e}"))?; + + let participant = CyclorsParticipant::create(domain_id)?; + + // Bridge /chatter: DDS publishers → Zenoh and Zenoh → DDS subscribers. + // The type hash for std_msgs/String is embedded in the liveliness token so + // remote bridges can reconstruct the DDS topic with the exact same type. + let _pub_bridge = node + .bridge_dds_pub::("/chatter", &participant) + .await?; + let _sub_bridge = node + .bridge_dds_sub::("/chatter", &participant) + .await?; + + tracing::info!("custom bridge active on DDS domain {domain_id}"); + tracing::info!(" pub/sub: /chatter (std_msgs/String)"); + + // Routes are RAII handles — they stay active as long as the variables are in scope. + tokio::signal::ctrl_c().await?; + tracing::info!("shutting down"); + Ok(()) +} diff --git a/crates/ros-z-dds/src/bridge.rs b/crates/ros-z-dds/src/bridge.rs new file mode 100644 index 00000000..ba972aa5 --- /dev/null +++ b/crates/ros-z-dds/src/bridge.rs @@ -0,0 +1,904 @@ +//! ZDdsBridge — automatic DDS↔Zenoh discovery and route management. + +use std::{ + collections::{HashMap, HashSet}, + time::Duration, +}; + +use anyhow::{Result, anyhow}; +use regex::Regex; +use ros_z::node::ZNode; +use ros_z_protocol::entity::{EndpointEntity, EndpointKind, Entity, TypeHash}; +use zenoh::{key_expr::OwnedKeyExpr, sample::SampleKind}; + +use crate::{ + discovery::{DiscoveredEndpoint, DiscoveryEvent}, + gid::Gid, + names::{ + dds_topic_to_ros2_name, dds_type_to_ros2_action_type, dds_type_to_ros2_service_type, + dds_type_to_ros2_type, is_action_get_result_topic, is_pubsub_topic, is_request_topic, + type_info_from_user_data, + }, + participant::{BridgeQos, DdsParticipant}, + pubsub::{ZDdsPubBridge, ZDdsSubBridge}, + qos::qos_profile_to_bridge_qos, + ros_discovery::RosDiscoveryPublisher, + service::{ZDdsClientBridge, ZDdsServiceBridge}, +}; + +// ─── Filter ─────────────────────────────────────────────────────────────────── + +struct Filter { + allow: Option, + deny: Option, +} + +impl Filter { + fn new(allow: Option<&str>, deny: Option<&str>) -> Result { + let allow = allow + .map(Regex::new) + .transpose() + .map_err(|e| anyhow!("invalid allow regex: {e}"))?; + let deny = deny + .map(Regex::new) + .transpose() + .map_err(|e| anyhow!("invalid deny regex: {e}"))?; + Ok(Self { allow, deny }) + } + + fn allows(&self, name: &str) -> bool { + if let Some(deny) = &self.deny + && deny.is_match(name) + { + return false; + } + if let Some(allow) = &self.allow { + return allow.is_match(name); + } + true + } +} + +// ─── RouteEntry ─────────────────────────────────────────────────────────────── + +/// A bridge route tracked by both local DDS GIDs and remote bridge Z IDs. +/// +/// The route is kept alive as long as at least one local GID or remote ZID is registered. +/// When both sets are empty, the caller should drop the route. +struct RouteEntry { + _route: R, + /// GIDs of local DDS endpoints that triggered this route. + local_gids: HashSet, + /// Zenoh session IDs of remote bridges that announced a complementary liveliness token. + remote_zids: HashSet, +} + +impl RouteEntry { + fn from_local(route: R, gid: Gid) -> Self { + Self { + _route: route, + local_gids: std::iter::once(gid).collect(), + remote_zids: HashSet::new(), + } + } + + fn from_remote(route: R, zid: String) -> Self { + Self { + _route: route, + local_gids: HashSet::new(), + remote_zids: std::iter::once(zid).collect(), + } + } + + fn is_unused(&self) -> bool { + self.local_gids.is_empty() && self.remote_zids.is_empty() + } +} + +// GIDs of the DDS reader(s) and writer(s) created for a single named route. +// Stored alongside routes so they can be unregistered from ros_discovery_info on removal. +#[derive(Default)] +struct RouteGids { + readers: Vec, + writers: Vec, +} + +// ─── ZDdsBridgeBuilder ──────────────────────────────────────────────────────── + +/// Builder for `ZDdsBridge` — configure filters before starting discovery. +pub struct ZDdsBridgeBuilder { + node: ZNode, + participant: P, + allow_topics: Option, + deny_topics: Option, + service_timeout: Duration, + action_get_result_timeout: Duration, + transient_local_cache_multiplier: usize, +} + +impl ZDdsBridgeBuilder

{ + /// Allow only topics/services whose DDS topic name matches `regex`. + pub fn allow_topics_regex(mut self, regex: Option<&str>) -> Self { + self.allow_topics = regex.map(str::to_owned); + self + } + + /// Deny topics/services whose DDS topic name matches `regex`. + pub fn deny_topics_regex(mut self, regex: Option<&str>) -> Self { + self.deny_topics = regex.map(str::to_owned); + self + } + + /// Timeout for service queryable replies (default 10 s). + pub fn service_timeout(mut self, d: Duration) -> Self { + self.service_timeout = d; + self + } + + /// Timeout for action get_result queryable replies (default 300 s). + pub fn action_get_result_timeout(mut self, d: Duration) -> Self { + self.action_get_result_timeout = d; + self + } + + /// TRANSIENT_LOCAL AdvancedPublisher cache depth multiplier (default 10). + pub fn transient_local_cache_multiplier(mut self, n: usize) -> Self { + self.transient_local_cache_multiplier = n; + self + } + + /// Start the discovery loop and run until all discovery channels close. + pub async fn run(self) -> Result<()> { + let filter = Filter::new(self.allow_topics.as_deref(), self.deny_topics.as_deref())?; + let ros_discovery = + RosDiscoveryPublisher::new(&self.participant, self.node.namespace(), self.node.name()) + .map_err(|e| tracing::warn!("ros_discovery_info publisher failed to start: {e}")) + .ok(); + + // Set up federation liveliness subscriber before entering run_loop. + let (lv_tx, lv_rx) = flume::bounded::<(OwnedKeyExpr, SampleKind)>(256); + let _lv_sub = self + .node + .session() + .liveliness() + .declare_subscriber(liveliness_subscription_pattern()) + .history(true) + .callback(move |sample| { + let ke = OwnedKeyExpr::from(sample.key_expr().clone()); + let _ = lv_tx.try_send((ke, sample.kind())); + }) + .await + .map_err(|e| anyhow!("liveliness subscriber failed: {e}"))?; + + let own_zid = self.node.session().zid().to_string(); + let bridge = ZDdsBridge { + node: self.node, + participant: self.participant, + pub_routes: HashMap::new(), + sub_routes: HashMap::new(), + srv_routes: HashMap::new(), + cli_routes: HashMap::new(), + gid_to_name: HashMap::new(), + route_gids: HashMap::new(), + ros_discovery, + filter, + service_timeout: self.service_timeout, + action_get_result_timeout: self.action_get_result_timeout, + cache_multiplier: self.transient_local_cache_multiplier, + own_zid, + lv_rx, + }; + bridge.run_loop().await + } +} + +// ─── ZDdsBridge ────────────────────────────────────────────────────────────── + +/// Full auto-discovery DDS↔Zenoh bridge. +/// +/// Watches one DDS participant for discovered publications and subscriptions and +/// creates the appropriate `ZDdsPubBridge`, `ZDdsSubBridge`, `ZDdsServiceBridge`, +/// or `ZDdsClientBridge` for each. +/// +/// Construct via `ZDdsBridge::new(node, participant)` which returns a builder; +/// call `.run().await` to start the discovery loop. +pub struct ZDdsBridge { + node: ZNode, + participant: P, + + /// DDS publisher → Zenoh publisher bridges, keyed by ros2_name. + pub_routes: HashMap>>, + /// Zenoh subscriber → DDS writer bridges, keyed by ros2_name. + sub_routes: HashMap>>, + /// DDS service server → Zenoh queryable bridges, keyed by ros2_name. + srv_routes: HashMap>>, + /// DDS service client → Zenoh querier bridges, keyed by ros2_name. + cli_routes: HashMap>>, + + /// Reverse map: gid → ros2_name, for O(1) removal on undiscovery. + gid_to_name: HashMap, + + /// GIDs of DDS readers/writers per route name — for ros_discovery_info unregistration. + route_gids: HashMap, + + /// Publishes `ros_discovery_info` so `ros2 topic/node/service list` can see bridge endpoints. + ros_discovery: Option, + + filter: Filter, + service_timeout: Duration, + action_get_result_timeout: Duration, + cache_multiplier: usize, + + /// Our own Zenoh session ID string — used to skip our own liveliness tokens. + own_zid: String, + /// Federation liveliness events from the Zenoh liveliness subscriber. + lv_rx: flume::Receiver<(OwnedKeyExpr, SampleKind)>, +} + +impl ZDdsBridge

{ + /// Create a new bridge builder for `node` and `participant`. + #[allow(clippy::new_ret_no_self)] + pub fn new(node: ZNode, participant: P) -> ZDdsBridgeBuilder

{ + ZDdsBridgeBuilder { + node, + participant, + allow_topics: None, + deny_topics: None, + service_timeout: Duration::from_secs(10), + action_get_result_timeout: Duration::from_secs(300), + transient_local_cache_multiplier: 10, + } + } + + async fn run_loop(mut self) -> Result<()> { + let discovery_rx = self.participant.run_discovery(); + tracing::info!("ZDdsBridge: discovery loop started"); + loop { + tokio::select! { + event = discovery_rx.recv_async() => { + match event { + Ok(ev) => { + if let Err(e) = self.handle_event(ev).await { + tracing::warn!("ZDdsBridge: route error: {e}"); + } + } + Err(_) => { + tracing::info!("ZDdsBridge: discovery channel closed, shutting down"); + break; + } + } + } + lv_event = self.lv_rx.recv_async() => { + match lv_event { + Ok((ke, kind)) => { + if let Err(e) = self.handle_liveliness(ke, kind).await { + tracing::debug!("ZDdsBridge: liveliness error: {e}"); + } + } + Err(_) => { + tracing::debug!("ZDdsBridge: liveliness channel closed"); + } + } + } + } + } + Ok(()) + } + + async fn handle_event(&mut self, event: DiscoveryEvent) -> Result<()> { + match event { + DiscoveryEvent::DiscoveredPublication(ep) => { + self.on_publication(ep).await?; + } + DiscoveryEvent::UndiscoveredPublication(gid) => { + if let Some(name) = self.gid_to_name.remove(&gid) { + self.remove_local_pub_gid(&name, gid); + self.remove_local_cli_gid(&name, gid); + tracing::debug!("ZDdsBridge: removed local gid for pub/cli route {name}"); + } + } + DiscoveryEvent::DiscoveredSubscription(ep) => { + self.on_subscription(ep).await?; + } + DiscoveryEvent::UndiscoveredSubscription(gid) => { + if let Some(name) = self.gid_to_name.remove(&gid) { + self.remove_local_sub_gid(&name, gid); + self.remove_local_srv_gid(&name, gid); + tracing::debug!("ZDdsBridge: removed local gid for sub/srv route {name}"); + } + } + } + Ok(()) + } + + async fn handle_liveliness(&mut self, ke: OwnedKeyExpr, kind: SampleKind) -> Result<()> { + let ke_ref: zenoh::key_expr::KeyExpr<'_> = (&ke).into(); + let entity = self + .node + .keyexpr_format() + .parse_liveliness(&ke_ref) + .map_err(|e| anyhow!("parse_liveliness failed for {ke}: {e}"))?; + + let ep = match entity { + Entity::Endpoint(ep) => ep, + Entity::Node(_) => return Ok(()), + }; + + // Skip our own liveliness tokens. + let remote_zid = match ep.node.as_ref().map(|n| n.z_id.to_string()) { + Some(zid) => zid, + // Tokens without a node identity (old ros2dds format) — extract from key. + None => ke.as_str().split('/').nth(1).unwrap_or("").to_string(), + }; + if remote_zid == self.own_zid { + return Ok(()); + } + + match kind { + SampleKind::Put => self.on_federation_announce(ep, remote_zid).await, + SampleKind::Delete => self.on_federation_retire(ep, &remote_zid).await, + } + } + + async fn on_federation_announce( + &mut self, + ep: EndpointEntity, + remote_zid: String, + ) -> Result<()> { + let ros2_name = ep.topic.clone(); + if !self.filter.allows(&ros2_name) { + return Ok(()); + } + + let type_name = match ep.type_info.as_ref() { + Some(ti) if !ti.name.is_empty() => ti.name.clone(), + _ => return Ok(()), // can't create DDS entity without a type name + }; + // Zero hash means hash was unknown — pass None to avoid type matching issues. + let type_hash = ep + .type_info + .as_ref() + .filter(|ti| ti.hash != TypeHash::zero()) + .map(|ti| ti.hash.clone()); + + let qos = qos_profile_to_bridge_qos(&ep.qos); + + match ep.kind { + // Remote Publisher → create local ZDdsSubBridge (Zenoh→DDS relay) + EndpointKind::Publisher => { + if let Some(entry) = self.sub_routes.get_mut(&ros2_name) { + entry.remote_zids.insert(remote_zid); + tracing::debug!( + "ZDdsBridge: federation: added remote pub ZID to existing sub route {ros2_name}" + ); + return Ok(()); + } + let bridge = ZDdsSubBridge::new( + &self.node, + &ros2_name, + &type_name, + type_hash, + &self.participant, + qos, + true, + ) + .await?; + tracing::info!( + "ZDdsBridge: federation sub route {ros2_name} (remote pub from {remote_zid})" + ); + self.sub_routes + .insert(ros2_name, RouteEntry::from_remote(bridge, remote_zid)); + } + // Remote Subscription → create local ZDdsPubBridge (DDS→Zenoh relay) + EndpointKind::Subscription => { + if let Some(entry) = self.pub_routes.get_mut(&ros2_name) { + entry.remote_zids.insert(remote_zid); + tracing::debug!( + "ZDdsBridge: federation: added remote sub ZID to existing pub route {ros2_name}" + ); + return Ok(()); + } + let bridge = ZDdsPubBridge::new( + &self.node, + &ros2_name, + &type_name, + type_hash, + &self.participant, + qos, + true, + self.cache_multiplier, + ) + .await?; + tracing::info!( + "ZDdsBridge: federation pub route {ros2_name} (remote sub from {remote_zid})" + ); + self.pub_routes + .insert(ros2_name, RouteEntry::from_remote(bridge, remote_zid)); + } + // Remote Service server → create local ZDdsClientBridge (DDS clients → remote server) + EndpointKind::Service => { + if let Some(entry) = self.cli_routes.get_mut(&ros2_name) { + entry.remote_zids.insert(remote_zid); + return Ok(()); + } + let timeout = if ros2_name.contains("/_action/get_result") { + self.action_get_result_timeout + } else { + self.service_timeout + }; + let bridge = ZDdsClientBridge::new( + &self.node, + &ros2_name, + &type_name, + type_hash, + &self.participant, + qos, + timeout, + ) + .await?; + tracing::info!( + "ZDdsBridge: federation cli route {ros2_name} (remote srv from {remote_zid})" + ); + self.cli_routes + .insert(ros2_name, RouteEntry::from_remote(bridge, remote_zid)); + } + // Remote Service client → create local ZDdsServiceBridge (DDS servers ← remote client) + EndpointKind::Client => { + if let Some(entry) = self.srv_routes.get_mut(&ros2_name) { + entry.remote_zids.insert(remote_zid); + return Ok(()); + } + let bridge = ZDdsServiceBridge::new( + &self.node, + &ros2_name, + &type_name, + &self.participant, + qos, + ) + .await?; + tracing::info!( + "ZDdsBridge: federation srv route {ros2_name} (remote cli from {remote_zid})" + ); + self.srv_routes + .insert(ros2_name, RouteEntry::from_remote(bridge, remote_zid)); + } + } + Ok(()) + } + + async fn on_federation_retire(&mut self, ep: EndpointEntity, remote_zid: &str) -> Result<()> { + let ros2_name = &ep.topic; + match ep.kind { + EndpointKind::Publisher => { + if let Some(entry) = self.sub_routes.get_mut(ros2_name) { + entry.remote_zids.remove(remote_zid); + if entry.is_unused() { + self.sub_routes.remove(ros2_name); + tracing::debug!( + "ZDdsBridge: federation sub route {ros2_name} removed (remote pub {remote_zid} retired)" + ); + } + } + } + EndpointKind::Subscription => { + if let Some(entry) = self.pub_routes.get_mut(ros2_name) { + entry.remote_zids.remove(remote_zid); + if entry.is_unused() { + self.pub_routes.remove(ros2_name); + tracing::debug!( + "ZDdsBridge: federation pub route {ros2_name} removed (remote sub {remote_zid} retired)" + ); + } + } + } + EndpointKind::Service => { + if let Some(entry) = self.cli_routes.get_mut(ros2_name) { + entry.remote_zids.remove(remote_zid); + if entry.is_unused() { + self.cli_routes.remove(ros2_name); + } + } + } + EndpointKind::Client => { + if let Some(entry) = self.srv_routes.get_mut(ros2_name) { + entry.remote_zids.remove(remote_zid); + if entry.is_unused() { + self.srv_routes.remove(ros2_name); + } + } + } + } + Ok(()) + } + + // ─── local-GID removal helpers ──────────────────────────────────────────── + + fn remove_local_pub_gid(&mut self, name: &str, gid: Gid) { + if let Some(entry) = self.pub_routes.get_mut(name) { + entry.local_gids.remove(&gid); + if entry.is_unused() { + self.pub_routes.remove(name); + self.unregister_route_gids(name); + } + } + } + + fn remove_local_sub_gid(&mut self, name: &str, gid: Gid) { + if let Some(entry) = self.sub_routes.get_mut(name) { + entry.local_gids.remove(&gid); + if entry.is_unused() { + self.sub_routes.remove(name); + self.unregister_route_gids(name); + } + } + } + + fn remove_local_cli_gid(&mut self, name: &str, gid: Gid) { + if let Some(entry) = self.cli_routes.get_mut(name) { + entry.local_gids.remove(&gid); + if entry.is_unused() { + self.cli_routes.remove(name); + self.unregister_route_gids(name); + } + } + } + + fn remove_local_srv_gid(&mut self, name: &str, gid: Gid) { + if let Some(entry) = self.srv_routes.get_mut(name) { + entry.local_gids.remove(&gid); + if entry.is_unused() { + self.srv_routes.remove(name); + self.unregister_route_gids(name); + } + } + } + + // ─── ros_discovery_info helpers ─────────────────────────────────────────── + + fn register_route_gids(&mut self, name: &str, gids: RouteGids) { + if let Some(rd) = &self.ros_discovery { + for g in &gids.readers { + rd.add_reader(*g); + } + for g in &gids.writers { + rd.add_writer(*g); + } + } + self.route_gids.insert(name.to_string(), gids); + } + + fn unregister_route_gids(&mut self, name: &str) { + if let Some(gids) = self.route_gids.remove(name) + && let Some(rd) = &self.ros_discovery + { + for g in gids.readers { + rd.remove_reader(g); + } + for g in gids.writers { + rd.remove_writer(g); + } + } + } + + // ─── local DDS discovery ────────────────────────────────────────────────── + + async fn on_publication(&mut self, ep: DiscoveredEndpoint) -> Result<()> { + if !self.filter.allows(&ep.topic_name) { + return Ok(()); + } + + if is_pubsub_topic(&ep.topic_name) { + let ros2_name = match dds_topic_to_ros2_name(&ep.topic_name) { + Some(n) => n, + None => return Ok(()), + }; + let ros2_type = dds_type_to_ros2_type(&ep.type_name); + let ud = ep.qos.user_data.as_deref().unwrap_or(&[]); + let type_info = type_info_from_user_data(&ros2_type, ud); + let type_hash = type_info.as_ref().map(|ti| ti.hash.clone()); + let qos = BridgeQos { + reliability: ep.qos.reliability.clone(), + durability: ep.qos.durability.clone(), + history: ep.qos.history.clone(), + durability_service: ep.qos.durability_service.clone(), + ..Default::default() + }; + + if let Some(entry) = self.pub_routes.get_mut(&ros2_name) { + // Route already exists (possibly from federation) — register local GID. + entry.local_gids.insert(ep.key); + self.gid_to_name.insert(ep.key, ros2_name); + return Ok(()); + } + + let bridge = ZDdsPubBridge::new( + &self.node, + &ros2_name, + &ros2_type, + type_hash, + &self.participant, + qos, + ep.keyless, + self.cache_multiplier, + ) + .await?; + let gids = RouteGids { + readers: bridge.reader_guid().into_iter().collect(), + writers: vec![], + }; + let gid = ep.key; + self.pub_routes + .insert(ros2_name.clone(), RouteEntry::from_local(bridge, gid)); + self.register_route_gids(&ros2_name, gids); + self.gid_to_name.insert(gid, ros2_name); + } else if is_request_topic(&ep.topic_name) { + // DDS service client: request writer discovered → create ZDdsClientBridge + let ros2_name = match dds_topic_to_ros2_name(&ep.topic_name) { + Some(n) => n, + None => return Ok(()), + }; + let ros2_type = if is_action_get_result_topic(&ep.topic_name) { + dds_type_to_ros2_action_type(&ep.type_name) + } else { + dds_type_to_ros2_service_type(&ep.type_name) + }; + + if let Some(entry) = self.cli_routes.get_mut(&ros2_name) { + entry.local_gids.insert(ep.key); + self.gid_to_name.insert(ep.key, ros2_name); + return Ok(()); + } + + let timeout = if is_action_get_result_topic(&ep.topic_name) { + self.action_get_result_timeout + } else { + self.service_timeout + }; + let ud = ep.qos.user_data.as_deref().unwrap_or(&[]); + let type_info = type_info_from_user_data(&ros2_type, ud); + let type_hash = type_info.as_ref().map(|ti| ti.hash.clone()); + let bridge = ZDdsClientBridge::new( + &self.node, + &ros2_name, + &ros2_type, + type_hash, + &self.participant, + BridgeQos::default(), + timeout, + ) + .await?; + let gids = RouteGids { + readers: bridge.reader_guid().into_iter().collect(), + writers: bridge.writer_guid().into_iter().collect(), + }; + self.cli_routes + .insert(ros2_name.clone(), RouteEntry::from_local(bridge, ep.key)); + self.register_route_gids(&ros2_name, gids); + self.gid_to_name.insert(ep.key, ros2_name); + } + Ok(()) + } + + async fn on_subscription(&mut self, ep: DiscoveredEndpoint) -> Result<()> { + if !self.filter.allows(&ep.topic_name) { + return Ok(()); + } + + if is_pubsub_topic(&ep.topic_name) { + let ros2_name = match dds_topic_to_ros2_name(&ep.topic_name) { + Some(n) => n, + None => return Ok(()), + }; + let ros2_type = dds_type_to_ros2_type(&ep.type_name); + let ud = ep.qos.user_data.as_deref().unwrap_or(&[]); + let type_info = type_info_from_user_data(&ros2_type, ud); + let type_hash = type_info.as_ref().map(|ti| ti.hash.clone()); + let qos = BridgeQos { + reliability: ep.qos.reliability.clone(), + durability: ep.qos.durability.clone(), + history: ep.qos.history.clone(), + durability_service: ep.qos.durability_service.clone(), + ..Default::default() + }; + + if let Some(entry) = self.sub_routes.get_mut(&ros2_name) { + // Route already exists (possibly from federation) — register local GID. + entry.local_gids.insert(ep.key); + self.gid_to_name.insert(ep.key, ros2_name); + return Ok(()); + } + + let bridge = ZDdsSubBridge::new( + &self.node, + &ros2_name, + &ros2_type, + type_hash, + &self.participant, + qos, + ep.keyless, + ) + .await?; + let gids = RouteGids { + readers: vec![], + writers: bridge.writer_guid().into_iter().collect(), + }; + let gid = ep.key; + self.sub_routes + .insert(ros2_name.clone(), RouteEntry::from_local(bridge, gid)); + self.register_route_gids(&ros2_name, gids); + self.gid_to_name.insert(gid, ros2_name); + } else if is_request_topic(&ep.topic_name) { + // DDS service server: request reader discovered → create ZDdsServiceBridge + let ros2_name = match dds_topic_to_ros2_name(&ep.topic_name) { + Some(n) => n, + None => return Ok(()), + }; + let ros2_type = if is_action_get_result_topic(&ep.topic_name) { + dds_type_to_ros2_action_type(&ep.type_name) + } else { + dds_type_to_ros2_service_type(&ep.type_name) + }; + + if let Some(entry) = self.srv_routes.get_mut(&ros2_name) { + entry.local_gids.insert(ep.key); + self.gid_to_name.insert(ep.key, ros2_name); + return Ok(()); + } + + let bridge = ZDdsServiceBridge::new( + &self.node, + &ros2_name, + &ros2_type, + &self.participant, + BridgeQos::default(), + ) + .await?; + let gids = RouteGids { + readers: bridge.reader_guid().into_iter().collect(), + writers: bridge.writer_guid().into_iter().collect(), + }; + self.srv_routes + .insert(ros2_name.clone(), RouteEntry::from_local(bridge, ep.key)); + self.register_route_gids(&ros2_name, gids); + self.gid_to_name.insert(ep.key, ros2_name); + } + Ok(()) + } +} + +// ─── helpers ───────────────────────────────────────────────────────────────── + +/// Returns the Zenoh liveliness subscription pattern for federation. +/// +/// Both RmwZenoh and Ros2Dds formats use `@ros2_lv` as the admin space prefix. +const fn liveliness_subscription_pattern() -> &'static str { + "@ros2_lv/**" +} + +// ─── tests ──────────────────────────────────────────────────────────────────── + +#[cfg(test)] +mod tests { + use super::Filter; + + #[test] + fn test_filter_no_rules_allows_all() { + let f = Filter::new(None, None).unwrap(); + assert!(f.allows("rt/chatter")); + assert!(f.allows("rq/add_two_intsRequest")); + } + + #[test] + fn test_filter_allow_only_matching() { + let f = Filter::new(Some("rt/chatter"), None).unwrap(); + assert!(f.allows("rt/chatter")); + assert!(!f.allows("rt/other")); + } + + #[test] + fn test_filter_deny_blocks() { + let f = Filter::new(None, Some("rt/private.*")).unwrap(); + assert!(!f.allows("rt/private_topic")); + assert!(f.allows("rt/chatter")); + } + + #[test] + fn test_filter_deny_overrides_allow() { + let f = Filter::new(Some(".*"), Some("rt/secret")).unwrap(); + assert!(!f.allows("rt/secret")); + assert!(f.allows("rt/chatter")); + } + + #[test] + fn test_filter_invalid_regex() { + assert!(Filter::new(Some("[invalid"), None).is_err()); + assert!(Filter::new(None, Some("[invalid")).is_err()); + } + + #[test] + fn test_dedup_by_name_same_route_skipped() { + use std::collections::HashMap; + let mut routes: HashMap = HashMap::new(); + let name = "/add_two_ints".to_string(); + + if !routes.contains_key(&name) { + routes.insert(name.clone(), "route_a"); + } + if !routes.contains_key(&name) { + routes.insert(name.clone(), "route_b"); + } + + assert_eq!(routes.len(), 1); + assert_eq!(routes[&name], "route_a"); + } + + #[test] + fn test_dedup_different_names_both_inserted() { + use std::collections::HashMap; + let mut routes: HashMap = HashMap::new(); + + for name in ["/service_a", "/service_b"] { + if !routes.contains_key(name) { + routes.insert(name.to_string(), "route"); + } + } + assert_eq!(routes.len(), 2); + } + + #[test] + fn test_gid_reverse_map_removal() { + use crate::gid::Gid; + use std::collections::HashMap; + + let gid = Gid::from([1u8; 16]); + let name = "/chatter".to_string(); + + let mut routes: HashMap = HashMap::new(); + let mut gid_to_name: HashMap = HashMap::new(); + + routes.insert(name.clone(), "route"); + gid_to_name.insert(gid, name.clone()); + + if let Some(removed_name) = gid_to_name.remove(&gid) { + routes.remove(&removed_name); + } + + assert!(routes.is_empty()); + assert!(gid_to_name.is_empty()); + } + + #[test] + fn test_route_entry_lifecycle() { + use crate::gid::Gid; + + struct FakeRoute; + let gid = Gid::from([1u8; 16]); + let mut entry = super::RouteEntry::from_local(FakeRoute, gid); + + assert!(!entry.is_unused()); + entry.local_gids.remove(&gid); + assert!(entry.is_unused()); + } + + #[test] + fn test_route_entry_remote_keeps_alive() { + use crate::gid::Gid; + + struct FakeRoute; + let gid = Gid::from([1u8; 16]); + let mut entry = super::RouteEntry::from_local(FakeRoute, gid); + entry.remote_zids.insert("remote-bridge-zid".to_string()); + + entry.local_gids.remove(&gid); + // Remote ZID still present — not unused. + assert!(!entry.is_unused()); + + entry.remote_zids.remove("remote-bridge-zid"); + assert!(entry.is_unused()); + } + + #[test] + fn test_liveliness_subscription_pattern() { + assert_eq!(super::liveliness_subscription_pattern(), "@ros2_lv/**"); + } +} diff --git a/crates/ros-z-dds/src/cyclors/discovery.rs b/crates/ros-z-dds/src/cyclors/discovery.rs new file mode 100644 index 00000000..1f66498a --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/discovery.rs @@ -0,0 +1,144 @@ +use std::{ffi::CStr, mem::MaybeUninit}; + +use cyclors::{ + DDS_BUILTIN_TOPIC_DCPSPUBLICATION, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, + dds_builtintopic_endpoint_t, dds_create_listener, dds_create_reader, dds_entity_t, + dds_get_instance_handle, dds_get_participant, dds_instance_handle_t, + dds_instance_state_DDS_IST_ALIVE, dds_lset_data_available, dds_return_loan, dds_sample_info_t, + dds_take, +}; +use flume::Sender; + +use crate::{ + discovery::{DiscoveredEndpoint, DiscoveryEvent}, + gid::Gid, + participant::BridgeQos, +}; + +const MAX_SAMPLES: usize = 32; + +#[derive(Clone, Copy)] +enum BuiltinTopicKind { + Publication, + Subscription, +} + +unsafe extern "C" fn on_data(dr: dds_entity_t, arg: *mut std::os::raw::c_void) { + let btx = unsafe { &*(arg as *const (BuiltinTopicKind, Sender)) }; + let kind = btx.0; + let sender = &btx.1; + + let (_, dpih) = unsafe { + let dp = dds_get_participant(dr); + let mut dpih: dds_instance_handle_t = 0; + let _ = dds_get_instance_handle(dp, &mut dpih); + (dp, dpih) + }; + + #[allow(clippy::uninit_assumed_init)] + let mut si = MaybeUninit::<[dds_sample_info_t; MAX_SAMPLES]>::uninit(); + let mut samples: [*mut std::os::raw::c_void; MAX_SAMPLES] = [std::ptr::null_mut(); MAX_SAMPLES]; + + let (n, si) = unsafe { + let n = dds_take( + dr, + samples.as_mut_ptr(), + si.as_mut_ptr() as *mut dds_sample_info_t, + MAX_SAMPLES, + MAX_SAMPLES as u32, + ); + let si = si.assume_init(); + (n, si) + }; + + for i in 0..n as usize { + let result = unsafe { + let sample = samples[i] as *mut dds_builtintopic_endpoint_t; + let participant_ih = (*sample).participant_instance_handle; + let is_alive = si[i].instance_state == dds_instance_state_DDS_IST_ALIVE; + let key: Gid = (*sample).key.v.into(); + + if participant_ih == dpih { + continue; + } + + let topic_name = match CStr::from_ptr((*sample).topic_name).to_str() { + Ok(s) => s.to_string(), + Err(_) => continue, + }; + if topic_name.starts_with("DCPS") { + continue; + } + let type_name = match CStr::from_ptr((*sample).type_name).to_str() { + Ok(s) => s.to_string(), + Err(_) => continue, + }; + let participant_key: Gid = (*sample).participant_key.v.into(); + let keyless = (*sample).key.v[15] == 3 || (*sample).key.v[15] == 4; + let bridge_qos: BridgeQos = cyclors::qos::Qos::from_qos_native((*sample).qos).into(); + + ( + key, + participant_key, + topic_name, + type_name, + keyless, + bridge_qos, + is_alive, + ) + }; + + let (key, participant_key, topic_name, type_name, keyless, qos, is_alive) = result; + + if is_alive { + tracing::debug!( + "Discovered DDS endpoint {key} on {topic_name} ({type_name}) keyless={keyless}", + ); + let endpoint = DiscoveredEndpoint { + key, + participant_key, + topic_name, + type_name, + keyless, + qos, + }; + let event = match kind { + BuiltinTopicKind::Publication => DiscoveryEvent::DiscoveredPublication(endpoint), + BuiltinTopicKind::Subscription => DiscoveryEvent::DiscoveredSubscription(endpoint), + }; + let _ = sender.try_send(event); + } else { + let event = match kind { + BuiltinTopicKind::Publication => DiscoveryEvent::UndiscoveredPublication(key), + BuiltinTopicKind::Subscription => DiscoveryEvent::UndiscoveredSubscription(key), + }; + let _ = sender.try_send(event); + } + } + + unsafe { + dds_return_loan(dr, samples.as_mut_ptr(), MAX_SAMPLES as i32); + } +} + +/// Install builtin-topic listeners on `dp` and forward events to `tx`. +/// +/// State boxes are intentionally leaked — they live as long as the participant. +pub fn run_discovery_raw(dp: dds_entity_t, tx: Sender) { + unsafe { + for kind in [ + BuiltinTopicKind::Publication, + BuiltinTopicKind::Subscription, + ] { + let state = Box::new((kind, tx.clone())); + let raw_ptr = Box::into_raw(state) as *mut std::os::raw::c_void; + let listener = dds_create_listener(raw_ptr); + dds_lset_data_available(listener, Some(on_data)); + let builtin = match kind { + BuiltinTopicKind::Publication => DDS_BUILTIN_TOPIC_DCPSPUBLICATION, + BuiltinTopicKind::Subscription => DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, + }; + dds_create_reader(dp, builtin, std::ptr::null(), listener); + } + } +} diff --git a/crates/ros-z-dds/src/cyclors/entity.rs b/crates/ros-z-dds/src/cyclors/entity.rs new file mode 100644 index 00000000..3a28bce0 --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/entity.rs @@ -0,0 +1,55 @@ +use anyhow::{Result, anyhow}; +use cyclors::{dds_delete, dds_entity_t, dds_get_guid, dds_guid_t}; + +use crate::participant::DdsReader; + +/// RAII wrapper around a `dds_entity_t`. +/// +/// Calls `dds_delete` on drop, preventing ghost-subscriber accumulation. +pub struct DdsEntity(pub(super) dds_entity_t); + +impl DdsEntity { + /// Wrap an existing DDS entity handle. + /// + /// # Safety + /// `handle` must be a valid entity created by the caller and not yet deleted. + pub unsafe fn new(handle: dds_entity_t) -> Self { + Self(handle) + } + + pub fn raw(&self) -> dds_entity_t { + self.0 + } +} + +impl Drop for DdsEntity { + fn drop(&mut self) { + if self.0 != 0 { + unsafe { + dds_delete(self.0); + } + } + } +} + +unsafe impl Send for DdsEntity {} +unsafe impl Sync for DdsEntity {} + +impl DdsReader for DdsEntity { + fn guid(&self) -> Result<[u8; 16]> { + get_entity_guid(self.0) + } +} + +/// Get the 16-byte DDS GUID of any DDS entity. +pub(super) fn get_entity_guid(entity: dds_entity_t) -> Result<[u8; 16]> { + unsafe { + let mut guid = dds_guid_t { v: [0u8; 16] }; + let r = dds_get_guid(entity, &mut guid); + if r == 0 { + Ok(guid.v) + } else { + Err(anyhow!("dds_get_guid failed: {r}")) + } + } +} diff --git a/crates/ros-z-dds/src/cyclors/mod.rs b/crates/ros-z-dds/src/cyclors/mod.rs new file mode 100644 index 00000000..b550b014 --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/mod.rs @@ -0,0 +1,8 @@ +pub mod discovery; +pub mod entity; +pub mod participant; +pub mod qos; +pub mod reader; +pub mod writer; + +pub use participant::CyclorsParticipant; diff --git a/crates/ros-z-dds/src/cyclors/participant.rs b/crates/ros-z-dds/src/cyclors/participant.rs new file mode 100644 index 00000000..a27d1750 --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/participant.rs @@ -0,0 +1,87 @@ +use std::ffi::CString; + +use anyhow::{Result, anyhow}; +use cyclors::{dds_create_domain, dds_create_participant}; +use flume::Receiver; + +use crate::{ + discovery::DiscoveryEvent, + participant::{BridgeQos, DdsParticipant}, +}; + +use super::{ + discovery::run_discovery_raw, + entity::{DdsEntity, get_entity_guid}, + reader::create_reader, + writer::{CyclorsWriter, create_writer}, +}; + +/// CycloneDDS-backed participant. +pub struct CyclorsParticipant { + entity: DdsEntity, +} + +// Safety: CycloneDDS entity handles are thread-safe for read/write operations. +unsafe impl Send for CyclorsParticipant {} +unsafe impl Sync for CyclorsParticipant {} + +impl DdsParticipant for CyclorsParticipant { + type Reader = DdsEntity; + type Writer = CyclorsWriter; + + fn create(domain_id: u32) -> Result { + // CycloneDDS 0.11+ does not read CYCLONEDDS_URI when dds_create_domain + // receives a null config pointer — it uses hardcoded defaults instead. + // Explicitly pass the env-var value so network interface overrides work. + let config_env = std::env::var("CYCLONEDDS_URI").unwrap_or_default(); + let config_cstr = + CString::new(config_env.as_str()).unwrap_or_else(|_| CString::new("").unwrap()); + let config_ptr = if config_env.is_empty() { + std::ptr::null() + } else { + config_cstr.as_ptr() + }; + + unsafe { + dds_create_domain(domain_id, config_ptr); + let participant = dds_create_participant(domain_id, std::ptr::null(), std::ptr::null()); + if participant < 0 { + return Err(anyhow!("dds_create_participant failed: {participant}")); + } + Ok(Self { + entity: DdsEntity::new(participant), + }) + } + } + + fn participant_guid(&self) -> Result<[u8; 16]> { + get_entity_guid(self.entity.raw()) + } + + fn run_discovery(&self) -> Receiver { + let (tx, rx) = flume::bounded(256); + run_discovery_raw(self.entity.raw(), tx); + rx + } + + fn create_reader( + &self, + topic: &str, + type_name: &str, + keyless: bool, + qos: BridgeQos, + callback: Box) + Send + 'static>, + ) -> Result { + create_reader(self.entity.raw(), topic, type_name, keyless, qos, callback) + } + + fn create_writer( + &self, + topic: &str, + type_name: &str, + keyless: bool, + qos: BridgeQos, + ) -> Result { + create_writer(self.entity.raw(), topic, type_name, keyless, qos) + } +} diff --git a/crates/ros-z-dds/src/cyclors/qos.rs b/crates/ros-z-dds/src/cyclors/qos.rs new file mode 100644 index 00000000..f605879e --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/qos.rs @@ -0,0 +1,347 @@ +use std::time::Duration; + +use cyclors::qos::{ + DDS_INFINITE_TIME, Durability as CDurability, DurabilityKind as CDurabilityKind, + DurabilityService as CDurabilityService, History as CHistory, HistoryKind as CHistoryKind, + IgnoreLocal, IgnoreLocalKind, Qos, Reliability as CReliability, + ReliabilityKind as CReliabilityKind, +}; + +use crate::participant::{ + BridgeQos, Durability, DurabilityKind, DurabilityService, History, HistoryKind, Reliability, + ReliabilityKind, +}; + +pub const DDS_LENGTH_UNLIMITED: i32 = -1; + +// ─── Duration helpers ───────────────────────────────────────────────────────── + +fn duration_to_nanos(d: Duration) -> i64 { + d.as_nanos().min(i64::MAX as u128) as i64 +} + +fn nanos_to_duration(ns: i64) -> Option { + if ns == DDS_INFINITE_TIME || ns < 0 { + None + } else { + Some(Duration::from_nanos(ns as u64)) + } +} + +fn duration_option_to_nanos(d: Option) -> i64 { + d.map(duration_to_nanos).unwrap_or(DDS_INFINITE_TIME) +} + +fn max_to_option(v: i32) -> Option { + if v == DDS_LENGTH_UNLIMITED { + None + } else { + Some(v.max(0) as usize) + } +} + +fn option_to_max(v: Option) -> i32 { + v.map(|n| n.min(i32::MAX as usize) as i32) + .unwrap_or(DDS_LENGTH_UNLIMITED) +} + +// ─── Kind conversions ───────────────────────────────────────────────────────── + +impl From for ReliabilityKind { + fn from(k: CReliabilityKind) -> Self { + match k { + CReliabilityKind::BEST_EFFORT => Self::BestEffort, + CReliabilityKind::RELIABLE => Self::Reliable, + } + } +} + +impl From for CReliabilityKind { + fn from(k: ReliabilityKind) -> Self { + match k { + ReliabilityKind::BestEffort => Self::BEST_EFFORT, + ReliabilityKind::Reliable => Self::RELIABLE, + } + } +} + +impl From for DurabilityKind { + fn from(k: CDurabilityKind) -> Self { + match k { + CDurabilityKind::VOLATILE => Self::Volatile, + CDurabilityKind::TRANSIENT_LOCAL => Self::TransientLocal, + // TRANSIENT and PERSISTENT are not used by ROS 2 nodes; treat as Volatile. + _ => Self::Volatile, + } + } +} + +impl From for CDurabilityKind { + fn from(k: DurabilityKind) -> Self { + match k { + DurabilityKind::Volatile => Self::VOLATILE, + DurabilityKind::TransientLocal => Self::TRANSIENT_LOCAL, + } + } +} + +impl From for HistoryKind { + fn from(k: CHistoryKind) -> Self { + match k { + CHistoryKind::KEEP_LAST => Self::KeepLast, + CHistoryKind::KEEP_ALL => Self::KeepAll, + } + } +} + +impl From for CHistoryKind { + fn from(k: HistoryKind) -> Self { + match k { + HistoryKind::KeepLast => Self::KEEP_LAST, + HistoryKind::KeepAll => Self::KEEP_ALL, + } + } +} + +// ─── BridgeQos ↔ cyclors::qos::Qos ────────────────────────────────────────── + +impl From for BridgeQos { + fn from(q: Qos) -> Self { + Self { + reliability: q.reliability.map(|r| Reliability { + kind: r.kind.into(), + max_blocking_time: nanos_to_duration(r.max_blocking_time), + }), + durability: q.durability.map(|d| Durability { + kind: d.kind.into(), + }), + history: q.history.map(|h| History { + kind: h.kind.into(), + depth: h.depth, + }), + durability_service: q.durability_service.map(|ds| DurabilityService { + service_cleanup_delay: nanos_to_duration(ds.service_cleanup_delay), + history_kind: ds.history_kind.into(), + history_depth: ds.history_depth, + max_samples: max_to_option(ds.max_samples), + max_instances: max_to_option(ds.max_instances), + max_samples_per_instance: max_to_option(ds.max_samples_per_instance), + }), + deadline: q.deadline.and_then(|d| nanos_to_duration(d.period)), + latency_budget: q + .latency_budget + .and_then(|lb| nanos_to_duration(lb.duration)), + lifespan: q.lifespan.and_then(|ls| nanos_to_duration(ls.duration)), + user_data: q.user_data, + ignore_local: q + .ignore_local + .is_some_and(|il| il.kind == IgnoreLocalKind::PARTICIPANT), + } + } +} + +impl From for Qos { + fn from(bq: BridgeQos) -> Self { + Qos { + reliability: bq.reliability.map(|r| CReliability { + kind: r.kind.into(), + max_blocking_time: duration_option_to_nanos(r.max_blocking_time), + }), + durability: bq.durability.map(|d| CDurability { + kind: d.kind.into(), + }), + history: bq.history.map(|h| CHistory { + kind: h.kind.into(), + depth: h.depth, + }), + durability_service: bq.durability_service.map(|ds| CDurabilityService { + service_cleanup_delay: duration_option_to_nanos(ds.service_cleanup_delay), + history_kind: ds.history_kind.into(), + history_depth: ds.history_depth, + max_samples: option_to_max(ds.max_samples), + max_instances: option_to_max(ds.max_instances), + max_samples_per_instance: option_to_max(ds.max_samples_per_instance), + }), + deadline: bq.deadline.map(|d| cyclors::qos::Deadline { + period: duration_to_nanos(d), + }), + latency_budget: bq.latency_budget.map(|d| cyclors::qos::LatencyBudget { + duration: duration_to_nanos(d), + }), + lifespan: bq.lifespan.map(|d| cyclors::qos::Lifespan { + duration: duration_to_nanos(d), + }), + user_data: bq.user_data, + ignore_local: if bq.ignore_local { + Some(IgnoreLocal { + kind: IgnoreLocalKind::PARTICIPANT, + }) + } else { + None + }, + ..Default::default() + } + } +} + +/// Adapt a discovered DDS writer's QoS to create a compatible reader. +pub fn adapt_writer_qos_for_reader(qos: &BridgeQos) -> BridgeQos { + BridgeQos { + reliability: Some(qos.reliability.clone().unwrap_or(Reliability { + kind: ReliabilityKind::BestEffort, + max_blocking_time: Some(DDS_100MS_DURATION_STD), + })), + durability: qos.durability.clone(), + history: qos.history.clone(), + user_data: qos.user_data.clone(), + ignore_local: true, + ..Default::default() + } +} + +/// Adapt a discovered DDS reader's QoS to create a compatible writer. +pub fn adapt_reader_qos_for_writer(qos: &BridgeQos) -> BridgeQos { + let is_transient = qos + .durability + .as_ref() + .is_some_and(|d| d.kind == DurabilityKind::TransientLocal); + + let history_kind = qos + .history + .as_ref() + .map_or(HistoryKind::KeepLast, |h| h.kind); + let history_depth = qos.history.as_ref().map_or(1, |h| h.depth); + + let durability_service = if is_transient { + Some(DurabilityService { + service_cleanup_delay: Some(Duration::from_secs(60)), + history_kind, + history_depth, + max_samples: None, + max_instances: None, + max_samples_per_instance: None, + }) + } else { + None + }; + + // Bump max_blocking_time by 1 ns for FastRTPS interop. + let reliability = Some(match &qos.reliability { + Some(r) => Reliability { + kind: r.kind, + max_blocking_time: Some( + r.max_blocking_time + .unwrap_or(Duration::from_millis(100)) + .saturating_add(Duration::from_nanos(1)), + ), + }, + None => Reliability { + kind: ReliabilityKind::Reliable, + max_blocking_time: Some(DDS_100MS_DURATION_STD + Duration::from_nanos(1)), + }, + }); + + BridgeQos { + reliability, + durability: qos.durability.clone(), + history: qos.history.clone(), + durability_service, + user_data: qos.user_data.clone(), + ignore_local: true, + ..Default::default() + } +} + +const DDS_100MS_DURATION_STD: Duration = Duration::from_millis(100); + +// ─── Unit tests ─────────────────────────────────────────────────────────────── + +#[cfg(test)] +mod tests { + use super::*; + + fn reliable_bridge_qos() -> BridgeQos { + BridgeQos { + reliability: Some(Reliability { + kind: ReliabilityKind::Reliable, + max_blocking_time: Some(Duration::from_millis(100)), + }), + ..Default::default() + } + } + + fn transient_bridge_qos(depth: i32) -> BridgeQos { + BridgeQos { + durability: Some(Durability { + kind: DurabilityKind::TransientLocal, + }), + history: Some(History { + kind: HistoryKind::KeepLast, + depth, + }), + ..Default::default() + } + } + + #[test] + fn test_round_trip_reliable_qos() { + let bq = reliable_bridge_qos(); + let cq: Qos = bq.clone().into(); + let back: BridgeQos = cq.into(); + assert_eq!(back.reliability, bq.reliability); + } + + #[test] + fn test_round_trip_transient_local() { + let bq = transient_bridge_qos(5); + let cq: Qos = bq.clone().into(); + let back: BridgeQos = cq.into(); + assert_eq!(back.durability, bq.durability); + assert_eq!(back.history, bq.history); + } + + #[test] + fn test_none_duration_becomes_infinite() { + let bq = BridgeQos { + reliability: Some(Reliability { + kind: ReliabilityKind::Reliable, + max_blocking_time: None, + }), + ..Default::default() + }; + let cq: Qos = bq.into(); + assert_eq!(cq.reliability.unwrap().max_blocking_time, DDS_INFINITE_TIME); + } + + #[test] + fn test_unlimited_instances_roundtrip() { + let bq = BridgeQos { + durability_service: Some(DurabilityService { + service_cleanup_delay: Some(Duration::from_secs(60)), + history_kind: HistoryKind::KeepLast, + history_depth: 1, + max_samples: None, + max_instances: None, + max_samples_per_instance: None, + }), + ..Default::default() + }; + let cq: Qos = bq.into(); + let ds = cq.durability_service.unwrap(); + assert_eq!(ds.max_instances, DDS_LENGTH_UNLIMITED); + } + + #[test] + fn test_adapt_writer_sets_ignore_local() { + let adapted = adapt_writer_qos_for_reader(&reliable_bridge_qos()); + assert!(adapted.ignore_local); + } + + #[test] + fn test_adapt_reader_transient_adds_durability_service() { + let adapted = adapt_reader_qos_for_writer(&transient_bridge_qos(5)); + let ds = adapted.durability_service.expect("durability_service set"); + assert_eq!(ds.history_depth, 5); + assert_eq!(ds.max_instances, None); + } +} diff --git a/crates/ros-z-dds/src/cyclors/reader.rs b/crates/ros-z-dds/src/cyclors/reader.rs new file mode 100644 index 00000000..2752bdb9 --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/reader.rs @@ -0,0 +1,125 @@ +use std::{mem::MaybeUninit, slice}; + +use anyhow::{Result, anyhow}; +use cyclors::{ + DDS_ANY_STATE, cdds_create_blob_topic, dds_create_listener, dds_create_reader, dds_entity_t, + dds_lset_data_available, dds_reader_wait_for_historical_data, dds_sample_info_t, + dds_strretcode, dds_takecdr, ddsi_serdata, ddsi_serdata_size, ddsi_serdata_to_ser_ref, + ddsi_serdata_to_ser_unref, ddsi_serdata_unref, ddsrt_iovec_t, +}; + +use crate::participant::BridgeQos; + +use super::entity::DdsEntity; + +// ─── Raw sample wrapper ─────────────────────────────────────────────────────── + +struct RawSample { + sdref: *mut ddsi_serdata, + data: ddsrt_iovec_t, +} + +impl RawSample { + unsafe fn create(serdata: *const ddsi_serdata) -> Self { + let mut data = ddsrt_iovec_t { + iov_base: std::ptr::null_mut(), + iov_len: 0, + }; + let sdref = unsafe { + let size = ddsi_serdata_size(serdata); + ddsi_serdata_to_ser_ref(serdata, 0, size as usize, &mut data) + }; + RawSample { sdref, data } + } + + fn as_slice(&self) -> &[u8] { + unsafe { slice::from_raw_parts(self.data.iov_base as *const u8, self.data.iov_len) } + } +} + +impl Drop for RawSample { + fn drop(&mut self) { + unsafe { + ddsi_serdata_to_ser_unref(self.sdref, &self.data); + } + } +} + +unsafe impl Send for RawSample {} + +// ─── Reader callback trampoline ─────────────────────────────────────────────── + +unsafe extern "C" fn on_data_available(dr: dds_entity_t, arg: *mut std::os::raw::c_void) +where + F: Fn(Vec), +{ + let cb = unsafe { &*(arg as *const F) }; + let mut zp: *mut ddsi_serdata = std::ptr::null_mut(); + #[allow(clippy::uninit_assumed_init)] + let mut si = MaybeUninit::<[dds_sample_info_t; 1]>::uninit(); + unsafe { + while dds_takecdr( + dr, + &mut zp, + 1, + si.as_mut_ptr() as *mut dds_sample_info_t, + DDS_ANY_STATE, + ) > 0 + { + let si = si.assume_init(); + if si[0].valid_data { + let raw = RawSample::create(zp); + cb(raw.as_slice().to_vec()); + } + ddsi_serdata_unref(zp); + } + } +} + +// ─── Public factory ─────────────────────────────────────────────────────────── + +/// Create a CycloneDDS blob reader. `callback` receives each valid CDR sample +/// as a `Vec` (4-byte CDR header + payload). +pub fn create_reader( + dp: dds_entity_t, + topic: &str, + type_name: &str, + keyless: bool, + qos: BridgeQos, + callback: F, +) -> Result +where + F: Fn(Vec) + Send + 'static, +{ + unsafe { + let c_topic = std::ffi::CString::new(topic).unwrap(); + let c_type = std::ffi::CString::new(type_name).unwrap(); + let topic_h = cdds_create_blob_topic(dp, c_topic.into_raw(), c_type.into_raw(), keyless); + if topic_h < 0 { + return Err(anyhow!("cdds_create_blob_topic failed: {topic_h}")); + } + + let cb_box = Box::new(callback); + let listener = dds_create_listener(Box::into_raw(cb_box) as *mut std::os::raw::c_void); + dds_lset_data_available(listener, Some(on_data_available::)); + + let cyclors_qos: cyclors::qos::Qos = qos.into(); + let qos_native = cyclors_qos.to_qos_native(); + let reader = dds_create_reader(dp, topic_h, qos_native, listener); + cyclors::qos::Qos::delete_qos_native(qos_native); + + if reader < 0 { + let msg = std::ffi::CStr::from_ptr(dds_strretcode(-reader)) + .to_str() + .unwrap_or("unknown DDS error"); + return Err(anyhow!("dds_create_reader failed: {msg}")); + } + + let res = dds_reader_wait_for_historical_data(reader, cyclors::qos::DDS_100MS_DURATION); + if res < 0 { + tracing::warn!("dds_reader_wait_for_historical_data: {res}"); + } + + Ok(DdsEntity::new(reader)) + } +} diff --git a/crates/ros-z-dds/src/cyclors/writer.rs b/crates/ros-z-dds/src/cyclors/writer.rs new file mode 100644 index 00000000..84300000 --- /dev/null +++ b/crates/ros-z-dds/src/cyclors/writer.rs @@ -0,0 +1,110 @@ +use std::ffi::{CStr, CString}; + +use anyhow::{Result, anyhow}; +use cyclors::{ + cdds_create_blob_topic, dds_create_writer, dds_entity_t, dds_get_entity_sertype, + dds_strretcode, dds_writecdr, ddsi_serdata_from_ser_iov, ddsi_serdata_kind_SDK_DATA, + ddsi_sertype, ddsrt_iov_len_t, ddsrt_iovec_t, +}; + +use crate::participant::{BridgeQos, DdsWriter}; + +use super::{entity::DdsEntity, entity::get_entity_guid}; + +/// CycloneDDS implementation of [`DdsWriter`]. +pub struct CyclorsWriter(pub(super) DdsEntity); + +impl DdsWriter for CyclorsWriter { + fn write_cdr(&self, data: Vec) -> Result<()> { + write_cdr_raw(self.0.raw(), data) + } + + fn instance_handle(&self) -> Result { + get_instance_handle(self.0.raw()) + } + + fn guid(&self) -> Result<[u8; 16]> { + get_entity_guid(self.0.raw()) + } +} + +/// Create a CycloneDDS blob writer on the given participant. +pub fn create_writer( + dp: dds_entity_t, + topic: &str, + type_name: &str, + keyless: bool, + qos: BridgeQos, +) -> Result { + unsafe { + let c_topic = CString::new(topic).unwrap(); + let c_type = CString::new(type_name).unwrap(); + let topic_h = cdds_create_blob_topic(dp, c_topic.into_raw(), c_type.into_raw(), keyless); + if topic_h < 0 { + return Err(anyhow!("cdds_create_blob_topic failed: {topic_h}")); + } + + let cyclors_qos: cyclors::qos::Qos = qos.into(); + let qos_native = cyclors_qos.to_qos_native(); + let writer = dds_create_writer(dp, topic_h, qos_native, std::ptr::null_mut()); + cyclors::qos::Qos::delete_qos_native(qos_native); + + if writer < 0 { + let msg = CStr::from_ptr(dds_strretcode(-writer)) + .to_str() + .unwrap_or("unknown"); + return Err(anyhow!("dds_create_writer failed: {msg}")); + } + Ok(CyclorsWriter(DdsEntity::new(writer))) + } +} + +/// Write raw CDR bytes through a DDS entity handle. +fn write_cdr_raw(writer: dds_entity_t, data: Vec) -> Result<()> { + unsafe { + let len = data.len(); + let mut data = std::mem::ManuallyDrop::new(data); + let ptr = data.as_mut_ptr(); + let cap = data.capacity(); + + let iov_len: ddsrt_iov_len_t = len; + let iov = ddsrt_iovec_t { + iov_base: ptr as *mut std::os::raw::c_void, + iov_len, + }; + + let mut sertype: *const ddsi_sertype = std::ptr::null(); + let ret = dds_get_entity_sertype(writer, &mut sertype); + if ret < 0 { + drop(Vec::from_raw_parts(ptr, len, cap)); + let msg = CStr::from_ptr(dds_strretcode(ret)) + .to_str() + .unwrap_or("unknown"); + return Err(anyhow!("dds_get_entity_sertype failed: {msg}")); + } + + let serdata = ddsi_serdata_from_ser_iov(sertype, ddsi_serdata_kind_SDK_DATA, 1, &iov, len); + let ret = dds_writecdr(writer, serdata); + drop(Vec::from_raw_parts(ptr, len, cap)); + + if ret < 0 { + let msg = CStr::from_ptr(dds_strretcode(ret)) + .to_str() + .unwrap_or("unknown"); + return Err(anyhow!("dds_writecdr failed: {msg}")); + } + Ok(()) + } +} + +fn get_instance_handle(entity: dds_entity_t) -> Result { + unsafe { + let mut handle: cyclors::dds_instance_handle_t = 0; + let ret = cyclors::dds_get_instance_handle(entity, &mut handle); + if ret == 0 { + Ok(handle) + } else { + Err(anyhow!("dds_get_instance_handle failed: {ret}")) + } + } +} diff --git a/crates/ros-z-dds/src/discovery.rs b/crates/ros-z-dds/src/discovery.rs new file mode 100644 index 00000000..7837580c --- /dev/null +++ b/crates/ros-z-dds/src/discovery.rs @@ -0,0 +1,21 @@ +use crate::{gid::Gid, participant::BridgeQos}; + +/// Metadata about a discovered DDS endpoint (publication or subscription). +#[derive(Debug, Clone)] +pub struct DiscoveredEndpoint { + pub key: Gid, + pub participant_key: Gid, + pub topic_name: String, + pub type_name: String, + pub keyless: bool, + pub qos: BridgeQos, +} + +/// Events emitted by the DDS discovery loop. +#[derive(Debug)] +pub enum DiscoveryEvent { + DiscoveredPublication(DiscoveredEndpoint), + UndiscoveredPublication(Gid), + DiscoveredSubscription(DiscoveredEndpoint), + UndiscoveredSubscription(Gid), +} diff --git a/crates/ros-z-dds/src/ext.rs b/crates/ros-z-dds/src/ext.rs new file mode 100644 index 00000000..49060dc1 --- /dev/null +++ b/crates/ros-z-dds/src/ext.rs @@ -0,0 +1,83 @@ +//! DdsBridgeExt — typed extension trait on ZNode for ergonomic DDS bridging. + +use anyhow::Result; +use ros_z::{MessageTypeInfo, node::ZNode}; +use ros_z_protocol::entity::TypeHash; + +use crate::{ + names::dds_type_to_ros2_type, + participant::{BridgeQos, DdsParticipant}, + pubsub::{ZDdsPubBridge, ZDdsSubBridge}, +}; + +/// Typed convenience methods on [`ZNode`] for creating DDS↔Zenoh pub/sub bridges. +/// +/// Each method extracts the ROS 2 type name and type hash from the compile-time +/// type parameter `T` and delegates to the corresponding untyped bridge constructor. +/// +/// ```rust,ignore +/// use ros_z_dds::DdsBridgeExt; +/// +/// let _pub = node.bridge_dds_pub::("/chatter", &participant).await?; +/// let _sub = node.bridge_dds_sub::("/chatter", &participant).await?; +/// ``` +#[allow(async_fn_in_trait)] +pub trait DdsBridgeExt { + /// Bridge a DDS topic publisher to a Zenoh publisher. + /// + /// The type `T` must implement `MessageTypeInfo` — all generated ros-z message + /// types satisfy this automatically. + async fn bridge_dds_pub( + &self, + topic: &str, + participant: &impl DdsParticipant, + ) -> Result>; + + /// Bridge a Zenoh subscriber to a DDS topic writer. + async fn bridge_dds_sub( + &self, + topic: &str, + participant: &impl DdsParticipant, + ) -> Result>; +} + +impl DdsBridgeExt for ZNode { + async fn bridge_dds_pub( + &self, + topic: &str, + participant: &impl DdsParticipant, + ) -> Result> { + let ros2_type = dds_type_to_ros2_type(T::type_name()); + let type_hash: TypeHash = T::type_hash(); + ZDdsPubBridge::new( + self, + topic, + &ros2_type, + Some(type_hash), + participant, + BridgeQos::default(), + true, + 10, + ) + .await + } + + async fn bridge_dds_sub( + &self, + topic: &str, + participant: &impl DdsParticipant, + ) -> Result> { + let ros2_type = dds_type_to_ros2_type(T::type_name()); + let type_hash: TypeHash = T::type_hash(); + ZDdsSubBridge::new( + self, + topic, + &ros2_type, + Some(type_hash), + participant, + BridgeQos::default(), + true, + ) + .await + } +} diff --git a/crates/ros-z-dds/src/gid.rs b/crates/ros-z-dds/src/gid.rs new file mode 100644 index 00000000..eebe3964 --- /dev/null +++ b/crates/ros-z-dds/src/gid.rs @@ -0,0 +1,38 @@ +use std::fmt; + +use serde::{Deserialize, Serialize}; + +/// 16-byte DDS Global Identifier (GID). +/// +/// Serializes as a raw 16-byte array (CDR `octet[16]`), matching the +/// Iron/Jazzy wire format in `ros_discovery_info`. +#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Default, Serialize, Deserialize)] +pub struct Gid([u8; 16]); + +impl From<[u8; 16]> for Gid { + fn from(b: [u8; 16]) -> Self { + Self(b) + } +} + +impl std::ops::Deref for Gid { + type Target = [u8; 16]; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl fmt::Debug for Gid { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + for b in &self.0 { + write!(f, "{b:02x}")?; + } + Ok(()) + } +} + +impl fmt::Display for Gid { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + fmt::Debug::fmt(self, f) + } +} diff --git a/crates/ros-z-dds/src/lib.rs b/crates/ros-z-dds/src/lib.rs new file mode 100644 index 00000000..1b593453 --- /dev/null +++ b/crates/ros-z-dds/src/lib.rs @@ -0,0 +1,24 @@ +//! ros-z-dds — DDS bridging as a first-class ros-z component. +//! +//! Provides `ZDdsPubBridge`, `ZDdsSubBridge`, `ZDdsServiceBridge`, `ZDdsClientBridge`, +//! and `ZDdsBridge` (auto-discovery). The `ros-z-bridge-dds` binary is a thin CLI +//! shell on top of `ZDdsBridge`. + +pub mod bridge; +pub(crate) mod cyclors; +pub(crate) mod discovery; +pub mod ext; +pub(crate) mod gid; +pub(crate) mod names; +pub mod participant; +pub mod pubsub; +pub(crate) mod qos; +pub(crate) mod ros_discovery; +pub mod service; + +pub use bridge::ZDdsBridge; +pub use cyclors::CyclorsParticipant; +pub use ext::DdsBridgeExt; +pub use participant::{BridgeQos, DdsParticipant}; +pub use pubsub::{ZDdsPubBridge, ZDdsSubBridge}; +pub use service::{ZDdsClientBridge, ZDdsServiceBridge}; diff --git a/crates/ros-z-dds/src/names.rs b/crates/ros-z-dds/src/names.rs new file mode 100644 index 00000000..be43393d --- /dev/null +++ b/crates/ros-z-dds/src/names.rs @@ -0,0 +1,277 @@ +use ros_z_protocol::entity::{TypeHash, TypeInfo}; + +/// Convert a ROS 2 pub/sub name (e.g. `/chatter`) to its DDS topic name (`rt/chatter`). +pub fn ros2_name_to_dds_pub_topic(ros2_name: &str) -> String { + let stripped = ros2_name.strip_prefix('/').unwrap_or(ros2_name); + format!("rt/{stripped}") +} + +/// Convert a ROS 2 service name (e.g. `/add_two_ints`) to the DDS request topic name. +pub fn ros2_name_to_dds_request_topic(ros2_name: &str) -> String { + let stripped = ros2_name.strip_prefix('/').unwrap_or(ros2_name); + format!("rq/{stripped}Request") +} + +/// Convert a ROS 2 service name to the DDS reply topic name. +pub fn ros2_name_to_dds_reply_topic(ros2_name: &str) -> String { + let stripped = ros2_name.strip_prefix('/').unwrap_or(ros2_name); + format!("rr/{stripped}Reply") +} + +const USER_DATA_TYPEHASH_PREFIX: &str = "typehash="; + +/// Parse the type hash from DDS `USER_DATA` QoS bytes. +/// +/// rmw_zenoh_cpp encodes it as `typehash=RIHS01_<64hex>;` (semicolons separate +/// multiple entries). Returns `None` when absent, malformed, or for Humble nodes +/// which do not set `user_data`. +pub fn type_info_from_user_data(type_name: &str, user_data: &[u8]) -> Option { + let s = std::str::from_utf8(user_data).ok()?; + for part in s.split(';') { + if let Some(rihs) = part.trim().strip_prefix(USER_DATA_TYPEHASH_PREFIX) + && let Some(hash) = TypeHash::from_rihs_string(rihs) + { + return Some(TypeInfo::new(type_name, hash)); + } + } + None +} + +/// Convert a DDS topic name to a ROS 2 topic/service name. +/// +/// DDS uses prefixes: `rt/` (topic), `rq/` (request), `rr/` (reply). +/// Returns `None` for unrecognised prefixes (e.g. built-in topics). +pub fn dds_topic_to_ros2_name(dds_topic: &str) -> Option { + // Topics: "rt/" → "/" + if let Some(rest) = dds_topic.strip_prefix("rt/") { + return Some(format!("/{rest}")); + } + // Service requests: "rq/Request" → "/" + if let Some(rest) = dds_topic.strip_prefix("rq/") + && let Some(name) = rest.strip_suffix("Request") + { + return Some(format!("/{name}")); + } + // Service replies: "rr/Reply" → "/" + if let Some(rest) = dds_topic.strip_prefix("rr/") + && let Some(name) = rest.strip_suffix("Reply") + { + return Some(format!("/{name}")); + } + None +} + +/// True if the DDS topic belongs to a ROS 2 service request channel. +pub fn is_request_topic(dds_topic: &str) -> bool { + dds_topic.starts_with("rq/") +} + +/// True if the DDS topic is a regular pub/sub topic. +pub fn is_pubsub_topic(dds_topic: &str) -> bool { + dds_topic.starts_with("rt/") +} + +/// Convert a DDS type string (e.g. `std_msgs::msg::dds_::String_`) to a ROS 2 type +/// (e.g. `std_msgs/msg/String`). +pub fn dds_type_to_ros2_type(dds_type: &str) -> String { + let result = dds_type.replace("::dds_::", "::").replace("::", "/"); + if result.ends_with('_') { + result[..result.len() - 1].into() + } else { + result + } +} + +/// Convert a ROS 2 type string to a DDS type string. +pub fn ros2_type_to_dds_type(ros2_type: &str) -> String { + let mut result = ros2_type.replace('/', "::"); + if let Some(pos) = result.rfind(':') { + result.insert_str(pos + 1, "dds_::"); + } + result.push('_'); + result +} + +/// Strip the service suffix (`_Request_` / `_Response_`) and convert to ROS 2 type. +pub fn dds_type_to_ros2_service_type(dds_type: &str) -> String { + dds_type_to_ros2_type( + dds_type + .strip_suffix("_Request_") + .or(dds_type.strip_suffix("_Response_")) + .unwrap_or(dds_type), + ) +} + +/// Strip the action-specific suffix and convert to a ROS 2 action type. +/// +/// Mirrors `dds_type_to_ros2_action_type` from zenoh-plugin-ros2dds. Handles all +/// five action component DDS type suffixes: `_SendGoal_{Request,Response}_`, +/// `_GetResult_{Request,Response}_`, and `_FeedbackMessage_`. +pub fn dds_type_to_ros2_action_type(dds_type: &str) -> String { + dds_type_to_ros2_type( + dds_type + .strip_suffix("_SendGoal_Request_") + .or(dds_type.strip_suffix("_SendGoal_Response_")) + .or(dds_type.strip_suffix("_GetResult_Request_")) + .or(dds_type.strip_suffix("_GetResult_Response_")) + .or(dds_type.strip_suffix("_FeedbackMessage_")) + .unwrap_or(dds_type), + ) +} + +/// True if the DDS request topic is an action `get_result` channel. +/// +/// Action get_result calls can block for hundreds of seconds while the goal executes. +/// They need a much longer querier timeout (300 s) than regular services (10 s). +pub fn is_action_get_result_topic(dds_topic: &str) -> bool { + // Pattern: "rq//_action/get_resultRequest" + dds_topic.starts_with("rq/") && dds_topic.ends_with("/_action/get_resultRequest") +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_action_get_result_topic() { + assert!(is_action_get_result_topic( + "rq/fibonacci/_action/get_resultRequest" + )); + assert!(is_action_get_result_topic( + "rq/my_ns/my_action/_action/get_resultRequest" + )); + assert!(!is_action_get_result_topic( + "rq/fibonacci/_action/send_goalRequest" + )); + assert!(!is_action_get_result_topic( + "rq/fibonacci/_action/cancel_goalRequest" + )); + assert!(!is_action_get_result_topic("rt/chatter")); + } + + #[test] + fn test_dds_topic_conversions() { + assert_eq!( + dds_topic_to_ros2_name("rt/chatter"), + Some("/chatter".into()) + ); + assert_eq!( + dds_topic_to_ros2_name("rq/add_two_intsRequest"), + Some("/add_two_ints".into()) + ); + assert_eq!( + dds_topic_to_ros2_name("rr/add_two_intsReply"), + Some("/add_two_ints".into()) + ); + assert_eq!(dds_topic_to_ros2_name("DCPS_something"), None); + } + + #[test] + fn test_type_conversions() { + assert_eq!( + dds_type_to_ros2_type("std_msgs::msg::dds_::String_"), + "std_msgs/msg/String" + ); + assert_eq!( + ros2_type_to_dds_type("std_msgs/msg/String"), + "std_msgs::msg::dds_::String_" + ); + assert_eq!( + dds_type_to_ros2_service_type("example_interfaces::srv::dds_::AddTwoInts_Request_"), + "example_interfaces/srv/AddTwoInts" + ); + assert_eq!( + dds_type_to_ros2_service_type("example_interfaces::srv::dds_::AddTwoInts_Response_"), + "example_interfaces/srv/AddTwoInts" + ); + assert_eq!( + dds_type_to_ros2_type("geometry_msgs::msg::dds_::Twist_"), + "geometry_msgs/msg/Twist" + ); + assert_eq!( + ros2_type_to_dds_type("geometry_msgs/msg/Twist"), + "geometry_msgs::msg::dds_::Twist_" + ); + } + + #[test] + fn test_action_type_conversions() { + let base = "example_interfaces/action/Fibonacci"; + assert_eq!( + dds_type_to_ros2_action_type( + "example_interfaces::action::dds_::Fibonacci_SendGoal_Request_" + ), + base + ); + assert_eq!( + dds_type_to_ros2_action_type( + "example_interfaces::action::dds_::Fibonacci_SendGoal_Response_" + ), + base + ); + assert_eq!( + dds_type_to_ros2_action_type( + "example_interfaces::action::dds_::Fibonacci_GetResult_Request_" + ), + base + ); + assert_eq!( + dds_type_to_ros2_action_type( + "example_interfaces::action::dds_::Fibonacci_GetResult_Response_" + ), + base + ); + assert_eq!( + dds_type_to_ros2_action_type( + "example_interfaces::action::dds_::Fibonacci_FeedbackMessage_" + ), + base + ); + } + + #[test] + fn test_is_request_topic() { + assert!(is_request_topic("rq/add_two_intsRequest")); + assert!(is_request_topic("rq/fibonacci/_action/send_goalRequest")); + assert!(!is_request_topic("rr/add_two_intsReply")); + assert!(!is_request_topic("rt/chatter")); + } + + #[test] + fn test_is_pubsub_topic() { + assert!(is_pubsub_topic("rt/chatter")); + assert!(is_pubsub_topic("rt/fibonacci/_action/feedback")); + assert!(!is_pubsub_topic("rq/add_two_intsRequest")); + assert!(!is_pubsub_topic("rr/add_two_intsReply")); + } + + #[test] + fn test_ros2_type_to_dds_type_roundtrip() { + let ros2_types = [ + "std_msgs/msg/String", + "geometry_msgs/msg/Twist", + "example_interfaces/srv/AddTwoInts", + "example_interfaces/action/Fibonacci", + ]; + for ros2 in ros2_types { + let dds = ros2_type_to_dds_type(ros2); + let back = dds_type_to_ros2_type(&dds); + assert_eq!( + back, ros2, + "roundtrip failed for {ros2}: dds={dds} back={back}" + ); + } + } + + #[test] + fn test_dds_topic_to_ros2_name_nested_namespace() { + assert_eq!( + dds_topic_to_ros2_name("rt/my_ns/chatter"), + Some("/my_ns/chatter".into()) + ); + assert_eq!( + dds_topic_to_ros2_name("rq/my_ns/add_two_intsRequest"), + Some("/my_ns/add_two_ints".into()) + ); + } +} diff --git a/crates/ros-z-dds/src/participant.rs b/crates/ros-z-dds/src/participant.rs new file mode 100644 index 00000000..c10fec3d --- /dev/null +++ b/crates/ros-z-dds/src/participant.rs @@ -0,0 +1,233 @@ +use std::time::Duration; + +use anyhow::Result; + +use crate::discovery::DiscoveryEvent; + +// ─── QoS kinds ─────────────────────────────────────────────────────────────── + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ReliabilityKind { + BestEffort, + Reliable, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum DurabilityKind { + Volatile, + TransientLocal, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum HistoryKind { + KeepLast, + KeepAll, +} + +// ─── QoS policy structs ─────────────────────────────────────────────────────── + +#[derive(Debug, Clone, PartialEq)] +pub struct Reliability { + pub kind: ReliabilityKind, + /// `None` = DDS infinite blocking time. + pub max_blocking_time: Option, +} + +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct Durability { + pub kind: DurabilityKind, +} + +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct History { + pub kind: HistoryKind, + pub depth: i32, +} + +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct DurabilityService { + /// `None` = DDS infinite cleanup delay. + pub service_cleanup_delay: Option, + pub history_kind: HistoryKind, + pub history_depth: i32, + /// `None` = DDS_LENGTH_UNLIMITED. + pub max_samples: Option, + /// `None` = DDS_LENGTH_UNLIMITED. + pub max_instances: Option, + /// `None` = DDS_LENGTH_UNLIMITED. + pub max_samples_per_instance: Option, +} + +// ─── BridgeQos ──────────────────────────────────────────────────────────────── + +/// Backend-independent QoS representation for the bridge. +/// +/// Covers the DDS-standard policies needed for pub/sub and service routing. +/// Backend-specific fields (e.g. CycloneDDS `entity_name`, `properties`, +/// `writer_batching`) are handled inside each backend's conversion layer. +#[derive(Debug, Clone, PartialEq, Default)] +pub struct BridgeQos { + pub reliability: Option, + pub durability: Option, + pub history: Option, + pub durability_service: Option, + /// `None` = infinite deadline. + pub deadline: Option, + pub latency_budget: Option, + /// `None` = infinite lifespan. + pub lifespan: Option, + pub user_data: Option>, + /// When true the reader/writer ignores traffic from other endpoints + /// in the same DDS participant (IgnoreLocal::PARTICIPANT). + pub ignore_local: bool, +} + +// ─── Backend traits ─────────────────────────────────────────────────────────── + +/// A live DDS participant on a single domain that can create readers/writers +/// and stream discovery events. +/// +/// Implement this trait to plug in a DDS library. The cyclors (CycloneDDS) +/// implementation lives in `dds::cyclors`. +pub trait DdsParticipant: Send + Sync + 'static { + /// RAII reader handle — dropped when the reader should be destroyed. + type Reader: DdsReader; + /// Writer handle that exposes `write_cdr`, `instance_handle`, and `guid`. + type Writer: DdsWriter; + + /// Create a participant on `domain_id`. + fn create(domain_id: u32) -> Result + where + Self: Sized; + + /// Return this participant's 16-byte DDS GUID. + fn participant_guid(&self) -> Result<[u8; 16]>; + + /// Start the builtin-topic discovery loop. + /// + /// Endpoint events are sent on the returned channel until the participant + /// (and therefore the channel sender) is dropped. + fn run_discovery(&self) -> flume::Receiver; + + /// Create a CDR blob reader; `callback` is called for each valid sample. + /// + /// The `Vec` passed to `callback` includes the 4-byte CDR representation + /// header followed by the payload, matching the raw DDS wire format. + fn create_reader( + &self, + topic: &str, + type_name: &str, + keyless: bool, + qos: BridgeQos, + callback: Box) + Send + 'static>, + ) -> Result; + + /// Create a CDR blob writer. + fn create_writer( + &self, + topic: &str, + type_name: &str, + keyless: bool, + qos: BridgeQos, + ) -> Result; +} + +/// A live DDS reader; minimal trait so the bridge can obtain its GUID. +pub trait DdsReader: Send + Sync + 'static { + /// Return this reader's 16-byte DDS GUID. + fn guid(&self) -> Result<[u8; 16]>; +} + +/// A live DDS writer that can send raw CDR blobs. +pub trait DdsWriter: Send + Sync + 'static { + /// Write raw CDR bytes (4-byte representation header + payload) to DDS. + fn write_cdr(&self, data: Vec) -> Result<()>; + + /// Return this writer's stable instance handle (GUID fragment). + /// + /// Used as the `client_guid` in ROS 2 service request headers so that + /// the DDS server routes replies back to this specific writer. + fn instance_handle(&self) -> Result; + + /// Return this writer's 16-byte DDS GUID. + fn guid(&self) -> Result<[u8; 16]>; +} + +#[cfg(test)] +mod tests { + use super::*; + + // ── wire_discriminant values must match zenoh-plugin-ros2dds ───────────── + // References: zenoh-plugin-ros2dds liveliness_mgt.rs, which encodes these as + // integers in the liveliness key expression for cross-bridge entity matching. + + impl ReliabilityKind { + fn wire_discriminant(self) -> i32 { + match self { + Self::BestEffort => 0, + Self::Reliable => 1, + } + } + } + + impl DurabilityKind { + fn wire_discriminant(self) -> i32 { + match self { + Self::Volatile => 0, + Self::TransientLocal => 1, + } + } + } + + impl HistoryKind { + fn wire_discriminant(self) -> i32 { + match self { + Self::KeepLast => 0, + Self::KeepAll => 1, + } + } + } + + #[test] + fn test_reliability_wire_discriminants() { + assert_eq!(ReliabilityKind::BestEffort.wire_discriminant(), 0); + assert_eq!(ReliabilityKind::Reliable.wire_discriminant(), 1); + } + + #[test] + fn test_durability_wire_discriminants() { + assert_eq!(DurabilityKind::Volatile.wire_discriminant(), 0); + assert_eq!(DurabilityKind::TransientLocal.wire_discriminant(), 1); + } + + #[test] + fn test_history_wire_discriminants() { + assert_eq!(HistoryKind::KeepLast.wire_discriminant(), 0); + assert_eq!(HistoryKind::KeepAll.wire_discriminant(), 1); + } + + #[test] + fn test_bridge_qos_default_has_no_policies() { + let qos = BridgeQos::default(); + assert!(qos.reliability.is_none()); + assert!(qos.durability.is_none()); + assert!(qos.history.is_none()); + assert!(qos.durability_service.is_none()); + assert!(qos.user_data.is_none()); + assert!(!qos.ignore_local); + } + + #[test] + fn test_bridge_qos_equality() { + use std::time::Duration; + let a = BridgeQos { + reliability: Some(Reliability { + kind: ReliabilityKind::Reliable, + max_blocking_time: Some(Duration::from_millis(100)), + }), + ..Default::default() + }; + let b = a.clone(); + assert_eq!(a, b); + } +} diff --git a/crates/ros-z-dds/src/pubsub.rs b/crates/ros-z-dds/src/pubsub.rs new file mode 100644 index 00000000..1f679f7c --- /dev/null +++ b/crates/ros-z-dds/src/pubsub.rs @@ -0,0 +1,449 @@ +//! ZDdsPubBridge (DDS→Zenoh) and ZDdsSubBridge (Zenoh→DDS). + +use std::sync::Arc; + +use anyhow::{Result, anyhow}; +use ros_z::node::ZNode; +use ros_z_protocol::entity::{EndpointEntity, EndpointKind, TypeHash, TypeInfo}; +use zenoh::{Wait, bytes::ZBytes, liveliness::LivelinessToken, pubsub::Publisher}; +use zenoh_ext::{ + AdvancedPublisher, AdvancedPublisherBuilderExt, AdvancedSubscriber, + AdvancedSubscriberBuilderExt, CacheConfig, HistoryConfig, +}; + +use crate::{ + gid::Gid, + names::{ros2_name_to_dds_pub_topic, ros2_type_to_dds_type}, + participant::{BridgeQos, DdsParticipant, DdsReader, DdsWriter, DurabilityKind, HistoryKind}, + qos::{adapt_reader_qos_for_writer, adapt_writer_qos_for_reader, bridge_qos_to_qos_profile}, +}; + +// ─── Zenoh publisher wrapper ────────────────────────────────────────────────── + +enum BridgePublisher { + Plain(Publisher<'static>), + Advanced(AdvancedPublisher<'static>), +} + +unsafe impl Send for BridgePublisher {} +unsafe impl Sync for BridgePublisher {} + +impl BridgePublisher { + fn put(&self, bytes: ZBytes) -> Result<()> { + match self { + Self::Plain(p) => p.put(bytes).wait().map_err(|e| anyhow!("{e}")), + Self::Advanced(p) => p.put(bytes).wait().map_err(|e| anyhow!("{e}")), + } + } +} + +// ─── Zenoh subscriber wrapper ───────────────────────────────────────────────── + +#[allow(dead_code)] +enum ZenohSubHandle { + Plain(zenoh::pubsub::Subscriber<()>), + Advanced(AdvancedSubscriber<()>), +} + +unsafe impl Send for ZenohSubHandle {} +unsafe impl Sync for ZenohSubHandle {} + +// ─── ZDdsPubBridge ──────────────────────────────────────────────────────────── + +/// Routes a DDS publication to a Zenoh publisher. +/// +/// The DDS reader is created on the raw DDS topic. On every sample the raw CDR bytes are +/// forwarded as-is to a Zenoh publisher declared on the rmw_zenoh (or ros2dds) key expression. +/// A liveliness token is declared so that `ZContext::graph()` and `ros2 topic list` see the +/// bridge as a publisher on the topic. +pub struct ZDdsPubBridge { + _dds_reader: P::Reader, + _publisher: Arc, + _lv_token: LivelinessToken, +} + +unsafe impl Send for ZDdsPubBridge

{} +unsafe impl Sync for ZDdsPubBridge

{} + +impl ZDdsPubBridge

{ + /// Create a new DDS→Zenoh publisher bridge. + /// + /// - `ros2_name` — ROS 2 topic name (e.g. `/chatter`) + /// - `ros2_type` — ROS 2 message type (e.g. `std_msgs/msg/String`) + /// - `type_hash` — optional RIHS01 type hash from DDS `user_data`; uses `EMPTY_TOPIC_HASH` when `None` + /// - `qos` — DDS QoS of the upstream publisher + /// - `keyless` — whether the DDS topic has no key fields + /// - `cache_multiplier` — TRANSIENT_LOCAL cache depth multiplier (pass 10 for the plugin default) + #[allow(clippy::too_many_arguments)] + pub async fn new( + node: &ZNode, + ros2_name: &str, + ros2_type: &str, + type_hash: Option, + participant: &P, + qos: BridgeQos, + keyless: bool, + cache_multiplier: usize, + ) -> Result { + let entity_id = node.next_entity_id(); + let qos_profile = bridge_qos_to_qos_profile(&qos); + let type_info = type_hash + .as_ref() + .map(|h| TypeInfo::new(ros2_type, h.clone())); + let entity = EndpointEntity { + id: entity_id, + node: Some(node.node_entity().clone()), + kind: EndpointKind::Publisher, + topic: ros2_name.to_string(), + type_info, + qos: qos_profile, + }; + // Liveliness entity always carries the type name (with zero hash when hash unknown) + // so remote bridges can reconstruct DDS routes for federation. + let lv_type_hash = type_hash.unwrap_or_else(TypeHash::zero); + let entity_lv = EndpointEntity { + id: entity_id, + node: Some(node.node_entity().clone()), + kind: EndpointKind::Publisher, + topic: ros2_name.to_string(), + type_info: Some(TypeInfo::new(ros2_type, lv_type_hash)), + qos: qos_profile, + }; + + let topic_ke = node + .keyexpr_format() + .topic_key_expr(&entity) + .map_err(|e| anyhow!("topic_key_expr failed: {e}"))?; + let lv_ke = node + .keyexpr_format() + .liveliness_key_expr(&entity_lv, &node.session().zid()) + .map_err(|e| anyhow!("liveliness_key_expr failed: {e}"))?; + + let ke: zenoh::key_expr::OwnedKeyExpr = topic_ke + .as_str() + .to_owned() + .try_into() + .map_err(|e| anyhow!("invalid key expr: {e}"))?; + + let is_transient_local = qos + .durability + .as_ref() + .is_some_and(|d| d.kind == DurabilityKind::TransientLocal); + + let publisher = if is_transient_local { + let cache_size = compute_cache_size(&qos, keyless, cache_multiplier); + tracing::debug!("ZDdsPubBridge: TRANSIENT_LOCAL {ros2_name}, cache_size={cache_size}"); + let adv = node + .session() + .declare_publisher(ke) + .cache(CacheConfig::default().max_samples(cache_size)) + .publisher_detection() + .await + .map_err(|e| anyhow!("declare_publisher(advanced) failed: {e}"))?; + Arc::new(BridgePublisher::Advanced(adv)) + } else { + let plain = node + .session() + .declare_publisher(ke) + .await + .map_err(|e| anyhow!("declare_publisher failed: {e}"))?; + Arc::new(BridgePublisher::Plain(plain)) + }; + + let dds_topic = ros2_name_to_dds_pub_topic(ros2_name); + let dds_type = ros2_type_to_dds_type(ros2_type); + let dds_reader_qos = adapt_writer_qos_for_reader(&qos); + + let pub_clone = Arc::clone(&publisher); + let ke_display = topic_ke.as_str().to_string(); + let dds_reader = participant.create_reader( + &dds_topic, + &dds_type, + keyless, + dds_reader_qos, + Box::new(move |bytes: Vec| { + if let Err(e) = pub_clone.put(ZBytes::from(bytes.as_slice())) { + tracing::warn!("ZDdsPubBridge: Zenoh put failed on {ke_display}: {e}"); + } + }), + )?; + + let lv_token = node + .session() + .liveliness() + .declare_token((*lv_ke).clone()) + .await + .map_err(|e| anyhow!("liveliness token failed: {e}"))?; + + tracing::info!( + "ZDdsPubBridge: DDS {} → Zenoh {}", + dds_topic, + topic_ke.as_str() + ); + + Ok(Self { + _dds_reader: dds_reader, + _publisher: publisher, + _lv_token: lv_token, + }) + } + + /// Return the DDS reader GID created for this bridge route. + pub(crate) fn reader_guid(&self) -> Option { + self._dds_reader.guid().ok().map(Gid::from) + } +} + +// ─── ZDdsSubBridge ──────────────────────────────────────────────────────────── + +/// Routes a Zenoh subscription to a DDS writer. +/// +/// In rmw-zenoh mode the Zenoh subscriber is declared on a wildcard key (`{domain}/{topic}/**`) +/// so it receives from any Zenoh publisher on the topic regardless of type hash. In ros2dds mode +/// the exact key expression is used. On every sample the raw CDR bytes are forwarded as-is to a +/// DDS writer. A liveliness token is declared with `Subscription` kind so that `ros2 topic list` +/// sees the bridge as a subscriber. +pub struct ZDdsSubBridge { + _writer: Arc, + _subscriber: ZenohSubHandle, + _lv_token: LivelinessToken, +} + +unsafe impl Send for ZDdsSubBridge

{} +unsafe impl Sync for ZDdsSubBridge

{} + +impl ZDdsSubBridge

{ + /// Create a new Zenoh→DDS subscriber bridge. + /// + /// - `ros2_name` — ROS 2 topic name (e.g. `/chatter`) + /// - `ros2_type` — ROS 2 message type (e.g. `std_msgs/msg/String`) + /// - `type_hash` — optional RIHS01 type hash; only used in the liveliness token so remote + /// bridges reconstruct the correct DDS type. The subscriber key expression is always a + /// wildcard in rmw-zenoh mode, so messages from any type hash are forwarded. + /// - `keyless` — whether the DDS topic has no key fields + pub async fn new( + node: &ZNode, + ros2_name: &str, + ros2_type: &str, + type_hash: Option, + participant: &P, + qos: BridgeQos, + keyless: bool, + ) -> Result { + let entity_id = node.next_entity_id(); + let qos_profile = bridge_qos_to_qos_profile(&qos); + let lv_type_hash = type_hash.unwrap_or_else(TypeHash::zero); + // Routing entity: no type_info so topic_key_expr returns the EMPTY placeholder key. + // We then widen it to a wildcard so the subscriber catches any Zenoh publisher on + // this topic regardless of what type name / hash the publisher uses. + let entity = EndpointEntity { + id: entity_id, + node: Some(node.node_entity().clone()), + kind: EndpointKind::Subscription, + topic: ros2_name.to_string(), + type_info: None, + qos: qos_profile, + }; + let entity_lv = EndpointEntity { + id: entity_id, + node: Some(node.node_entity().clone()), + kind: EndpointKind::Subscription, + topic: ros2_name.to_string(), + type_info: Some(TypeInfo::new(ros2_type, lv_type_hash)), + qos: qos_profile, + }; + + let topic_ke = node + .keyexpr_format() + .topic_key_expr(&entity) + .map_err(|e| anyhow!("topic_key_expr failed: {e}"))?; + let lv_ke = node + .keyexpr_format() + .liveliness_key_expr(&entity_lv, &node.session().zid()) + .map_err(|e| anyhow!("liveliness_key_expr failed: {e}"))?; + + // Convert "…/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH" → "…/**" for rmw-zenoh keys so + // the subscriber matches any publisher type/hash on this topic. For ros2dds keys + // (which have no type/hash components) the suffix is absent and the key is used as-is. + let ke_str = topic_ke.as_str(); + let ke_str = ke_str + .strip_suffix("/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .map(|prefix| format!("{prefix}/**")) + .unwrap_or_else(|| ke_str.to_string()); + let ke: zenoh::key_expr::OwnedKeyExpr = ke_str + .try_into() + .map_err(|e| anyhow!("invalid key expr: {e}"))?; + + let dds_topic = ros2_name_to_dds_pub_topic(ros2_name); + let dds_type = ros2_type_to_dds_type(ros2_type); + let dds_writer_qos = adapt_reader_qos_for_writer(&qos); + + let writer = + Arc::new(participant.create_writer(&dds_topic, &dds_type, keyless, dds_writer_qos)?); + + let is_transient_local = qos + .durability + .as_ref() + .is_some_and(|d| d.kind == DurabilityKind::TransientLocal); + + let writer_cb = Arc::clone(&writer); + let ke_display = ke.as_str().to_string(); + let dds_topic_log = dds_topic.clone(); + + let subscriber = if is_transient_local { + tracing::debug!("ZDdsSubBridge: TRANSIENT_LOCAL {ros2_name}, using AdvancedSubscriber"); + let adv = node + .session() + .declare_subscriber(ke) + .history(HistoryConfig::default().detect_late_publishers()) + .callback(move |sample| { + let bytes: Vec = sample.payload().to_bytes().into_owned(); + if let Err(e) = writer_cb.write_cdr(bytes) { + tracing::warn!("ZDdsSubBridge: DDS write failed on {dds_topic_log}: {e}"); + } + }) + .await + .map_err(|e| anyhow!("declare_subscriber(advanced) failed: {e}"))?; + ZenohSubHandle::Advanced(adv) + } else { + let sub = node + .session() + .declare_subscriber(ke) + .callback(move |sample| { + let bytes: Vec = sample.payload().to_bytes().into_owned(); + if let Err(e) = writer_cb.write_cdr(bytes) { + tracing::warn!("ZDdsSubBridge: DDS write failed on {dds_topic_log}: {e}"); + } + }) + .await + .map_err(|e| anyhow!("declare_subscriber failed: {e}"))?; + ZenohSubHandle::Plain(sub) + }; + + let lv_token = node + .session() + .liveliness() + .declare_token((*lv_ke).clone()) + .await + .map_err(|e| anyhow!("liveliness token failed: {e}"))?; + + tracing::info!("ZDdsSubBridge: Zenoh {} → DDS {}", ke_display, dds_topic); + + Ok(Self { + _writer: writer, + _subscriber: subscriber, + _lv_token: lv_token, + }) + } + + /// Return the DDS writer GID created for this bridge route. + pub(crate) fn writer_guid(&self) -> Option { + self._writer.guid().ok().map(Gid::from) + } +} + +// ─── compute_cache_size ─────────────────────────────────────────────────────── + +/// Compute the AdvancedPublisher cache size from DDS QoS. +/// +/// Mirrors the zenoh-plugin-ros2dds formula: +/// - KEEP_ALL → `usize::MAX` +/// - keyless topics → `depth × multiplier` +/// - KEEP_LAST, unlimited instances → `usize::MAX` +/// - KEEP_LAST, N instances → `depth × N × multiplier` +fn compute_cache_size(qos: &BridgeQos, keyless: bool, multiplier: usize) -> usize { + let depth = match &qos.history { + Some(h) => match h.kind { + HistoryKind::KeepAll => return usize::MAX, + HistoryKind::KeepLast => h.depth as usize, + }, + None => 1, + }; + + if keyless { + return depth.saturating_mul(multiplier); + } + + match qos + .durability_service + .as_ref() + .and_then(|ds| ds.max_instances) + { + None => usize::MAX, + Some(n) => depth.saturating_mul(n).saturating_mul(multiplier), + } +} + +// ─── tests ──────────────────────────────────────────────────────────────────── + +#[cfg(test)] +mod tests { + use crate::participant::{ + BridgeQos, Durability, DurabilityKind, DurabilityService, History, HistoryKind, + }; + + use super::compute_cache_size; + + fn qos_transient_local(depth: i32) -> BridgeQos { + BridgeQos { + durability: Some(Durability { + kind: DurabilityKind::TransientLocal, + }), + history: Some(History { + kind: HistoryKind::KeepLast, + depth, + }), + ..Default::default() + } + } + + #[test] + fn test_cache_size_keep_all() { + let qos = BridgeQos { + history: Some(History { + kind: HistoryKind::KeepAll, + depth: 0, + }), + ..Default::default() + }; + assert_eq!(compute_cache_size(&qos, true, 10), usize::MAX); + } + + #[test] + fn test_cache_size_keyless_keep_last() { + let qos = qos_transient_local(5); + assert_eq!(compute_cache_size(&qos, true, 10), 50); + } + + #[test] + fn test_cache_size_keyed_unlimited_instances() { + let qos = qos_transient_local(5); + assert_eq!(compute_cache_size(&qos, false, 10), usize::MAX); + } + + #[test] + fn test_cache_size_keyed_limited_instances() { + let qos = BridgeQos { + history: Some(History { + kind: HistoryKind::KeepLast, + depth: 5, + }), + durability_service: Some(DurabilityService { + service_cleanup_delay: None, + history_kind: HistoryKind::KeepLast, + history_depth: 5, + max_samples: None, + max_instances: Some(3), + max_samples_per_instance: None, + }), + ..Default::default() + }; + assert_eq!(compute_cache_size(&qos, false, 10), 150); + } + + #[test] + fn test_cache_size_no_history_keyless() { + let qos = BridgeQos::default(); + assert_eq!(compute_cache_size(&qos, true, 10), 10); + } +} diff --git a/crates/ros-z-dds/src/qos.rs b/crates/ros-z-dds/src/qos.rs new file mode 100644 index 00000000..10f1ede5 --- /dev/null +++ b/crates/ros-z-dds/src/qos.rs @@ -0,0 +1,77 @@ +use ros_z_protocol::qos::{QosDurability, QosHistory, QosProfile, QosReliability}; + +use crate::participant::{ + BridgeQos, Durability, DurabilityKind, History, HistoryKind, Reliability, ReliabilityKind, +}; + +pub use crate::cyclors::qos::{adapt_reader_qos_for_writer, adapt_writer_qos_for_reader}; + +/// Convert `ros-z-protocol` `QosProfile` back to `BridgeQos` (for creating DDS entities from +/// parsed liveliness tokens). +pub fn qos_profile_to_bridge_qos(qos: &QosProfile) -> BridgeQos { + let reliability = Some(Reliability { + kind: match qos.reliability { + QosReliability::Reliable => ReliabilityKind::Reliable, + QosReliability::BestEffort => ReliabilityKind::BestEffort, + }, + max_blocking_time: None, + }); + let durability = Some(Durability { + kind: match qos.durability { + QosDurability::TransientLocal => DurabilityKind::TransientLocal, + _ => DurabilityKind::Volatile, + }, + }); + let history = Some(History { + kind: match qos.history { + QosHistory::KeepAll => HistoryKind::KeepAll, + _ => HistoryKind::KeepLast, + }, + depth: match qos.history { + QosHistory::KeepLast(n) => n as i32, + _ => 1, + }, + }); + BridgeQos { + reliability, + durability, + history, + ..Default::default() + } +} + +/// Convert `BridgeQos` (DDS QoS) to `ros-z-protocol` `QosProfile` for key construction. +pub fn bridge_qos_to_qos_profile(qos: &BridgeQos) -> QosProfile { + let reliability = qos + .reliability + .as_ref() + .map(|r| match r.kind { + ReliabilityKind::Reliable => QosReliability::Reliable, + ReliabilityKind::BestEffort => QosReliability::BestEffort, + }) + .unwrap_or_default(); + + let durability = qos + .durability + .as_ref() + .map(|d| match d.kind { + DurabilityKind::TransientLocal => QosDurability::TransientLocal, + _ => QosDurability::Volatile, + }) + .unwrap_or_default(); + + let history = qos + .history + .as_ref() + .map(|h| match h.kind { + HistoryKind::KeepAll => QosHistory::KeepAll, + HistoryKind::KeepLast => QosHistory::KeepLast(h.depth as usize), + }) + .unwrap_or_default(); + + QosProfile { + reliability, + durability, + history, + } +} diff --git a/crates/ros-z-dds/src/ros_discovery.rs b/crates/ros-z-dds/src/ros_discovery.rs new file mode 100644 index 00000000..a3715a7d --- /dev/null +++ b/crates/ros-z-dds/src/ros_discovery.rs @@ -0,0 +1,277 @@ +//! `ros_discovery_info` publishing — makes bridge DDS entities visible to the ROS 2 graph. +//! +//! Publishes a `ParticipantEntitiesInfo` CDR-LE message on the `ros_discovery_info` DDS topic +//! at ~1 Hz whenever the set of bridged DDS readers/writers changes. This allows standard ROS 2 +//! tools (`ros2 topic list`, `ros2 node list`, `ros2 service list`) to see the bridge. +//! +//! Wire format: CDR-LE with 16-byte GIDs (Iron/Jazzy compatible). + +use std::{collections::HashSet, sync::Arc, time::Duration}; + +use anyhow::Result; +use parking_lot::Mutex; +use serde::{Deserialize, Serialize}; +use tokio::task::JoinHandle; + +use crate::{ + gid::Gid, + participant::{ + BridgeQos, DdsParticipant, DdsWriter, Durability, DurabilityKind, History, HistoryKind, + Reliability, ReliabilityKind, + }, +}; + +const ROS_DISCOVERY_INFO_TOPIC_NAME: &str = "ros_discovery_info"; +const ROS_DISCOVERY_INFO_TOPIC_TYPE: &str = "rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_"; +const PUBLISH_INTERVAL_MS: u64 = 1000; + +// ─── CDR wire structs ──────────────────────────────────────────────────────── + +#[derive(Serialize, Deserialize, Clone)] +struct NodeEntitiesInfo { + node_namespace: String, + node_name: String, + reader_gid_seq: Vec, + writer_gid_seq: Vec, +} + +#[derive(Serialize, Deserialize, Clone)] +struct ParticipantEntitiesInfo { + gid: Gid, + node_entities_info_seq: Vec, +} + +// ─── Internal state ─────────────────────────────────────────────────────────── + +struct State { + participant_gid: Gid, + node_namespace: String, + node_name: String, + reader_gids: HashSet, + writer_gids: HashSet, + changed: bool, +} + +impl State { + fn serialize_cdr(&self) -> Option> { + let info = ParticipantEntitiesInfo { + gid: self.participant_gid, + node_entities_info_seq: vec![NodeEntitiesInfo { + node_namespace: self.node_namespace.clone(), + node_name: self.node_name.clone(), + reader_gid_seq: self.reader_gids.iter().cloned().collect(), + writer_gid_seq: self.writer_gids.iter().cloned().collect(), + }], + }; + match cdr::serialize::<_, _, cdr::CdrLe>(&info, cdr::Infinite) { + Ok(bytes) => Some(bytes), + Err(e) => { + tracing::warn!("ros_discovery_info: CDR serialization failed: {e}"); + None + } + } + } +} + +// ─── RosDiscoveryPublisher ──────────────────────────────────────────────────── + +/// Publishes `ros_discovery_info` CDR messages so the ROS 2 graph can see bridge endpoints. +/// +/// Created once per [`ZDdsBridge`](crate::bridge::ZDdsBridge). Call +/// [`add_reader`] / [`add_writer`] as DDS bridge routes are established, and the +/// matching `remove_*` methods when routes are torn down. Changes are broadcast +/// to the DDS participant graph at ~1 Hz. +pub struct RosDiscoveryPublisher { + state: Arc>, + _task: JoinHandle<()>, +} + +impl RosDiscoveryPublisher { + /// Create a new publisher backed by `participant`. + /// + /// - `namespace` — bridge node namespace (e.g. `"/"`) + /// - `node_name` — bridge node name (e.g. `"ros_z_bridge"`) + pub fn new( + participant: &P, + namespace: &str, + node_name: &str, + ) -> Result { + let participant_gid = Gid::from(participant.participant_guid()?); + + let qos = BridgeQos { + reliability: Some(Reliability { + kind: ReliabilityKind::Reliable, + max_blocking_time: None, + }), + durability: Some(Durability { + kind: DurabilityKind::TransientLocal, + }), + history: Some(History { + kind: HistoryKind::KeepLast, + depth: 1, + }), + ignore_local: true, + ..Default::default() + }; + + let writer = participant.create_writer( + ROS_DISCOVERY_INFO_TOPIC_NAME, + ROS_DISCOVERY_INFO_TOPIC_TYPE, + true, + qos, + )?; + + let state = Arc::new(Mutex::new(State { + participant_gid, + node_namespace: namespace.to_string(), + node_name: node_name.to_string(), + reader_gids: HashSet::new(), + writer_gids: HashSet::new(), + changed: true, + })); + + let state_bg = Arc::clone(&state); + let task = tokio::spawn(async move { + let mut interval = tokio::time::interval(Duration::from_millis(PUBLISH_INTERVAL_MS)); + interval.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Delay); + loop { + interval.tick().await; + let payload = { + let mut s = state_bg.lock(); + if !s.changed { + continue; + } + s.changed = false; + s.serialize_cdr() + }; + if let Some(bytes) = payload { + if let Err(e) = writer.write_cdr(bytes) { + tracing::warn!("ros_discovery_info: DDS write failed: {e}"); + } else { + tracing::debug!("ros_discovery_info: published update"); + } + } + } + }); + + tracing::info!( + "ros_discovery_info: publishing for {}{} (participant GID {})", + namespace, + node_name, + participant_gid, + ); + + Ok(Self { state, _task: task }) + } + + pub fn add_reader(&self, gid: Gid) { + let mut s = self.state.lock(); + s.reader_gids.insert(gid); + s.changed = true; + tracing::debug!("ros_discovery_info: +reader {gid}"); + } + + pub fn remove_reader(&self, gid: Gid) { + let mut s = self.state.lock(); + s.reader_gids.remove(&gid); + s.changed = true; + tracing::debug!("ros_discovery_info: -reader {gid}"); + } + + pub fn add_writer(&self, gid: Gid) { + let mut s = self.state.lock(); + s.writer_gids.insert(gid); + s.changed = true; + tracing::debug!("ros_discovery_info: +writer {gid}"); + } + + pub fn remove_writer(&self, gid: Gid) { + let mut s = self.state.lock(); + s.writer_gids.remove(&gid); + s.changed = true; + tracing::debug!("ros_discovery_info: -writer {gid}"); + } +} + +// ─── tests ──────────────────────────────────────────────────────────────────── + +#[cfg(test)] +mod tests { + use super::*; + + fn make_gid(byte: u8) -> Gid { + Gid::from([byte; 16]) + } + + #[test] + fn test_state_serializes_to_valid_cdr() { + let state = State { + participant_gid: make_gid(0x01), + node_namespace: "/".to_string(), + node_name: "bridge".to_string(), + reader_gids: HashSet::new(), + writer_gids: HashSet::new(), + changed: false, + }; + let bytes = state.serialize_cdr().expect("serialization failed"); + // CDR header (4) + participant gid (16) + seq len (4) + 1 node entry + assert!( + bytes.len() >= 24, + "CDR output too short: {} bytes", + bytes.len() + ); + // CDR LE header = [0, 1, 0, 0] + assert_eq!(&bytes[..4], &[0, 1, 0, 0]); + // participant gid bytes + assert_eq!(&bytes[4..20], &[0x01u8; 16]); + // node_entities_info_seq length = 1 + assert_eq!(&bytes[20..24], &[1, 0, 0, 0]); + } + + #[test] + fn test_state_serializes_gids_in_reader_writer_seqs() { + let mut state = State { + participant_gid: make_gid(0x02), + node_namespace: "/".to_string(), + node_name: "bridge".to_string(), + reader_gids: HashSet::from([make_gid(0xAA)]), + writer_gids: HashSet::from([make_gid(0xBB), make_gid(0xCC)]), + changed: false, + }; + let bytes = state.serialize_cdr().expect("serialization failed"); + // Round-trip: count reader GIDs and writer GIDs embedded in the payload + let reader_count = bytes.windows(16).filter(|w| *w == [0xAA; 16]).count(); + let writer_count = bytes + .windows(16) + .filter(|w| *w == [0xBB; 16] || *w == [0xCC; 16]) + .count(); + assert_eq!(reader_count, 1); + assert_eq!(writer_count, 2); + // Swap and verify changed flag works + state.changed = true; + let bytes2 = state.serialize_cdr(); + assert!(bytes2.is_some()); + } + + #[test] + fn test_add_remove_reader_writer() { + let gid_a = make_gid(0x10); + let gid_b = make_gid(0x20); + let mut state = State { + participant_gid: make_gid(0x00), + node_namespace: "/".to_string(), + node_name: "bridge".to_string(), + reader_gids: HashSet::new(), + writer_gids: HashSet::new(), + changed: false, + }; + + state.reader_gids.insert(gid_a); + state.writer_gids.insert(gid_b); + assert!(state.reader_gids.contains(&gid_a)); + assert!(state.writer_gids.contains(&gid_b)); + + state.reader_gids.remove(&gid_a); + assert!(!state.reader_gids.contains(&gid_a)); + } +} diff --git a/crates/ros-z-dds/src/service.rs b/crates/ros-z-dds/src/service.rs new file mode 100644 index 00000000..f50541a3 --- /dev/null +++ b/crates/ros-z-dds/src/service.rs @@ -0,0 +1,532 @@ +//! ZDdsServiceBridge (DDS server→Zenoh queryable) and ZDdsClientBridge (DDS client→Zenoh querier). + +use std::{ + collections::HashMap, + sync::{ + Arc, + atomic::{AtomicU64, Ordering}, + }, + time::Duration, +}; + +use anyhow::{Result, anyhow}; +use parking_lot::Mutex; +use ros_z::node::ZNode; +use ros_z_protocol::entity::{EndpointEntity, EndpointKind, TypeHash, TypeInfo}; +use zenoh::{ + Wait, + bytes::ZBytes, + query::{Query, Queryable}, +}; + +use crate::{ + gid::Gid, + names::{ros2_name_to_dds_reply_topic, ros2_name_to_dds_request_topic, ros2_type_to_dds_type}, + participant::{BridgeQos, DdsParticipant, DdsReader, DdsWriter}, + qos::{adapt_reader_qos_for_writer, adapt_writer_qos_for_reader, bridge_qos_to_qos_profile}, +}; + +// ─── CDR helpers ───────────────────────────────────────────────────────────── + +const CDR_HEADER_LE: [u8; 4] = [0, 1, 0, 0]; +const CDR_HEADER_BE: [u8; 4] = [0, 0, 0, 0]; + +fn cdr_header_matching(payload: &[u8]) -> [u8; 4] { + if payload.get(1).copied().unwrap_or(1) == 1 { + CDR_HEADER_LE + } else { + CDR_HEADER_BE + } +} + +/// Sequence number embedded in every ROS 2 CDR service payload. +/// +/// Layout: [4-byte CDR header] [8-byte client_guid] [8-byte seq_num LE] [payload] +fn extract_sequence_number(raw: &[u8]) -> Option { + if raw.len() < 20 { + return None; + } + Some(u64::from_le_bytes(raw[12..20].try_into().unwrap())) +} + +/// Derive the DDS reply type name from the request type name. +/// +/// `example_interfaces::srv::dds_::AddTwoInts_Request_` → `…_Response_` +fn reply_type_from_request_type(req_type: &str, ros2_type: &str) -> String { + if let Some(base) = req_type.strip_suffix("_Request_") { + format!("{base}_Response_") + } else { + let dds_base = ros2_type.replace('/', "::"); + format!("{dds_base}_Response_") + } +} + +// ─── ZDdsServiceBridge ──────────────────────────────────────────────────────── + +/// Routes a DDS service server to a Zenoh queryable. +/// +/// A Zenoh queryable is declared with `.complete(true)` so clients across a router +/// topology see this bridge as a complete service provider. Each Zenoh query is +/// translated to a DDS request; the matching DDS reply (matched by sequence number +/// from the CDR header) is forwarded back as the query reply. +pub struct ZDdsServiceBridge { + _req_writer: Arc, + _rep_reader: P::Reader, + _queryable: Queryable<()>, +} + +unsafe impl Send for ZDdsServiceBridge

{} +unsafe impl Sync for ZDdsServiceBridge

{} + +impl ZDdsServiceBridge

{ + /// Create a new DDS-server → Zenoh queryable bridge. + /// + /// The Zenoh queryable is declared on a wildcard key (`{domain}/{service}/**`) so + /// that clients using any type hash — or none at all — can reach it transparently. + /// + /// - `ros2_name` — ROS 2 service name (e.g. `/add_two_ints`) + /// - `ros2_type` — ROS 2 service type (e.g. `example_interfaces/srv/AddTwoInts`) + /// - `qos` — DDS QoS for the request writer / reply reader + pub async fn new( + node: &ZNode, + ros2_name: &str, + ros2_type: &str, + participant: &P, + qos: BridgeQos, + ) -> Result { + // Routing entity: no type_info so topic_key_expr returns the EMPTY placeholder key. + // We then widen it to a wildcard so any Zenoh client can reach this bridge queryable + // regardless of the type hash embedded in the client's query key expression. + let entity = EndpointEntity { + id: node.next_entity_id(), + node: Some(node.node_entity().clone()), + kind: EndpointKind::Service, + topic: ros2_name.to_string(), + type_info: None, + qos: bridge_qos_to_qos_profile(&qos), + }; + let topic_ke = node + .keyexpr_format() + .topic_key_expr(&entity) + .map_err(|e| anyhow!("topic_key_expr failed: {e}"))?; + + // Convert "…/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH" → "…/**" for rmw-zenoh keys so + // the queryable responds to clients regardless of their type/hash. For ros2dds + // keys (which carry no type/hash) the key is used as-is. + let ke_str = topic_ke.as_str(); + let ke_str = ke_str + .strip_suffix("/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .map(|prefix| format!("{prefix}/**")) + .unwrap_or_else(|| ke_str.to_string()); + let ke: zenoh::key_expr::OwnedKeyExpr = ke_str + .try_into() + .map_err(|e| anyhow!("invalid key expr: {e}"))?; + + let dds_type = ros2_type_to_dds_type(ros2_type); + let req_type = format!("{}_Request_", dds_type.trim_end_matches('_')); + let rep_type = reply_type_from_request_type(&req_type, ros2_type); + let req_topic = ros2_name_to_dds_request_topic(ros2_name); + let rep_topic = ros2_name_to_dds_reply_topic(ros2_name); + let req_writer_qos = adapt_reader_qos_for_writer(&qos); + + tracing::info!( + "ZDdsServiceBridge: DDS {} → Zenoh {}", + req_topic, + ke.as_str() + ); + + let seq_counter = Arc::new(AtomicU64::new(0)); + let in_flight: Arc>> = Arc::new(Mutex::new(HashMap::new())); + + let req_writer = Arc::new(participant.create_writer( + &req_topic, + &req_type, + true, + req_writer_qos.clone(), + )?); + let client_guid = req_writer.instance_handle()?; + + let in_flight_rep = Arc::clone(&in_flight); + let rep_reader_qos = adapt_writer_qos_for_reader(&qos); + let rep_reader = participant.create_reader( + &rep_topic, + &rep_type, + true, + rep_reader_qos, + Box::new(move |bytes: Vec| { + let raw = bytes.as_slice(); + let seq = match extract_sequence_number(raw) { + Some(s) => s, + None => { + tracing::warn!("ZDdsServiceBridge: reply too short ({} bytes)", raw.len()); + return; + } + }; + if let Some(query) = in_flight_rep.lock().remove(&seq) { + let zenoh_payload = if raw.len() >= 20 { + let mut v = Vec::with_capacity(4 + (raw.len() - 20)); + v.extend_from_slice(&cdr_header_matching(raw)); + v.extend_from_slice(&raw[20..]); + v + } else { + raw.to_vec() + }; + let zbytes: ZBytes = zenoh_payload.into(); + let ke = query.key_expr().clone(); + if let Err(e) = query.reply(ke, zbytes).wait() { + tracing::warn!("ZDdsServiceBridge: query reply failed: {e}"); + } + } else { + tracing::debug!("ZDdsServiceBridge: no in-flight query for seq={seq}"); + } + }), + )?; + + let in_flight_q = Arc::clone(&in_flight); + let seq_counter_q = Arc::clone(&seq_counter); + let req_writer_q = Arc::clone(&req_writer); + let ke_log = topic_ke.as_str().to_string(); + + let queryable = node + .session() + .declare_queryable(ke.clone()) + .complete(true) + .callback(move |query: Query| { + tracing::debug!("ZDdsServiceBridge: query on {ke_log}"); + let seq = seq_counter_q.fetch_add(1, Ordering::Relaxed); + + let query_payload: Vec = match query.payload() { + Some(p) => p.to_bytes().into_owned(), + None => vec![], + }; + let payload_body = if query_payload.len() >= 4 { + &query_payload[4..] + } else { + query_payload.as_slice() + }; + + let cdr_hdr = cdr_header_matching(&query_payload); + let mut dds_payload = Vec::with_capacity(4 + 16 + payload_body.len()); + dds_payload.extend_from_slice(&cdr_hdr); + dds_payload.extend_from_slice(&client_guid.to_le_bytes()); + dds_payload.extend_from_slice(&seq.to_le_bytes()); + dds_payload.extend_from_slice(payload_body); + + in_flight_q.lock().insert(seq, query); + + if let Err(e) = req_writer_q.write_cdr(dds_payload) { + tracing::warn!("ZDdsServiceBridge: DDS write failed: {e}"); + in_flight_q.lock().remove(&seq); + } + }) + .await + .map_err(|e| anyhow!("declare_queryable failed: {e}"))?; + + Ok(Self { + _req_writer: req_writer, + _rep_reader: rep_reader, + _queryable: queryable, + }) + } + + /// Return the DDS request-writer GID (appears in `writer_gid_seq` in ros_discovery_info). + pub(crate) fn writer_guid(&self) -> Option { + self._req_writer.guid().ok().map(Gid::from) + } + + /// Return the DDS reply-reader GID (appears in `reader_gid_seq` in ros_discovery_info). + pub(crate) fn reader_guid(&self) -> Option { + self._rep_reader.guid().ok().map(Gid::from) + } +} + +// ─── ZDdsClientBridge ──────────────────────────────────────────────────────── + +/// Request forwarded from the DDS reader callback to the async dispatch task. +struct PendingRequest { + hdr: [u8; 16], + payload: Vec, +} + +/// Routes a DDS service client to a Zenoh querier. +/// +/// A DDS reader watches the DDS request topic. Each incoming request is forwarded +/// as a Zenoh `get()` query to the service key expression. The reply is reconstructed +/// with the original 16-byte DDS header (client GUID + sequence number) and written +/// back to the DDS reply topic. +pub struct ZDdsClientBridge { + _req_reader: P::Reader, + _rep_writer: Arc, +} + +unsafe impl Send for ZDdsClientBridge

{} +unsafe impl Sync for ZDdsClientBridge

{} + +impl ZDdsClientBridge

{ + /// Create a new DDS-client → Zenoh querier bridge. + /// + /// - `ros2_name` — ROS 2 service name (e.g. `/add_two_ints`) + /// - `ros2_type` — ROS 2 service type (e.g. `example_interfaces/srv/AddTwoInts`) + /// - `type_hash` — optional RIHS01 type hash + /// - `qos` — DDS QoS for the reply writer / request reader + /// - `timeout` — Zenoh querier timeout (use 10s for regular services, 300s for action get_result) + pub async fn new( + node: &ZNode, + ros2_name: &str, + ros2_type: &str, + type_hash: Option, + participant: &P, + qos: BridgeQos, + timeout: Duration, + ) -> Result { + let type_info = type_hash.map(|h| TypeInfo::new(ros2_type, h)); + let entity = EndpointEntity { + id: node.next_entity_id(), + node: Some(node.node_entity().clone()), + kind: EndpointKind::Client, + topic: ros2_name.to_string(), + type_info, + qos: bridge_qos_to_qos_profile(&qos), + }; + + let topic_ke = node + .keyexpr_format() + .topic_key_expr(&entity) + .map_err(|e| anyhow!("topic_key_expr failed: {e}"))?; + + let ke: zenoh::key_expr::OwnedKeyExpr = topic_ke + .as_str() + .to_owned() + .try_into() + .map_err(|e| anyhow!("invalid key expr: {e}"))?; + + let dds_type = ros2_type_to_dds_type(ros2_type); + let req_type = format!("{}_Request_", dds_type.trim_end_matches('_')); + let rep_type = reply_type_from_request_type(&req_type, ros2_type); + let req_topic = ros2_name_to_dds_request_topic(ros2_name); + let rep_topic = ros2_name_to_dds_reply_topic(ros2_name); + let rep_writer_qos = adapt_reader_qos_for_writer(&qos); + let req_reader_qos = adapt_reader_qos_for_writer(&qos); + + tracing::info!( + "ZDdsClientBridge: Zenoh {} → DDS {}", + topic_ke.as_str(), + rep_topic + ); + + let rep_writer = + Arc::new(participant.create_writer(&rep_topic, &rep_type, true, rep_writer_qos)?); + + let (tx, rx) = flume::bounded::(64); + + let querier = node + .session() + .declare_querier(ke) + .timeout(timeout) + .await + .map_err(|e| anyhow!("declare_querier failed: {e}"))?; + + let writer_task = Arc::clone(&rep_writer); + let rep_topic_log = rep_topic.clone(); + tokio::spawn(async move { + while let Ok(req) = rx.recv_async().await { + let zbytes: ZBytes = req.payload.into(); + let replies = match querier.get().payload(zbytes).await { + Ok(r) => r, + Err(e) => { + tracing::warn!("ZDdsClientBridge: querier.get() failed: {e}"); + continue; + } + }; + + for reply in replies { + let reply_bytes: Vec = match reply.result() { + Ok(sample) => sample.payload().to_bytes().into_owned(), + Err(e) => { + tracing::warn!("ZDdsClientBridge: reply error: {e}"); + continue; + } + }; + let reply_body = if reply_bytes.len() >= 4 { + &reply_bytes[4..] + } else { + reply_bytes.as_slice() + }; + let mut dds_reply = Vec::with_capacity(4 + 16 + reply_body.len()); + dds_reply.extend_from_slice(&cdr_header_matching(&reply_bytes)); + dds_reply.extend_from_slice(&req.hdr); + dds_reply.extend_from_slice(reply_body); + + if let Err(e) = writer_task.write_cdr(dds_reply) { + tracing::warn!( + "ZDdsClientBridge: DDS write to {rep_topic_log} failed: {e}" + ); + } + break; + } + } + }); + + let req_reader = participant.create_reader( + &req_topic, + &req_type, + true, + req_reader_qos, + Box::new(move |bytes: Vec| { + let raw = bytes.as_slice(); + if raw.len() < 20 { + tracing::warn!("ZDdsClientBridge: request too short ({} bytes)", raw.len()); + return; + } + let mut hdr = [0u8; 16]; + hdr.copy_from_slice(&raw[4..20]); + let mut payload = Vec::with_capacity(4 + raw.len() - 20); + payload.extend_from_slice(&cdr_header_matching(raw)); + payload.extend_from_slice(&raw[20..]); + let _ = tx.try_send(PendingRequest { hdr, payload }); + }), + )?; + + Ok(Self { + _req_reader: req_reader, + _rep_writer: rep_writer, + }) + } + + /// Return the DDS request-reader GID (appears in `reader_gid_seq` in ros_discovery_info). + pub(crate) fn reader_guid(&self) -> Option { + self._req_reader.guid().ok().map(Gid::from) + } + + /// Return the DDS reply-writer GID (appears in `writer_gid_seq` in ros_discovery_info). + pub(crate) fn writer_guid(&self) -> Option { + self._rep_writer.guid().ok().map(Gid::from) + } +} + +// ─── tests ──────────────────────────────────────────────────────────────────── + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_extract_sequence_number_valid() { + let mut raw = vec![0u8; 24]; + raw[12..20].copy_from_slice(&42u64.to_le_bytes()); + assert_eq!(extract_sequence_number(&raw), Some(42)); + } + + #[test] + fn test_extract_sequence_number_too_short() { + assert_eq!(extract_sequence_number(&[0u8; 19]), None); + assert_eq!(extract_sequence_number(&[]), None); + } + + #[test] + fn test_extract_sequence_number_exact_boundary() { + assert_eq!(extract_sequence_number(&[0u8; 20]), Some(0)); + } + + #[test] + fn test_extract_sequence_number_max_u64() { + let mut raw = vec![0u8; 24]; + raw[12..20].copy_from_slice(&u64::MAX.to_le_bytes()); + assert_eq!(extract_sequence_number(&raw), Some(u64::MAX)); + } + + #[test] + fn test_cdr_header_matching_le() { + assert_eq!(cdr_header_matching(&[0, 1, 0, 0]), CDR_HEADER_LE); + } + + #[test] + fn test_cdr_header_matching_be() { + assert_eq!(cdr_header_matching(&[0, 0, 0, 0]), CDR_HEADER_BE); + } + + #[test] + fn test_cdr_header_matching_empty_defaults_to_le() { + assert_eq!(cdr_header_matching(&[]), CDR_HEADER_LE); + } + + #[test] + fn test_reply_type_from_request_type_standard() { + assert_eq!( + reply_type_from_request_type( + "example_interfaces::srv::dds_::AddTwoInts_Request_", + "example_interfaces/srv/AddTwoInts" + ), + "example_interfaces::srv::dds_::AddTwoInts_Response_" + ); + } + + #[test] + fn test_reply_type_from_request_type_fallback() { + assert_eq!( + reply_type_from_request_type( + "something_unexpected", + "example_interfaces/srv/AddTwoInts" + ), + "example_interfaces::srv::AddTwoInts_Response_" + ); + } + + #[test] + fn test_request_payload_construction() { + let client_guid = 0xaabbccdd_u64; + let seq = 5u64; + let payload_body = &[10u8, 20, 30]; + + let mut dds_payload = Vec::new(); + dds_payload.extend_from_slice(&CDR_HEADER_LE); + dds_payload.extend_from_slice(&client_guid.to_le_bytes()); + dds_payload.extend_from_slice(&seq.to_le_bytes()); + dds_payload.extend_from_slice(payload_body); + + assert_eq!(&dds_payload[..4], &CDR_HEADER_LE); + assert_eq!(&dds_payload[4..12], &client_guid.to_le_bytes()); + assert_eq!(&dds_payload[12..20], &seq.to_le_bytes()); + assert_eq!(&dds_payload[20..], payload_body); + } + + #[test] + fn test_reply_header_stripping() { + let mut dds_reply = vec![0u8; 24]; + dds_reply[1] = 1; // LE + dds_reply[20..24].copy_from_slice(&[1, 2, 3, 4]); + + let raw = dds_reply.as_slice(); + let zenoh_payload: Vec = if raw.len() >= 20 { + let mut v = Vec::new(); + v.extend_from_slice(&cdr_header_matching(raw)); + v.extend_from_slice(&raw[20..]); + v + } else { + raw.to_vec() + }; + assert_eq!(&zenoh_payload[..4], &CDR_HEADER_LE); + assert_eq!(&zenoh_payload[4..], &[1, 2, 3, 4]); + } + + #[test] + fn test_reply_reconstruction_injects_saved_header() { + let mut saved_hdr = [0u8; 16]; + saved_hdr[..8].copy_from_slice(&0x1234_u64.to_le_bytes()); + saved_hdr[8..].copy_from_slice(&7u64.to_le_bytes()); + + let body = [0xAA, 0xBB, 0xCC]; + let mut zenoh_reply = CDR_HEADER_LE.to_vec(); + zenoh_reply.extend_from_slice(&body); + + let reply_body = &zenoh_reply[4..]; + let mut dds_reply = Vec::new(); + dds_reply.extend_from_slice(&cdr_header_matching(&zenoh_reply)); + dds_reply.extend_from_slice(&saved_hdr); + dds_reply.extend_from_slice(reply_body); + + assert_eq!(&dds_reply[..4], &CDR_HEADER_LE); + assert_eq!(&dds_reply[4..20], &saved_hdr); + assert_eq!(&dds_reply[20..], &body); + } +} diff --git a/crates/ros-z-tests/Cargo.toml b/crates/ros-z-tests/Cargo.toml index bb7f4a6b..ebfd29a2 100644 --- a/crates/ros-z-tests/Cargo.toml +++ b/crates/ros-z-tests/Cargo.toml @@ -11,6 +11,7 @@ publish = false [dependencies] ros-z = { path = "../ros-z", default-features = false, features = ["protobuf"] } ros-z-msgs = { path = "../ros-z-msgs", default-features = false, optional = true } +ros-z-dds = { path = "../ros-z-dds", optional = true } ros-z-cdr = { path = "../ros-z-cdr" } ros-z-schema = { path = "../ros-z-schema" } protobuf_demo = { path = "../ros-z/examples/protobuf_demo" } @@ -58,6 +59,10 @@ humble-jazzy-bridge-tests = [ # This enables testing ros-z with DDS-based ROS 2 nodes via zenoh-bridge-ros2dds ros2dds-interop = ["ros-msgs", "ros-z/ros2dds", "ros-z/rmw-zenoh"] +# Native ros-z-bridge-dds integration tests. +# Includes API-level tests that use ros-z-dds directly and typed bridge tests (std_msgs). +dds-bridge-interop = ["dep:ros-z-dds", "ros-msgs"] + # ROS 2 distro compatibility - propagate to ros-z and ros-z-msgs humble = ["ros-z/humble", "ros-z-msgs/humble"] jazzy = ["ros-z/jazzy", "ros-z-msgs/jazzy"] diff --git a/crates/ros-z-tests/tests/bridge_dds.rs b/crates/ros-z-tests/tests/bridge_dds.rs new file mode 100644 index 00000000..2585e99e --- /dev/null +++ b/crates/ros-z-tests/tests/bridge_dds.rs @@ -0,0 +1,1602 @@ +//! Integration tests for ros-z-bridge-dds. +//! +//! Covers both wire formats: +//! - `--wire-format ros2dds` (legacy): compatible with zenoh-plugin-ros2dds +//! - `--wire-format rmw-zenoh` (default): interops with ros-z / rmw_zenoh_cpp +//! +//! Legacy test coverage (ros2dds wire format, tests 1-6): +//! | # | Scenario | +//! |---|--------------------------------------| +//! | 1 | Zenoh pub → bridge → DDS sub | +//! | 2 | DDS pub → bridge → Zenoh sub | +//! | 3 | Zenoh queryable ← bridge ← DDS cli | +//! | 4 | DDS server ← bridge ← Zenoh get | +//! | 5 | Zenoh action server ← bridge ← DDS | +//! | 6 | DDS action server ← bridge ← Zenoh | +//! +//! Primary tests (rmw-zenoh wire format, tests 7-10): +//! | # | Scenario | +//! |---|--------------------------------------| +//! | 7 | DDS pub → bridge → Zenoh sub (rmw_zenoh keys) | +//! | 8 | Zenoh pub → bridge → DDS sub (rmw_zenoh keys) | +//! | 9 | DDS server ← bridge ← Zenoh get (rmw_zenoh) | +//! |10 | Zenoh server ← bridge ← DDS client (rmw_zenoh) | +//! +//! rmw-zenoh action tests (tests 14-15): +//! | # | Scenario | +//! |---|--------------------------------------| +//! |14 | Zenoh action server ← bridge ← DDS (rmw_zenoh) | +//! |15 | DDS action server ← bridge ← Zenoh (rmw_zenoh) | +//! +//! Federation tests (tests 16-17): +//! | # | Scenario | +//! |---|--------------------------------------| +//! |16 | DDS domain A pub → Bridge A → Zenoh → Bridge B → DDS domain B sub | +//! |17 | DDS domain A client → Bridge A → Zenoh → Bridge B → DDS domain B server | +//! +//! Topic filtering test (test 18): +//! | # | Scenario | +//! |---|--------------------------------------| +//! |18 | --deny blocks a topic from being bridged | +//! +//! API-level tests (no binary spawn, tests 11-13, 19-20): +//! | # | Scenario | +//! |---|--------------------------------------| +//! |11 | ZDdsPubBridge::new() constructs cleanly | +//! |12 | ZDdsSubBridge::new() constructs cleanly | +//! |13 | DdsBridgeExt typed constructors work | +//! |19 | ZDdsServiceBridge::new() constructs cleanly | +//! |20 | ZDdsClientBridge::new() constructs cleanly | +//! +//! Requirements: +//! - ROS 2 Jazzy with `rmw_cyclonedds_cpp`, `demo_nodes_cpp`, `action_tutorials_cpp` +//! - `ros-z-bridge-dds` binary built at `target/debug/` or in PATH +//! +//! Run with: +//! ```bash +//! cargo test -p ros-z-tests --features dds-bridge-interop,jazzy +//! ``` + +#![cfg(feature = "dds-bridge-interop")] + +mod common; + +use std::{ + process::{Child, Command, Output, Stdio}, + sync::{Arc, Mutex}, + thread, + time::Duration, +}; + +use common::{ProcessGuard, TestRouter}; +use nix::{ + sys::signal::{Signal, kill}, + unistd::Pid, +}; +use zenoh::Wait; + +#[cfg(feature = "dds-bridge-interop")] +use ros_z_dds::{ + CyclorsParticipant, DdsBridgeExt, ZDdsClientBridge, ZDdsPubBridge, ZDdsServiceBridge, + ZDdsSubBridge, + participant::{BridgeQos, DdsParticipant}, +}; + +#[cfg(feature = "dds-bridge-interop")] +use ros_z_msgs::std_msgs::String as RosString; + +/// Kill an entire process group (including any children spawned by `ros2 run`) +/// and then collect stdout output. Using `child.kill()` alone only kills the +/// direct child; `ros2 run` may exec-fork a C++ node that keeps the pipe open. +fn kill_group_and_collect(child: Child) -> Output { + let pgid = Pid::from_raw(-(child.id() as i32)); + let _ = kill(pgid, Signal::SIGKILL); + child.wait_with_output().unwrap() +} + +// ── helpers ────────────────────────────────────────────────────────────────── + +/// CycloneDDS URI that forces use of the loopback interface with multicast enabled. +/// +/// Without this, CycloneDDS tries the primary network interface (wlp1s0 etc.) for +/// multicast discovery. Between processes on the same host, that often fails when +/// multicast routing is not configured. The loopback interface reliably supports +/// multicast on Linux and is always available. +const CYCLONEDDS_URI: &str = "\ + \ + "; + +fn bridge_binary() -> String { + let manifest = env!("CARGO_MANIFEST_DIR"); + for profile in ["release", "debug"] { + let path = format!("{manifest}/../../target/{profile}/ros-z-bridge-dds"); + if std::path::Path::new(&path).exists() { + return path; + } + } + "ros-z-bridge-dds".to_string() +} + +fn ros2_available() -> bool { + Command::new("ros2") + .args(["pkg", "list"]) + .stdout(Stdio::null()) + .stderr(Stdio::null()) + .status() + .map(|s| s.success()) + .unwrap_or(false) +} + +fn pkg_available(pkg: &str) -> bool { + Command::new("ros2") + .args(["pkg", "prefix", pkg]) + .stdout(Stdio::null()) + .stderr(Stdio::null()) + .status() + .map(|s| s.success()) + .unwrap_or(false) +} + +fn spawn_bridge_fmt(zenoh_endpoint: &str, domain_id: u32, wire_format: &str) -> ProcessGuard { + use std::os::unix::process::CommandExt; + let bin = bridge_binary(); + let log_path = format!( + "{}/../../_tmp/bridge-{domain_id}.log", + env!("CARGO_MANIFEST_DIR") + ); + let log_file = std::fs::File::create(&log_path) + .unwrap_or_else(|e| panic!("Failed to create bridge log {log_path}: {e}")); + let stderr_file = log_file.try_clone().expect("clone log file for stderr"); + let child = Command::new(&bin) + .args([ + "--zenoh-endpoint", + zenoh_endpoint, + "--domain-id", + &domain_id.to_string(), + "--wire-format", + wire_format, + ]) + .env("RUST_LOG", "info") + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(log_file) + .stderr(stderr_file) + .process_group(0) + .spawn() + .unwrap_or_else(|e| panic!("Failed to spawn {bin}: {e}")); + thread::sleep(Duration::from_secs(5)); + ProcessGuard::new(child, "ros-z-bridge-dds") +} + +/// Spawn bridge in legacy ros2dds mode (zenoh-plugin-ros2dds compatible key expressions). +fn spawn_bridge_ros2dds(zenoh_endpoint: &str, domain_id: u32) -> ProcessGuard { + spawn_bridge_fmt(zenoh_endpoint, domain_id, "ros2dds") +} + +/// Spawn bridge in default rmw-zenoh mode (ros-z / rmw_zenoh_cpp compatible key expressions). +fn spawn_bridge(zenoh_endpoint: &str, domain_id: u32) -> ProcessGuard { + spawn_bridge_fmt(zenoh_endpoint, domain_id, "rmw-zenoh") +} + +/// Spawn bridge with additional CLI flags (e.g. `--allow`/`--deny`). +fn spawn_bridge_extra( + zenoh_endpoint: &str, + domain_id: u32, + wire_format: &str, + extra: &[&str], +) -> ProcessGuard { + use std::os::unix::process::CommandExt; + let bin = bridge_binary(); + let log_path = format!( + "{}/../../_tmp/bridge-{domain_id}-extra.log", + env!("CARGO_MANIFEST_DIR") + ); + let log_file = std::fs::File::create(&log_path) + .unwrap_or_else(|e| panic!("Failed to create bridge log {log_path}: {e}")); + let stderr_file = log_file.try_clone().expect("clone log file for stderr"); + let domain_str = domain_id.to_string(); + let child = Command::new(&bin) + .args([ + "--zenoh-endpoint", + zenoh_endpoint, + "--domain-id", + &domain_str, + "--wire-format", + wire_format, + ]) + .args(extra) + .env("RUST_LOG", "info") + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(log_file) + .stderr(stderr_file) + .process_group(0) + .spawn() + .unwrap_or_else(|e| panic!("Failed to spawn {bin}: {e}")); + thread::sleep(Duration::from_secs(5)); + ProcessGuard::new(child, "ros-z-bridge-dds") +} + +fn spawn_cyclone(domain_id: u32, pkg: &str, node: &str, extra_args: &[&str]) -> ProcessGuard { + use std::os::unix::process::CommandExt; + let name = format!("{}/{}", pkg, node); + let mut cmd = Command::new("ros2"); + cmd.args(["run", pkg, node]) + .args(extra_args) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::null()) + .process_group(0); + let child = cmd + .spawn() + .unwrap_or_else(|e| panic!("Failed to spawn {name}: {e}")); + thread::sleep(Duration::from_secs(2)); + ProcessGuard::new(child, &name) +} + +fn zenoh_session(endpoint: &str) -> zenoh::Session { + let mut cfg = zenoh::Config::default(); + cfg.insert_json5("connect/endpoints", &format!(r#"["{}"]"#, endpoint)) + .unwrap(); + cfg.insert_json5("scouting/multicast/enabled", "false") + .unwrap(); + cfg.insert_json5("mode", r#""client""#).unwrap(); + zenoh::open(cfg).wait().unwrap() +} + +/// Minimal CDR string payload: 4-byte LE header + 4-byte length + UTF-8 bytes + NUL + padding. +fn cdr_string(s: &str) -> Vec { + let bytes = s.as_bytes(); + let len = bytes.len() as u32 + 1; // include NUL + let mut v = Vec::with_capacity(4 + 4 + len as usize); + v.extend_from_slice(&[0, 1, 0, 0]); // CDR LE header + v.extend_from_slice(&len.to_le_bytes()); + v.extend_from_slice(bytes); + v.push(0); // NUL terminator + while v.len() % 4 != 0 { + v.push(0); // align + } + v +} + +/// CDR payload for AddTwoInts request (two i64 values, no request-header). +fn cdr_add_two_ints_request(a: i64, b: i64) -> Vec { + let mut v = Vec::with_capacity(4 + 16); + v.extend_from_slice(&[0, 1, 0, 0]); // CDR LE header + v.extend_from_slice(&a.to_le_bytes()); + v.extend_from_slice(&b.to_le_bytes()); + v +} + +/// Extract i64 sum from a CDR AddTwoInts response payload (skip 4-byte CDR header). +fn parse_add_two_ints_response(bytes: &[u8]) -> Option { + // bytes = 4-byte CDR header + optional 16-byte request header + 8-byte i64 sum + // When coming from the DDS server, the bridge strips the request header. + // Attempt to parse at offset 4 (no request header) or offset 20 (with header). + for offset in [4usize, 20] { + if bytes.len() >= offset + 8 { + let sum = i64::from_le_bytes(bytes[offset..offset + 8].try_into().unwrap()); + if sum > 0 && sum < 1_000_000 { + return Some(sum); + } + } + } + None +} + +// ── Test 1: Zenoh pub → bridge → DDS subscriber (ros2dds) ──────────────────── + +/// Mirrors `test_zenoh_pub_ros_sub` from zenoh-plugin-ros2dds. +/// +/// Zenoh publishes a CDR String on `chatter`. The bridge forwards it to DDS. +/// A CycloneDDS listener receives it — we verify by capturing its stdout. +#[test] +fn test_zenoh_pub_ros_sub() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_zenoh_pub_ros_sub: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let domain_id = 51u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_ros2dds(&endpoint, domain_id); + + // Start listener; ROS 2 RCLCPP_INFO logs go to stderr, not stdout. + let mut listener_cmd = Command::new("ros2"); + listener_cmd + .args(["run", "demo_nodes_cpp", "listener"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::piped()); + use std::os::unix::process::CommandExt; + listener_cmd.process_group(0); + let listener_child = listener_cmd.spawn().expect("Failed to spawn listener"); + // Wait for DDS discovery: CycloneDDS on loopback needs ~5 s to discover the bridge. + // Wait extra time for the bridge to create its DDS writer and for the listener to + // discover that new writer. + thread::sleep(Duration::from_secs(10)); + + // Publish from Zenoh side; repeat to handle transient delivery gaps. + let session = zenoh_session(&endpoint); + let publisher = session.declare_publisher("chatter").wait().unwrap(); + let payload = cdr_string("Hello from Zenoh"); + for _ in 0..30 { + publisher + .put(zenoh::bytes::ZBytes::from(payload.clone())) + .wait() + .unwrap(); + thread::sleep(Duration::from_millis(500)); + } + + // Read listener output (RCLCPP_INFO goes to stderr in ROS 2 Jazzy) + let output = kill_group_and_collect(listener_child); + let stderr = String::from_utf8_lossy(&output.stderr); + assert!( + stderr.contains("Hello"), + "listener received nothing; stderr={stderr:?}" + ); +} + +// ── Test 2: DDS pub → bridge → Zenoh subscriber (ros2dds) ──────────────────── + +/// Mirrors `test_ros_pub_zenoh_sub` from zenoh-plugin-ros2dds. +/// +/// A CycloneDDS talker publishes on `/chatter`. The bridge forwards CDR bytes +/// to Zenoh. A Zenoh subscriber receives at least one sample. +#[test] +fn test_ros_pub_zenoh_sub() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_ros_pub_zenoh_sub: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let domain_id = 52u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_ros2dds(&endpoint, domain_id); + + let received = Arc::new(Mutex::new(false)); + let received_clone = received.clone(); + + let session = zenoh_session(&endpoint); + let _sub = session + .declare_subscriber("chatter") + .callback(move |_sample| { + *received_clone.lock().unwrap() = true; + }) + .wait() + .unwrap(); + + let _talker = spawn_cyclone(domain_id, "demo_nodes_cpp", "talker", &[]); + + // Wait up to 10 s for a message + for _ in 0..20 { + if *received.lock().unwrap() { + break; + } + thread::sleep(Duration::from_millis(500)); + } + + assert!( + *received.lock().unwrap(), + "Zenoh subscriber received nothing from CycloneDDS talker" + ); +} + +// ── Test 3: Zenoh queryable ← bridge ← DDS service client (ros2dds) ───────── + +/// Mirrors `test_ros_client_zenoh_service` from zenoh-plugin-ros2dds. +/// +/// A Zenoh queryable acts as the AddTwoInts service server. +/// A DDS `add_two_ints_client` calls the service through the bridge. +/// We verify the bridge forwards the request to Zenoh and the client gets a reply. +#[test] +fn test_ros_client_zenoh_service() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_ros_client_zenoh_service: dependencies not available"); + return; + } + + let domain_id = 53u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_ros2dds(&endpoint, domain_id); + + // Zenoh service server: receives CDR request, returns CDR reply + let session = zenoh_session(&endpoint); + let _queryable = session + .declare_queryable("add_two_ints") + .callback(|query| { + let payload: Vec = query + .payload() + .map(|p| p.to_bytes().into_owned()) + .unwrap_or_default(); + + // Request layout: 4-byte CDR header + 8-byte a + 8-byte b + let (a, b) = if payload.len() >= 20 { + ( + i64::from_le_bytes(payload[4..12].try_into().unwrap()), + i64::from_le_bytes(payload[12..20].try_into().unwrap()), + ) + } else { + (0i64, 0i64) + }; + + // Reply: 4-byte CDR header + 8-byte sum + let mut reply = Vec::with_capacity(12); + reply.extend_from_slice(&[0, 1, 0, 0]); + reply.extend_from_slice(&(a + b).to_le_bytes()); + + let ke = query.key_expr().clone(); + query + .reply(ke, zenoh::bytes::ZBytes::from(reply)) + .wait() + .unwrap(); + }) + .wait() + .unwrap(); + + // Give time for the queryable to be discovered + thread::sleep(Duration::from_secs(2)); + + // Spawn DDS client (sends one request: 1 + 2); RCLCPP_INFO goes to stderr in ROS 2 Jazzy. + let mut client_cmd = Command::new("ros2"); + client_cmd + .args(["run", "demo_nodes_cpp", "add_two_ints_client"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::piped()); + use std::os::unix::process::CommandExt; + client_cmd.process_group(0); + let client_child = client_cmd.spawn().expect("spawn add_two_ints_client"); + + // Wait for client to complete (it exits after one response) + let output = { + let deadline = std::time::Instant::now() + Duration::from_secs(15); + let mut child = client_child; + loop { + match child.try_wait() { + Ok(Some(_)) => break child.wait_with_output().unwrap(), + Ok(None) if std::time::Instant::now() < deadline => { + thread::sleep(Duration::from_millis(200)); + } + _ => break kill_group_and_collect(child), + } + } + }; + let stderr = String::from_utf8_lossy(&output.stderr); + assert!( + stderr.contains("3") || output.status.success(), + "DDS client did not receive a valid reply; stderr={stderr:?}" + ); +} + +// ── Test 4: Zenoh client → bridge → DDS service server (ros2dds) ───────────── + +/// Mirrors `test_zenoh_client_ros_service` from zenoh-plugin-ros2dds. +/// +/// A CycloneDDS `add_two_ints_server` is running. A Zenoh `get()` call reaches +/// it through the bridge and receives sum=3 (1+2). +#[test] +fn test_zenoh_client_ros_service() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_zenoh_client_ros_service: dependencies not available"); + return; + } + + let domain_id = 54u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_ros2dds(&endpoint, domain_id); + let _server = spawn_cyclone(domain_id, "demo_nodes_cpp", "add_two_ints_server", &[]); + thread::sleep(Duration::from_secs(2)); // let server + bridge settle + + let session = zenoh_session(&endpoint); + let payload = cdr_add_two_ints_request(1, 2); + + let replies: Vec<_> = session + .get("add_two_ints") + .payload(zenoh::bytes::ZBytes::from(payload)) + .timeout(Duration::from_secs(10)) + .wait() + .unwrap() + .into_iter() + .collect(); + + assert!( + !replies.is_empty(), + "No reply from DDS add_two_ints_server via bridge" + ); + + let reply_bytes: Vec = replies[0] + .result() + .expect("reply error") + .payload() + .to_bytes() + .into_owned(); + + let sum = parse_add_two_ints_response(&reply_bytes); + assert_eq!( + sum, + Some(3), + "Expected sum=3, got {:?}; raw={reply_bytes:?}", + sum + ); +} + +// ── Test 5: Zenoh action server ← bridge ← DDS action client (ros2dds) ─────── + +/// Mirrors `test_ros_client_zenoh_action` from zenoh-plugin-ros2dds. +/// +/// A Zenoh side implements the three action service components +/// (send_goal, get_result) and the feedback publisher. +/// A DDS `fibonacci_action_client` sends a goal through the bridge and +/// receives feedback and result. +/// +/// Uses `action_tutorials_cpp` package (Jazzy+). +#[test] +fn test_ros_client_zenoh_action() { + if !ros2_available() || !pkg_available("action_tutorials_cpp") { + eprintln!("Skipping test_ros_client_zenoh_action: action_tutorials_cpp not available"); + return; + } + + let domain_id = 55u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_ros2dds(&endpoint, domain_id); + thread::sleep(Duration::from_secs(1)); + + let session = zenoh_session(&endpoint); + + // Zenoh action server components + // send_goal queryable + let _send_goal = session + .declare_queryable("fibonacci/_action/send_goal") + .callback(move |query| { + // Accept immediately: [bool accept=true, int32 sec=0, uint32 nanosec=0] + let mut reply = vec![0u8; 4 + 1 + 4 + 4]; + reply[..4].copy_from_slice(&[0, 1, 0, 0]); // CDR LE + reply[4] = 1; // accept = true + let ke = query.key_expr().clone(); + query + .reply(ke, zenoh::bytes::ZBytes::from(reply)) + .wait() + .unwrap(); + }) + .wait() + .unwrap(); + + // feedback publisher + let fb_pub = session + .declare_publisher("fibonacci/_action/feedback") + .wait() + .unwrap(); + + // get_result queryable: publishes feedback then returns result + let _get_result = session + .declare_queryable("fibonacci/_action/get_result") + .callback(move |query| { + // Publish one feedback sample: goal_id(16) + sequence [0,1,1,2,3] + let mut fb = vec![0u8; 4 + 16 + 4 + 5 * 4]; + fb[..4].copy_from_slice(&[0, 1, 0, 0]); + let seq = [0i32, 1, 1, 2, 3]; + let off = 4 + 16 + 4; + for (i, &v) in seq.iter().enumerate() { + fb[off + i * 4..off + i * 4 + 4].copy_from_slice(&v.to_le_bytes()); + } + // length prefix for sequence + let len_off = 4 + 16; + let len = seq.len() as u32; + fb[len_off..len_off + 4].copy_from_slice(&len.to_le_bytes()); + let _ = fb_pub.put(zenoh::bytes::ZBytes::from(fb)).wait(); + + // Reply with result: status=4 (SUCCEEDED), sequence [0,1,1,2,3,5] + let result_seq = [0i32, 1, 1, 2, 3, 5]; + let mut reply = vec![0u8; 4 + 1 + 4 + result_seq.len() * 4]; + reply[..4].copy_from_slice(&[0, 1, 0, 0]); + reply[4] = 4; // status = SUCCEEDED + let rlen_off = 5; + let rlen = result_seq.len() as u32; + reply[rlen_off..rlen_off + 4].copy_from_slice(&rlen.to_le_bytes()); + for (i, &v) in result_seq.iter().enumerate() { + let off = rlen_off + 4 + i * 4; + reply[off..off + 4].copy_from_slice(&v.to_le_bytes()); + } + let ke = query.key_expr().clone(); + query + .reply(ke, zenoh::bytes::ZBytes::from(reply)) + .wait() + .unwrap(); + }) + .wait() + .unwrap(); + + thread::sleep(Duration::from_secs(2)); + + // Spawn DDS action client + let mut child = Command::new("ros2"); + child + .args(["run", "action_tutorials_cpp", "fibonacci_action_client"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::piped()) + .stderr(Stdio::null()); + use std::os::unix::process::CommandExt; + child.process_group(0); + let client_child = child.spawn().expect("spawn fibonacci_action_client"); + + let output = { + let timeout = std::time::Instant::now() + Duration::from_secs(20); + let mut c = client_child; + loop { + match c.try_wait() { + Ok(Some(_)) => break c.wait_with_output().unwrap(), + Ok(None) if std::time::Instant::now() < timeout => { + thread::sleep(Duration::from_millis(300)); + } + _ => break kill_group_and_collect(c), + } + } + }; + let stdout = String::from_utf8_lossy(&output.stdout); + // The client prints the result sequence; we just check it ran and got something + assert!( + output.status.success() || stdout.contains("sequence"), + "DDS action client did not receive action result; stdout={stdout:?}" + ); +} + +// ── Test 6: Zenoh action client → bridge → DDS action server ───────────────── + +/// Mirrors `test_zenoh_client_ros_action` from zenoh-plugin-ros2dds. +/// +/// A CycloneDDS `fibonacci_action_server` runs on the DDS side. +/// A Zenoh client drives the three service components manually: +/// send_goal → subscribe feedback → get_result. +/// +/// Uses `action_tutorials_cpp` package (Jazzy+). +#[test] +fn test_zenoh_client_ros_action() { + if !ros2_available() || !pkg_available("action_tutorials_cpp") { + eprintln!("Skipping test_zenoh_client_ros_action: action_tutorials_cpp not available"); + return; + } + + let domain_id = 56u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_ros2dds(&endpoint, domain_id); + let _server = spawn_cyclone( + domain_id, + "action_tutorials_cpp", + "fibonacci_action_server", + &[], + ); + thread::sleep(Duration::from_secs(3)); + + let rt = tokio::runtime::Runtime::new().unwrap(); + let (tx, rx) = std::sync::mpsc::channel::(); + + rt.block_on(async move { + let mut cfg = zenoh::Config::default(); + cfg.insert_json5("connect/endpoints", &format!(r#"["{}"]"#, endpoint)) + .unwrap(); + cfg.insert_json5("scouting/multicast/enabled", "false") + .unwrap(); + cfg.insert_json5("mode", r#""client""#).unwrap(); + let session = zenoh::open(cfg).await.unwrap(); + + let goal_id = [1u8; 16]; + let order = 5i32; + + // 1. send_goal + let mut sg_payload = Vec::with_capacity(4 + 16 + 4); + sg_payload.extend_from_slice(&[0, 1, 0, 0]); // CDR LE + sg_payload.extend_from_slice(&goal_id); + sg_payload.extend_from_slice(&order.to_le_bytes()); + + let sg_replies: Vec<_> = session + .get("fibonacci/_action/send_goal") + .payload(zenoh::bytes::ZBytes::from(sg_payload)) + .timeout(Duration::from_secs(10)) + .await + .unwrap() + .into_iter() + .collect(); + + if sg_replies.is_empty() { + tx.send(false).unwrap(); + return; + } + + // 2. subscribe to feedback + let fb_received = Arc::new(Mutex::new(false)); + let fb_clone = fb_received.clone(); + let _fb_sub = session + .declare_subscriber("fibonacci/_action/feedback") + .callback(move |_| { + *fb_clone.lock().unwrap() = true; + }) + .await + .unwrap(); + + // 3. get_result + let mut gr_payload = Vec::with_capacity(4 + 16); + gr_payload.extend_from_slice(&[0, 1, 0, 0]); // CDR LE + gr_payload.extend_from_slice(&goal_id); + + let gr_replies: Vec<_> = session + .get("fibonacci/_action/get_result") + .payload(zenoh::bytes::ZBytes::from(gr_payload)) + .timeout(Duration::from_secs(10)) + .await + .unwrap() + .into_iter() + .collect(); + + let success = !gr_replies.is_empty(); + tx.send(success).unwrap(); + }); + + let got = rx.recv_timeout(Duration::from_secs(30)).unwrap_or(false); + rt.shutdown_background(); + assert!( + got, + "Zenoh action client did not receive result from DDS fibonacci_action_server" + ); +} + +// ── Test 7: DDS pub → bridge → Zenoh sub (rmw-zenoh) ───────────────────────── + +/// CycloneDDS talker publishes on `/chatter`. Bridge runs in default rmw-zenoh mode. +/// A Zenoh subscriber at the wildcard rmw-zenoh key `0/chatter/**` receives at least +/// one sample. The `0` prefix is the ZContext domain_id (default when not set). +#[test] +fn test_dds_pub_rosz_sub() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_dds_pub_rosz_sub: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let domain_id = 57u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge(&endpoint, domain_id); + + let received = Arc::new(Mutex::new(false)); + let received_clone = received.clone(); + + let session = zenoh_session(&endpoint); + let _sub = session + .declare_subscriber("0/chatter/**") + .callback(move |_sample| { + *received_clone.lock().unwrap() = true; + }) + .wait() + .unwrap(); + + let _talker = spawn_cyclone(domain_id, "demo_nodes_cpp", "talker", &[]); + + for _ in 0..20 { + if *received.lock().unwrap() { + break; + } + thread::sleep(Duration::from_millis(500)); + } + + assert!( + *received.lock().unwrap(), + "Zenoh subscriber received nothing from CycloneDDS talker via rmw-zenoh bridge" + ); +} + +// ── Test 8: Zenoh pub → bridge → DDS sub (rmw-zenoh) ───────────────────────── + +/// Zenoh publishes a CDR String on `0/chatter/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH`. +/// Bridge (rmw-zenoh mode) discovers the DDS listener and creates a DDS writer route. +/// The CycloneDDS listener receives the message. +#[test] +fn test_rosz_pub_dds_sub() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_rosz_pub_dds_sub: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let domain_id = 58u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge(&endpoint, domain_id); + + // Start listener; RCLCPP_INFO goes to stderr in ROS 2 Jazzy. + let mut listener_cmd = Command::new("ros2"); + listener_cmd + .args(["run", "demo_nodes_cpp", "listener"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::piped()); + use std::os::unix::process::CommandExt; + listener_cmd.process_group(0); + let listener_child = listener_cmd.spawn().expect("Failed to spawn listener"); + + // Wait for the bridge to discover the DDS listener and create a Zenoh subscriber route. + thread::sleep(Duration::from_secs(10)); + + let session = zenoh_session(&endpoint); + let publisher = session + .declare_publisher("0/chatter/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .wait() + .unwrap(); + let payload = cdr_string("Hello from rmw-zenoh"); + for _ in 0..30 { + publisher + .put(zenoh::bytes::ZBytes::from(payload.clone())) + .wait() + .unwrap(); + thread::sleep(Duration::from_millis(500)); + } + + let output = kill_group_and_collect(listener_child); + let stderr = String::from_utf8_lossy(&output.stderr); + assert!( + stderr.contains("Hello"), + "listener received nothing; stderr={stderr:?}" + ); +} + +// ── Test 9: DDS server ← bridge ← Zenoh get (rmw-zenoh) ────────────────────── + +/// CycloneDDS add_two_ints_server runs. Bridge (rmw-zenoh mode) exposes it as a +/// Zenoh queryable. A Zenoh get() at `0/add_two_ints/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH` +/// returns sum=3. +#[test] +fn test_dds_srv_rosz_client() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_dds_srv_rosz_client: dependencies not available"); + return; + } + + let domain_id = 59u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge(&endpoint, domain_id); + let _server = spawn_cyclone(domain_id, "demo_nodes_cpp", "add_two_ints_server", &[]); + thread::sleep(Duration::from_secs(2)); + + let session = zenoh_session(&endpoint); + let payload = cdr_add_two_ints_request(1, 2); + + let replies: Vec<_> = session + .get("0/add_two_ints/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .payload(zenoh::bytes::ZBytes::from(payload)) + .timeout(Duration::from_secs(10)) + .wait() + .unwrap() + .into_iter() + .collect(); + + assert!( + !replies.is_empty(), + "No reply from DDS add_two_ints_server via rmw-zenoh bridge" + ); + + let reply_bytes: Vec = replies[0] + .result() + .expect("reply error") + .payload() + .to_bytes() + .into_owned(); + + let sum = parse_add_two_ints_response(&reply_bytes); + assert_eq!( + sum, + Some(3), + "Expected sum=3, got {:?}; raw={reply_bytes:?}", + sum + ); +} + +// ── Test 10: Zenoh server ← bridge ← DDS client (rmw-zenoh) ────────────────── + +/// A Zenoh queryable at `0/add_two_ints/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH` acts +/// as the AddTwoInts server. A DDS add_two_ints_client calls through the bridge +/// (rmw-zenoh mode) and receives the computed reply. +#[test] +fn test_rosz_srv_dds_client() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_rosz_srv_dds_client: dependencies not available"); + return; + } + + let domain_id = 60u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge(&endpoint, domain_id); + + let session = zenoh_session(&endpoint); + let _queryable = session + .declare_queryable("0/add_two_ints/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .callback(|query| { + let payload: Vec = query + .payload() + .map(|p| p.to_bytes().into_owned()) + .unwrap_or_default(); + + // Request layout: 4-byte CDR header + 8-byte a + 8-byte b + let (a, b) = if payload.len() >= 20 { + ( + i64::from_le_bytes(payload[4..12].try_into().unwrap()), + i64::from_le_bytes(payload[12..20].try_into().unwrap()), + ) + } else { + (0i64, 0i64) + }; + + // Reply: 4-byte CDR header + 8-byte sum + let mut reply = Vec::with_capacity(12); + reply.extend_from_slice(&[0, 1, 0, 0]); + reply.extend_from_slice(&(a + b).to_le_bytes()); + + let ke = query.key_expr().clone(); + query + .reply(ke, zenoh::bytes::ZBytes::from(reply)) + .wait() + .unwrap(); + }) + .wait() + .unwrap(); + + thread::sleep(Duration::from_secs(2)); + + let mut client_cmd = Command::new("ros2"); + client_cmd + .args(["run", "demo_nodes_cpp", "add_two_ints_client"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::piped()); + use std::os::unix::process::CommandExt; + client_cmd.process_group(0); + let client_child = client_cmd.spawn().expect("spawn add_two_ints_client"); + + let output = { + let deadline = std::time::Instant::now() + Duration::from_secs(15); + let mut child = client_child; + loop { + match child.try_wait() { + Ok(Some(_)) => break child.wait_with_output().unwrap(), + Ok(None) if std::time::Instant::now() < deadline => { + thread::sleep(Duration::from_millis(200)); + } + _ => break kill_group_and_collect(child), + } + } + }; + let stderr = String::from_utf8_lossy(&output.stderr); + assert!( + stderr.contains("3") || output.status.success(), + "DDS client did not receive a valid reply; stderr={stderr:?}" + ); +} + +// ── Test 11: ZDdsPubBridge::new() API construction ──────────────────────────── + +/// Verifies that `ZDdsPubBridge::new()` constructs without error when given a +/// valid ZNode and CyclorsParticipant. No message passing — pure construction test. +#[cfg(feature = "dds-bridge-interop")] +#[test] +fn test_api_pub_bridge() { + use ros_z::{Builder, context::ZContextBuilder}; + + let domain_id = 70u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + unsafe { std::env::set_var("CYCLONEDDS_URI", CYCLONEDDS_URI) }; + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let ctx = ZContextBuilder::default() + .with_connect_endpoints([endpoint.as_str()]) + .build() + .expect("ZContext"); + let node = ctx + .create_node("test_api_pub_bridge") + .build() + .expect("ZNode"); + let participant = CyclorsParticipant::create(domain_id).expect("participant"); + + let bridge = ZDdsPubBridge::new( + &node, + "/chatter", + "std_msgs/msg/String", + None, + &participant, + BridgeQos::default(), + true, + 10, + ) + .await; + + assert!( + bridge.is_ok(), + "ZDdsPubBridge::new failed: {:?}", + bridge.err() + ); + }); +} + +// ── Test 12: ZDdsSubBridge::new() API construction ──────────────────────────── + +/// Verifies that `ZDdsSubBridge::new()` constructs without error. +#[cfg(feature = "dds-bridge-interop")] +#[test] +fn test_api_sub_bridge() { + use ros_z::{Builder, context::ZContextBuilder}; + + let domain_id = 71u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + unsafe { std::env::set_var("CYCLONEDDS_URI", CYCLONEDDS_URI) }; + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let ctx = ZContextBuilder::default() + .with_connect_endpoints([endpoint.as_str()]) + .build() + .expect("ZContext"); + let node = ctx + .create_node("test_api_sub_bridge") + .build() + .expect("ZNode"); + let participant = CyclorsParticipant::create(domain_id).expect("participant"); + + let bridge = ZDdsSubBridge::new( + &node, + "/chatter", + "std_msgs/msg/String", + None, + &participant, + BridgeQos::default(), + true, + ) + .await; + + assert!( + bridge.is_ok(), + "ZDdsSubBridge::new failed: {:?}", + bridge.err() + ); + }); +} + +// ── Test 13: DdsBridgeExt typed constructors ────────────────────────────────── + +/// Verifies that the typed `DdsBridgeExt` convenience methods on `ZNode` construct +/// without error for both publisher and subscriber directions. +#[cfg(feature = "dds-bridge-interop")] +#[test] +fn test_api_typed_bridge() { + use ros_z::{Builder, context::ZContextBuilder}; + + let domain_id = 72u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + unsafe { std::env::set_var("CYCLONEDDS_URI", CYCLONEDDS_URI) }; + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let ctx = ZContextBuilder::default() + .with_connect_endpoints([endpoint.as_str()]) + .build() + .expect("ZContext"); + let node = ctx + .create_node("test_api_typed_bridge") + .build() + .expect("ZNode"); + let participant = CyclorsParticipant::create(domain_id).expect("participant"); + + let pub_bridge = node + .bridge_dds_pub::("/chatter", &participant) + .await; + assert!( + pub_bridge.is_ok(), + "bridge_dds_pub:: failed: {:?}", + pub_bridge.err() + ); + + let sub_bridge = node + .bridge_dds_sub::("/chatter", &participant) + .await; + assert!( + sub_bridge.is_ok(), + "bridge_dds_sub:: failed: {:?}", + sub_bridge.err() + ); + }); +} + +// ── Test 14: rmw-zenoh action, DDS client → bridge → Zenoh action server ─────── + +/// Mirrors test 5 (`test_ros_client_zenoh_action`) using the default rmw-zenoh +/// wire format. The Zenoh action server queryables are registered at +/// `0/fibonacci/_action//EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH` — the +/// bridge's `ZDdsClientBridge` querier discovers them via BestMatching. +#[test] +fn test_ros_action_rosz_server_rmw_zenoh() { + if !ros2_available() || !pkg_available("action_tutorials_cpp") { + eprintln!( + "Skipping test_ros_action_rosz_server_rmw_zenoh: action_tutorials_cpp not available" + ); + return; + } + + let domain_id = 61u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge(&endpoint, domain_id); + thread::sleep(Duration::from_secs(1)); + + let session = zenoh_session(&endpoint); + + // send_goal queryable: reply accept=true + let _send_goal = session + .declare_queryable("0/fibonacci/_action/send_goal/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .callback(|query| { + let mut reply = vec![0u8; 4 + 1 + 4 + 4]; + reply[..4].copy_from_slice(&[0, 1, 0, 0]); + reply[4] = 1; // accepted = true + let ke = query.key_expr().clone(); + query + .reply(ke, zenoh::bytes::ZBytes::from(reply)) + .wait() + .unwrap(); + }) + .wait() + .unwrap(); + + // feedback publisher: one sample so the client sees progress + let fb_pub = session + .declare_publisher("0/fibonacci/_action/feedback/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .wait() + .unwrap(); + + // get_result queryable: publish feedback then return SUCCEEDED result + let _get_result = session + .declare_queryable("0/fibonacci/_action/get_result/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .callback(move |query| { + // One feedback: goal_id(16B) + seq length(4B) + seq [0,1,1,2,3] + let mut fb = vec![0u8; 4 + 16 + 4 + 5 * 4]; + fb[..4].copy_from_slice(&[0, 1, 0, 0]); + let seq = [0i32, 1, 1, 2, 3]; + let len_off = 4 + 16; + fb[len_off..len_off + 4].copy_from_slice(&(seq.len() as u32).to_le_bytes()); + let body_off = len_off + 4; + for (i, &v) in seq.iter().enumerate() { + fb[body_off + i * 4..body_off + i * 4 + 4].copy_from_slice(&v.to_le_bytes()); + } + let _ = fb_pub.put(zenoh::bytes::ZBytes::from(fb)).wait(); + + // Result: status=4 (SUCCEEDED), seq [0,1,1,2,3,5] + let result_seq = [0i32, 1, 1, 2, 3, 5]; + let mut reply = vec![0u8; 4 + 1 + 4 + result_seq.len() * 4]; + reply[..4].copy_from_slice(&[0, 1, 0, 0]); + reply[4] = 4; // SUCCEEDED + reply[5..9].copy_from_slice(&(result_seq.len() as u32).to_le_bytes()); + for (i, &v) in result_seq.iter().enumerate() { + let off = 9 + i * 4; + reply[off..off + 4].copy_from_slice(&v.to_le_bytes()); + } + let ke = query.key_expr().clone(); + query + .reply(ke, zenoh::bytes::ZBytes::from(reply)) + .wait() + .unwrap(); + }) + .wait() + .unwrap(); + + thread::sleep(Duration::from_secs(2)); + + let mut child = Command::new("ros2"); + child + .args(["run", "action_tutorials_cpp", "fibonacci_action_client"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", domain_id.to_string()) + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::piped()) + .stderr(Stdio::null()); + use std::os::unix::process::CommandExt; + child.process_group(0); + let client_child = child.spawn().expect("spawn fibonacci_action_client"); + + let output = { + let timeout = std::time::Instant::now() + Duration::from_secs(25); + let mut c = client_child; + loop { + match c.try_wait() { + Ok(Some(_)) => break c.wait_with_output().unwrap(), + Ok(None) if std::time::Instant::now() < timeout => { + thread::sleep(Duration::from_millis(300)); + } + _ => break kill_group_and_collect(c), + } + } + }; + let stdout = String::from_utf8_lossy(&output.stdout); + assert!( + output.status.success() || stdout.contains("sequence"), + "DDS action client (rmw-zenoh bridge) did not receive action result; stdout={stdout:?}" + ); +} + +// ── Test 15: rmw-zenoh action, Zenoh client → bridge → DDS action server ─────── + +/// Mirrors test 6 (`test_zenoh_client_ros_action`) using the default rmw-zenoh +/// wire format. Zenoh queries go to `0/fibonacci/_action//EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH` +/// which matches the bridge's wildcard `ZDdsServiceBridge` queryable. +#[test] +fn test_rosz_action_dds_server_rmw_zenoh() { + if !ros2_available() || !pkg_available("action_tutorials_cpp") { + eprintln!( + "Skipping test_rosz_action_dds_server_rmw_zenoh: action_tutorials_cpp not available" + ); + return; + } + + let domain_id = 62u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge(&endpoint, domain_id); + let _server = spawn_cyclone( + domain_id, + "action_tutorials_cpp", + "fibonacci_action_server", + &[], + ); + thread::sleep(Duration::from_secs(3)); + + let rt = tokio::runtime::Runtime::new().unwrap(); + let (tx, rx) = std::sync::mpsc::channel::(); + + rt.block_on(async move { + let mut cfg = zenoh::Config::default(); + cfg.insert_json5("connect/endpoints", &format!(r#"["{}"]"#, endpoint)) + .unwrap(); + cfg.insert_json5("scouting/multicast/enabled", "false") + .unwrap(); + cfg.insert_json5("mode", r#""client""#).unwrap(); + let session = zenoh::open(cfg).await.unwrap(); + + let goal_id = [1u8; 16]; + let order = 5i32; + + // send_goal: key matches bridge's wildcard ZDdsServiceBridge queryable + let mut sg_payload = Vec::with_capacity(4 + 16 + 4); + sg_payload.extend_from_slice(&[0, 1, 0, 0]); + sg_payload.extend_from_slice(&goal_id); + sg_payload.extend_from_slice(&order.to_le_bytes()); + + let sg_replies: Vec<_> = session + .get("0/fibonacci/_action/send_goal/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .payload(zenoh::bytes::ZBytes::from(sg_payload)) + .timeout(Duration::from_secs(10)) + .await + .unwrap() + .into_iter() + .collect(); + + if sg_replies.is_empty() { + tx.send(false).unwrap(); + return; + } + + // subscribe to feedback (wildcard catches bridge's ZDdsPubBridge publication) + let fb_received = Arc::new(Mutex::new(false)); + let fb_clone = fb_received.clone(); + let _fb_sub = session + .declare_subscriber("0/fibonacci/_action/feedback/**") + .callback(move |_| { + *fb_clone.lock().unwrap() = true; + }) + .await + .unwrap(); + + // get_result + let mut gr_payload = Vec::with_capacity(4 + 16); + gr_payload.extend_from_slice(&[0, 1, 0, 0]); + gr_payload.extend_from_slice(&goal_id); + + let gr_replies: Vec<_> = session + .get("0/fibonacci/_action/get_result/EMPTY_TOPIC_TYPE/EMPTY_TOPIC_HASH") + .payload(zenoh::bytes::ZBytes::from(gr_payload)) + .timeout(Duration::from_secs(15)) + .await + .unwrap() + .into_iter() + .collect(); + + tx.send(!gr_replies.is_empty()).unwrap(); + }); + + let got = rx.recv_timeout(Duration::from_secs(40)).unwrap_or(false); + rt.shutdown_background(); + assert!( + got, + "Zenoh action client (rmw-zenoh bridge) did not receive result from DDS fibonacci_action_server" + ); +} + +// ── Test 16: bridge-to-bridge federation — pub/sub ─────────────────────────── + +/// Two bridge instances run on separate DDS domains but share the same Zenoh router. +/// A DDS talker in domain 63 publishes on `/chatter`. Bridge A (domain 63) routes +/// it to Zenoh and declares a Publisher liveliness token. Bridge B (domain 64) sees +/// that token and creates a ZDdsSubBridge in domain 64, making the messages available +/// to DDS listeners in domain 64. +#[test] +fn test_federation_pub_sub() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_federation_pub_sub: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + // Both bridges connect to the same router; different DDS domains. + let _bridge_a = spawn_bridge(&endpoint, 63); + let _bridge_b = spawn_bridge(&endpoint, 64); + + // DDS talker in domain 63 — Bridge A discovers it and creates ZDdsPubBridge. + let _talker = spawn_cyclone(63, "demo_nodes_cpp", "talker", &[]); + + // Wait for Bridge A's Publisher liveliness to propagate to Bridge B and for + // Bridge B to create a ZDdsSubBridge (DDS writer) in domain 64. + thread::sleep(Duration::from_secs(8)); + + // Start DDS listener in domain 64 — receives messages via the federation chain. + let mut listener_cmd = Command::new("ros2"); + listener_cmd + .args(["run", "demo_nodes_cpp", "listener"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", "64") + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::piped()); + use std::os::unix::process::CommandExt; + listener_cmd.process_group(0); + let listener_child = listener_cmd + .spawn() + .expect("Failed to spawn listener (domain 64)"); + + thread::sleep(Duration::from_secs(15)); + + let output = kill_group_and_collect(listener_child); + let stderr = String::from_utf8_lossy(&output.stderr); + assert!( + stderr.contains("Hello"), + "federation pub/sub: DDS listener in domain 64 received nothing; stderr={stderr:?}" + ); +} + +// ── Test 17: bridge-to-bridge federation — service ─────────────────────────── + +/// Two bridge instances share a Zenoh router across separate DDS domains. +/// A DDS add_two_ints_server lives in domain 66. Bridge B (domain 66) discovers +/// it and creates a ZDdsServiceBridge, then declares a Service liveliness token. +/// Bridge A (domain 65) sees the token and creates a ZDdsClientBridge in domain 65. +/// A DDS add_two_ints_client in domain 65 calls the service and gets sum=3 via the +/// full cross-domain federation chain. +#[test] +fn test_federation_service() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_federation_service: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge_a = spawn_bridge(&endpoint, 65); // DDS client side + let _bridge_b = spawn_bridge(&endpoint, 66); // DDS server side + + // Service server in domain 66 — Bridge B creates ZDdsServiceBridge. + let _server = spawn_cyclone(66, "demo_nodes_cpp", "add_two_ints_server", &[]); + + // Wait for Bridge B's Service liveliness to reach Bridge A and for Bridge A to + // create a ZDdsClientBridge in domain 65. + thread::sleep(Duration::from_secs(8)); + + // DDS client in domain 65 calls through the federation chain. + let mut client_cmd = Command::new("ros2"); + client_cmd + .args(["run", "demo_nodes_cpp", "add_two_ints_client"]) + .env("RMW_IMPLEMENTATION", "rmw_cyclonedds_cpp") + .env("ROS_DOMAIN_ID", "65") + .env("CYCLONEDDS_URI", CYCLONEDDS_URI) + .stdout(Stdio::null()) + .stderr(Stdio::piped()); + use std::os::unix::process::CommandExt; + client_cmd.process_group(0); + let client_child = client_cmd + .spawn() + .expect("spawn add_two_ints_client (domain 65)"); + + let output = { + let deadline = std::time::Instant::now() + Duration::from_secs(20); + let mut child = client_child; + loop { + match child.try_wait() { + Ok(Some(_)) => break child.wait_with_output().unwrap(), + Ok(None) if std::time::Instant::now() < deadline => { + thread::sleep(Duration::from_millis(200)); + } + _ => break kill_group_and_collect(child), + } + } + }; + let stderr = String::from_utf8_lossy(&output.stderr); + assert!( + stderr.contains("3") || output.status.success(), + "federation service: DDS client in domain 65 did not receive reply; stderr={stderr:?}" + ); +} + +// ── Test 18: topic --deny filter ────────────────────────────────────────────── + +/// Bridge started with `--deny "^rt/chatter$"` must not route the /chatter topic. +/// A DDS talker publishes on /chatter; a Zenoh subscriber should receive nothing. +#[test] +fn test_topic_deny_filter() { + if !ros2_available() || !pkg_available("demo_nodes_cpp") { + eprintln!("Skipping test_topic_deny_filter: ROS 2 / demo_nodes_cpp not available"); + return; + } + + let domain_id = 67u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + let _bridge = spawn_bridge_extra( + &endpoint, + domain_id, + "rmw-zenoh", + &["--deny", "^rt/chatter$"], + ); + + let received = Arc::new(Mutex::new(false)); + let received_clone = received.clone(); + + let session = zenoh_session(&endpoint); + let _sub = session + .declare_subscriber("0/chatter/**") + .callback(move |_| { + *received_clone.lock().unwrap() = true; + }) + .wait() + .unwrap(); + + let _talker = spawn_cyclone(domain_id, "demo_nodes_cpp", "talker", &[]); + + // Wait long enough that messages would have arrived if not filtered. + thread::sleep(Duration::from_secs(8)); + + assert!( + !*received.lock().unwrap(), + "deny filter failed: Zenoh subscriber received messages from denied /chatter topic" + ); +} + +// ── Test 19: ZDdsServiceBridge::new() API construction ─────────────────────── + +/// Verifies that `ZDdsServiceBridge::new()` constructs without error when given a +/// valid ZNode and CyclorsParticipant. No DDS traffic — pure construction test. +#[cfg(feature = "dds-bridge-interop")] +#[test] +fn test_api_service_bridge() { + use ros_z::{Builder, context::ZContextBuilder}; + + let domain_id = 76u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + unsafe { std::env::set_var("CYCLONEDDS_URI", CYCLONEDDS_URI) }; + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let ctx = ZContextBuilder::default() + .with_connect_endpoints([endpoint.as_str()]) + .build() + .expect("ZContext"); + let node = ctx + .create_node("test_api_service_bridge") + .build() + .expect("ZNode"); + let participant = CyclorsParticipant::create(domain_id).expect("participant"); + + let bridge = ZDdsServiceBridge::new( + &node, + "/add_two_ints", + "example_interfaces/srv/AddTwoInts", + &participant, + BridgeQos::default(), + ) + .await; + + assert!( + bridge.is_ok(), + "ZDdsServiceBridge::new failed: {:?}", + bridge.err() + ); + }); +} + +// ── Test 20: ZDdsClientBridge::new() API construction ──────────────────────── + +/// Verifies that `ZDdsClientBridge::new()` constructs without error when given a +/// valid ZNode and CyclorsParticipant. No DDS traffic — pure construction test. +#[cfg(feature = "dds-bridge-interop")] +#[test] +fn test_api_client_bridge() { + use ros_z::{Builder, context::ZContextBuilder}; + + let domain_id = 77u32; + let router = TestRouter::new(); + let endpoint = router.endpoint().to_string(); + + unsafe { std::env::set_var("CYCLONEDDS_URI", CYCLONEDDS_URI) }; + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let ctx = ZContextBuilder::default() + .with_connect_endpoints([endpoint.as_str()]) + .build() + .expect("ZContext"); + let node = ctx + .create_node("test_api_client_bridge") + .build() + .expect("ZNode"); + let participant = CyclorsParticipant::create(domain_id).expect("participant"); + + let bridge = ZDdsClientBridge::new( + &node, + "/add_two_ints", + "example_interfaces/srv/AddTwoInts", + None, + &participant, + BridgeQos::default(), + Duration::from_secs(10), + ) + .await; + + assert!( + bridge.is_ok(), + "ZDdsClientBridge::new failed: {:?}", + bridge.err() + ); + }); +} diff --git a/crates/ros-z/src/node.rs b/crates/ros-z/src/node.rs index f0b9a9d4..b4a9ad7c 100644 --- a/crates/ros-z/src/node.rs +++ b/crates/ros-z/src/node.rs @@ -723,6 +723,16 @@ impl ZNode { &self.session } + /// Get the key expression format configured on this node's context. + pub fn keyexpr_format(&self) -> &ros_z_protocol::KeyExprFormat { + &self.keyexpr_format + } + + /// Allocate the next unique entity ID from the context counter. + pub fn next_entity_id(&self) -> usize { + self.counter.increment() + } + /// Access this node's clock. pub fn clock(&self) -> &crate::time::ZClock { &self.clock @@ -999,4 +1009,53 @@ mod tests { let rules = RemapRules::default(); assert_eq!(rules.apply("/foo"), "/foo"); } + + #[test] + fn test_counter_increment_sequential() { + let counter = GlobalCounter::default(); + let a = counter.increment(); + let b = counter.increment(); + assert!(b > a, "counter should be monotonically increasing"); + } + + #[test] + fn test_znode_keyexpr_format_and_next_entity_id() { + use std::collections::HashMap; + + let session = + Arc::new(zenoh::Wait::wait(zenoh::open(zenoh::Config::default())).expect("zenoh open")); + let graph = Arc::new( + crate::graph::Graph::new(&session, 0, ros_z_protocol::KeyExprFormat::default()) + .expect("graph"), + ); + let counter = Arc::new(GlobalCounter::default()); + let builder = ZNodeBuilder { + domain_id: 0, + name: "test_node".to_string(), + namespace: String::new(), + enclave: String::new(), + session, + counter, + graph, + remap_rules: RemapRules::default(), + clock: crate::time::ZClock::default(), + shm_config: None, + keyexpr_format: ros_z_protocol::KeyExprFormat::default(), + enable_type_desc_service: false, + enable_parameters: false, + parameter_overrides: HashMap::new(), + }; + let node = crate::Builder::build(builder).expect("build node"); + + assert_eq!( + *node.keyexpr_format(), + ros_z_protocol::KeyExprFormat::default() + ); + let id1 = node.next_entity_id(); + let id2 = node.next_entity_id(); + assert!( + id2 > id1, + "next_entity_id should be monotonically increasing" + ); + } } diff --git a/docs/user-guide/dds-bridge.md b/docs/user-guide/dds-bridge.md new file mode 100644 index 00000000..a9f28867 --- /dev/null +++ b/docs/user-guide/dds-bridge.md @@ -0,0 +1,453 @@ +# DDS Bridge + +`ros-z-bridge-dds` is a standalone binary that connects **existing DDS-based ROS 2 nodes** (using any DDS middleware: Fast DDS, CycloneDDS, Connext, …) to a Zenoh/ros-z network. Once running, every ros-z node, Python node, Go node, and `rmw_zenoh_cpp` node on the Zenoh side can communicate transparently with any node on the DDS side — no recompilation or code changes needed. + +!!! tip "Wrong bridge?" + If you need to connect a **Humble** network to a **Jazzy/Kilted** network (both already using Zenoh), see the [Cross-Distro Bridge](./bridge.md) chapter instead. + +## How It Works + +The bridge runs a CycloneDDS participant in a chosen DDS domain. It watches DDS discovery traffic and creates matching Zenoh routes for every publisher, subscriber, service server, and service client it sees. Messages flow as raw CDR bytes in both directions — no deserialization occurs inside the bridge. + +```mermaid +graph LR +accTitle: ros-z-bridge-dds connecting a DDS domain to a Zenoh network +accDescr: A ROS 2 talker uses rmw_cyclonedds_cpp to publish into DDS domain 0. The bridge subscribes in domain 0 and republishes on a Zenoh key expression. A ros-z listener and an rmw_zenoh_cpp listener both receive the message from the Zenoh router. + + talker["ROS 2 talker
(rmw_cyclonedds_cpp)"] + listener_cpp["ROS 2 listener
(rmw_zenoh_cpp)"] + listener_rz["ros-z listener"] + + subgraph DDS ["DDS domain 0"] + talker + end + + bridge["ros-z-bridge-dds"] + + subgraph Zenoh ["Zenoh network"] + router(["Zenoh router"]) + listener_cpp + listener_rz + end + + talker -- "DDS / CycloneDDS" --> bridge + bridge -- "Zenoh" --> router + router --> listener_cpp + router --> listener_rz +``` + +The bridge also publishes `ros_discovery_info` so that `ros2 topic list` / `ros2 service list` on the DDS side shows the bridge's Zenoh entities, and vice versa. + +## Installation + +### Build from Source + +Requires Rust 1.85+: + +```bash +cargo build --release -p ros-z-bridge-dds +``` + +The binary is at `target/release/ros-z-bridge-dds`. + +## Quick Start + +### Prerequisites + +- A Zenoh router on `localhost:7447` (see [Networking](./networking.md)) +- A ROS 2 installation with any DDS middleware (Fast DDS, CycloneDDS, …) + +### 1 — Start a Zenoh router + +```bash +# From the ros-z repository +cargo run --example zenoh_router + +# Or with zenohd +zenohd +``` + +### 2 — Start the bridge + +```bash +./ros-z-bridge-dds +``` + +By default the bridge connects to `tcp/127.0.0.1:7447` and joins DDS domain 0. + +### 3 — Run a DDS talker + +```bash +# Uses the system DDS middleware (rmw_cyclonedds_cpp, rmw_fastrtps_cpp, …) +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +ros2 run demo_nodes_cpp talker +``` + +### 4 — Receive on the Zenoh side + +```bash +# ros-z listener +cargo run --example demo_nodes_listener +``` + +The ros-z listener prints messages published by the DDS talker — no shared router configuration needed on the DDS side. + +## CLI Reference + +```text +USAGE: + ros-z-bridge-dds [OPTIONS] + +OPTIONS: + -z, --zenoh-endpoint + Zenoh endpoint to connect to [default: tcp/127.0.0.1:7447] + + -d, --domain-id + ROS 2 domain ID. Defaults to ROS_DOMAIN_ID env var, or 0 + + -n, --namespace + Namespace prefix applied to all bridged topics/services on the Zenoh side + + --node-name + Node name used for liveliness tokens [default: zenoh_bridge_dds] + + --allow + Topic allow-list. Only DDS topic names matching this pattern are bridged + + --deny + Topic deny-list. DDS topic names matching this pattern are not bridged + + --service-timeout-secs + Timeout for Zenoh get() calls on service routes [default: 10] + + --action-get-result-timeout-secs + Timeout for action get_result Zenoh get() calls [default: 300] + + --transient-local-cache-multiplier + TRANSIENT_LOCAL AdvancedPublisher cache depth multiplier [default: 10] + + --wire-format + Zenoh key expression wire format [default: rmw-zenoh] + [possible values: rmw-zenoh, ros2dds] + + -h, --help + Print help +``` + +## Wire Format + +The bridge supports two Zenoh key expression formats, selected with `--wire-format`: + +| Value | Compatible with | When to use | +|---|---|---| +| `rmw-zenoh` (default) | ros-z, rmw_zenoh_cpp v0.2+ | New deployments. All ros-z nodes and modern `rmw_zenoh_cpp` nodes. | +| `ros2dds` | zenoh-plugin-ros2dds | Existing deployments using the legacy Zenoh DDS plugin. | + +When in doubt, use the default. Switch to `ros2dds` only if you have existing infrastructure built around `zenoh-plugin-ros2dds` key expressions. + +## Topic Filtering + +Use `--allow` and `--deny` to control which topics and services are bridged. Both accept [RE2 regular expressions](https://github.com/google/re2/wiki/Syntax) matched against the **raw DDS topic name** (e.g. `rt/chatter`, `rq/add_two_intsRequest`). + +```bash +# Bridge only /chatter and /tf +ros-z-bridge-dds --allow "^rt/(chatter|tf)$" + +# Bridge everything except /rosout +ros-z-bridge-dds --deny "^rt/rosout$" + +# Combine: allow navigation topics but not raw odometry +ros-z-bridge-dds --allow "^rt/nav" --deny "^rt/nav/raw_odom$" +``` + +If both `--allow` and `--deny` are set, `--deny` is evaluated first. + +## Bridge-to-Bridge Federation + +When two bridge instances run on separate DDS domains and connect to the **same Zenoh network**, they automatically federate — each bridge detects the other's liveliness tokens and creates complementary local DDS routes. + +```mermaid +graph LR +accTitle: Two ros-z-bridge-dds instances federating across a shared Zenoh router +accDescr: Bridge A in DDS domain 0 detects Bridge B's Subscription liveliness token and creates a ZDdsPubBridge for /chatter in domain 0. Bridge B in DDS domain 1 detects Bridge A's Publisher liveliness token and creates a ZDdsSubBridge for /chatter in domain 1. + + subgraph A ["DDS domain 0"] + nodeA["ROS 2 node
(publisher)"] + end + + subgraph Zenoh ["Zenoh network"] + router(["Zenoh router"]) + end + + subgraph B ["DDS domain 1"] + nodeB["ROS 2 node
(subscriber)"] + end + + bridgeA["Bridge A"] + bridgeB["Bridge B"] + + nodeA -- "DDS" --> bridgeA + bridgeA -- "Zenoh" --> router + router -- "Zenoh" --> bridgeB + bridgeB -- "DDS" --> nodeB +``` + +**Federation rules:** + +| Remote liveliness token | Local route created | +|---|---| +| Publisher on `/chatter` | `ZDdsSubBridge`: Zenoh→DDS relay so domain B subscribers receive data | +| Subscription on `/chatter` | `ZDdsPubBridge`: DDS→Zenoh relay so domain A publishers have a consumer | +| Service server on `/add_two_ints` | `ZDdsClientBridge`: local DDS clients can call the remote service | +| Service client on `/add_two_ints` | `ZDdsServiceBridge`: local DDS servers can respond to remote clients | + +Federation happens automatically — no configuration required. Each bridge filters out its own liveliness tokens to avoid self-routes. + +## DDS Discovery Scope + +By default CycloneDDS discovers peers via multicast, which may pick up unintended DDS participants. Use the `CYCLONEDDS_URI` environment variable to scope discovery: + +```bash +# Localhost only +export CYCLONEDDS_URI=' + +' + +# Single subnet (192.168.1.0/24) +export CYCLONEDDS_URI=' + +' + +# Unicast-only (disable multicast) +export CYCLONEDDS_URI=' + false +' +``` + +!!! note + `ROS_LOCALHOST_ONLY` and `ROS_AUTOMATIC_DISCOVERY_RANGE` are read by `rmw_cyclonedds_cpp` when ROS 2 initialises the DDS layer. The bridge bypasses `rmw` and talks to CycloneDDS directly, so those variables have no effect. Use `CYCLONEDDS_URI` instead. + +## What Gets Bridged + +| Entity | Direction | Notes | +|---|---|---| +| Topics (pub/sub) | DDS ↔ Zenoh | Raw CDR bytes forwarded unchanged. TRANSIENT_LOCAL durability uses `AdvancedPublisher` cache. | +| Services | DDS ↔ Zenoh | Correct CDR framing: `[4B CDR hdr] + [8B client_guid] + [8B seq_num] + [body]`. | +| Actions | DDS ↔ Zenoh | Actions are services under the hood (`/_action/send_goal`, `/_action/get_result`, `/_action/cancel_goal`, `/_action/feedback`). All sub-topics are bridged. | +| `ros_discovery_info` | Bridge → DDS | Updated at ~1 Hz so `ros2 topic list` on the DDS side shows bridge entities. | + +## Programmatic API + +In addition to the standalone binary, `ros-z-dds` exposes the bridge as a Rust library for embedding in your own applications. + +### Auto-Discovery Bridge + +The simplest approach: let the bridge discover and route everything automatically. + +```rust +--8<-- "crates/ros-z-dds/examples/auto_bridge.rs" +``` + +Run it: + +```bash +cargo run --example auto_bridge -p ros-z-dds +``` + +### Typed Bridge (DdsBridgeExt) + +Use `DdsBridgeExt` to bridge specific typed topics at compile time, with full type-hash embedding in the liveliness token: + +```rust +--8<-- "crates/ros-z-dds/examples/custom_bridge.rs" +``` + +Run it: + +```bash +cargo run --example custom_bridge -p ros-z-dds +``` + +### Manual Route Construction + +For full control, construct routes directly: + +```rust +use std::time::Duration; +use ros_z_dds::{CyclorsParticipant, ZDdsPubBridge, ZDdsSubBridge, ZDdsServiceBridge, ZDdsClientBridge}; +use ros_z_dds::participant::BridgeQos; +use ros_z_protocol::entity::TypeHash; + +// DDS publisher → Zenoh publisher (no type hash — any DDS node) +let route = ZDdsPubBridge::new( + &node, + "/chatter", + "std_msgs/msg/String", + None, // type_hash: None uses EMPTY_TOPIC_HASH in topic key expr + &participant, + BridgeQos::default(), + true, // keyless + 10, // TRANSIENT_LOCAL cache multiplier +) +.await?; + +// Zenoh subscriber → DDS writer +let route = ZDdsSubBridge::new( + &node, "/chatter", "std_msgs/msg/String", + None, &participant, BridgeQos::default(), true, +) +.await?; + +// DDS service server → Zenoh queryable (DDS clients call a ros-z service server) +let route = ZDdsServiceBridge::new( + &node, "/add_two_ints", "example_interfaces/srv/AddTwoInts", + &participant, BridgeQos::default(), +) +.await?; + +// Zenoh queryable → DDS service client (ros-z clients call a DDS service server) +let route = ZDdsClientBridge::new( + &node, "/add_two_ints", "example_interfaces/srv/AddTwoInts", + None, &participant, BridgeQos::default(), + Duration::from_secs(10), // timeout for the Zenoh get() call +) +.await?; +``` + +Routes are RAII handles — drop them to tear down the bridge. All bridges are `Send + Sync` and can be stored in a struct. + +## Migrating from zenoh-plugin-ros2dds + +`ros-z-bridge-dds` is the successor to `zenoh-bridge-ros2dds` (the binary bundled with `zenoh-plugin-ros2dds`). The two bridges are wire-compatible when you pass `--wire-format ros2dds` to the new bridge. + +### CLI Flag Mapping + +| `zenoh-bridge-ros2dds` flag | `ros-z-bridge-dds` equivalent | Notes | +|---|---|---| +| `--namespace ` | `--namespace ` | Identical | +| `--nodename ` | `--node-name ` | Renamed (hyphen) | +| `--domain ` | `--domain-id ` | Renamed | +| `--allow ` | `--allow ` | Identical | +| `--deny ` | `--deny ` | Identical | +| `--queries_timeout_default ` | `--service-timeout-secs ` | Action get_result has its own `--action-get-result-timeout-secs` flag (default 300 s) | +| `--transient_local_cache_multiplier ` | `--transient-local-cache-multiplier ` | Renamed (hyphens) | +| `--ros_localhost_only` | — | Use `CYCLONEDDS_URI` to scope DDS discovery. See [DDS Discovery Scope](#dds-discovery-scope). | +| `--ros_automatic_discovery_range ` | — | Use `CYCLONEDDS_URI` | +| `--ros_static_peers ` | — | Use `CYCLONEDDS_URI` | +| `--pub_max_frequency ` | — | Not implemented. Throttle at the DDS layer via QoS instead. | +| `--rest_http_port ` | — | Not implemented | +| `--watchdog` | — | Not implemented | +| `--dds_enable_shm` | — | Not implemented | + +### Feature Comparison + +| Feature | zenoh-plugin-ros2dds | ros-z-bridge-dds | +|---|---|---| +| Pub/sub bridging | Yes | Yes | +| Service bridging | Yes | Yes | +| Action bridging | Yes | Yes | +| `ros_discovery_info` | Yes | Yes | +| Bridge-to-bridge federation | Yes | Yes | +| Per-entity-type allow/deny | Yes (separate regex per publisher/subscriber/service_server/…) | No — single `--allow`/`--deny` matched against the raw DDS topic name | +| Per-topic rate limiting | Yes (`--pub_max_frequency`) | No | +| Per-topic Zenoh priorities | Yes | No | +| Admin space route introspection | Yes | No | +| DDS SHM | Yes | No | +| Configurable action get_result timeout | No (shared `queries_timeout`) | Yes (`--action-get-result-timeout-secs`, default 300 s) | +| Embedded library API | No | Yes (`ros-z-dds` crate) | + +### Wire Format and Gradual Migration + +By default, `ros-z-bridge-dds` uses the `rmw-zenoh` key expression format, which is not compatible with `zenoh-plugin-ros2dds`. To run both bridges side-by-side during a migration: + +```bash +# New bridge, compatible with zenoh-plugin-ros2dds infrastructure +./ros-z-bridge-dds --wire-format ros2dds +``` + +Once all `zenoh-plugin-ros2dds` instances are replaced, drop the flag to use the default `rmw-zenoh` format, which is also understood by all ros-z nodes and `rmw_zenoh_cpp` nodes. + +### Allowance Model Change + +The old plugin let you allow/deny each entity type independently: + +```json5 +// zenoh-plugin-ros2dds config (old) +allowance: { + publishers: { allow: "^rt/chatter$" }, + subscribers: { allow: "^rt/chatter$" }, + service_servers: { deny: ".*" }, + service_clients: { deny: ".*" }, +} +``` + +The new bridge uses a single `--allow`/`--deny` regex matched against the raw DDS topic name, which covers all entity types: + +```bash +# Bridge only /chatter topics and services (all entity types) +ros-z-bridge-dds --allow "^rt/chatter$|^r[qr]/chatter" +``` + +If you need to suppress services entirely, use `--deny` with a service topic pattern: + +```bash +# Bridge topics but not services +ros-z-bridge-dds --allow "^rt/" --deny "^r[qr]/" +``` + + + +## Troubleshooting + +??? question "DDS nodes do not appear on the Zenoh side" + Enable debug logging to watch discovery events: + + ```bash + RUST_LOG=ros_z_dds=debug ./ros-z-bridge-dds + ``` + + Look for lines like: + + ```text + INFO ros_z_dds::pubsub: ZDdsPubBridge: DDS rt/chatter → Zenoh 0/chatter/std_msgs%msg%String/RIHS01_… + ``` + + If no discovery lines appear, check: + + 1. The bridge and the DDS node are on the same DDS domain (`ROS_DOMAIN_ID` / `--domain-id`). + 2. No firewall blocks DDS multicast (UDP port 7400 by default). + 3. CycloneDDS can reach the DDS nodes — check `CYCLONEDDS_URI` scoping. + +??? question "`ros2 topic list` does not show bridge topics" + `ros_discovery_info` is published at ~1 Hz. Wait a second after the bridge starts, then retry with `--spin-time 5 --no-daemon` to avoid stale daemon cache: + + ```bash + ros2 topic list --spin-time 5 --no-daemon + ``` + +??? question "Service calls time out" + The bridge proxies Zenoh `get()` calls with a configurable timeout (default 10 s). If the ros-z service server is slow to start or the Zenoh router is under load, the first call may time out. Increase the timeout: + + ```bash + ./ros-z-bridge-dds --service-timeout-secs 30 + ``` + + Check that a matching queryable is registered: + + ```bash + RUST_LOG=ros_z_dds=debug ./ros-z-bridge-dds 2>&1 | grep -i "service\|queryable" + ``` + +??? question "Federation routes not appearing" + Federation relies on Zenoh liveliness tokens. Ensure both bridges connect to the **same Zenoh router** (or a federated Zenoh network). Check that both bridges use the **same `--wire-format`** — mixing `rmw-zenoh` on one side with `ros2dds` on the other will prevent liveliness token parsing. + + ```bash + RUST_LOG=ros_z_dds::bridge=debug ./ros-z-bridge-dds 2>&1 | grep -i "federation" + ``` + +??? question "Messages arrive corrupted" + The bridge forwards raw CDR bytes without any transformation. If messages are corrupted: + + 1. Both sides must use the **same CDR encoding** (little-endian is standard). + 2. The message definition must be **identical** on both sides. Mismatched field order or types cause silent corruption. + 3. Service CDR payloads include a 20-byte header prepended by the bridge (`[4B hdr] + [8B guid] + [8B seq]`). Custom service bridge code must handle this framing. diff --git a/docs/user-guide/examples.md b/docs/user-guide/examples.md index 39665712..319137bb 100644 --- a/docs/user-guide/examples.md +++ b/docs/user-guide/examples.md @@ -310,3 +310,41 @@ cargo run --example z_parameter_callback cargo run --example z_parameter_yaml cargo run --example z_parameter_client ``` + +--- + +## DDS Bridge Examples + +These examples demonstrate the `ros-z-dds` library — connecting existing DDS-based ROS 2 nodes to a Zenoh/ros-z network. See the [DDS Bridge](./dds-bridge.md) chapter for comprehensive documentation. + +Both examples require a Zenoh router running at `tcp/127.0.0.1:7447` and a ROS 2 installation with CycloneDDS (or another DDS middleware). + +### Auto Bridge + +Discovers and bridges every DDS publisher, subscriber, service server, and service client automatically. Equivalent to running the `ros-z-bridge-dds` binary. + +```bash +cargo run --example auto_bridge -p ros-z-dds +``` + +With a specific DDS domain: + +```bash +ROS_DOMAIN_ID=42 cargo run --example auto_bridge -p ros-z-dds +``` + +### Custom Bridge + +Bridges specific typed topics at compile time using `DdsBridgeExt`. Bridges `/chatter` (`std_msgs/String`) with full type-hash embedding in the liveliness token — remote bridges and ros-z nodes see the exact ROS 2 type. + +```bash +cargo run --example custom_bridge -p ros-z-dds +``` + +Then test from the DDS side: + +```bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +ros2 run demo_nodes_cpp talker # publishes /chatter +ros2 run demo_nodes_cpp listener # subscribes to /chatter +``` diff --git a/mkdocs.yml b/mkdocs.yml index 1fa527f5..e2c343a3 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -99,6 +99,7 @@ nav: - Advanced: user-guide/config-advanced.md - ROS 2 Interoperability: user-guide/interop.md - Cross-Distro Bridge: user-guide/bridge.md + - DDS Bridge: user-guide/dds-bridge.md - Messages & Serialization: - Message Cache: core-concepts/message-cache.md - Message Generation: user-guide/message-generation.md