diff --git a/Cargo.lock b/Cargo.lock index 76008359..e6611485 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2835,6 +2835,7 @@ dependencies = [ "pyo3", "ros-z-cdr", "ros-z-codegen", + "ros-z-derive", "ros-z-msgs", "ros-z-protocol", "ros-z-schema", @@ -2848,6 +2849,7 @@ dependencies = [ "tokio", "tokio-util", "tracing", + "trybuild", "uuid", "zenoh", "zenoh-buffers", @@ -3402,6 +3404,15 @@ dependencies = [ "serde_core", ] +[[package]] +name = "serde_spanned" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8bbf91e5a4d6315eee45e704372590b30e260ee83af6639d64557f51b067776" +dependencies = [ + "serde_core", +] + [[package]] name = "serde_with" version = "3.15.1" @@ -3852,6 +3863,12 @@ version = "0.12.16" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" +[[package]] +name = "target-triple" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "591ef38edfb78ca4771ee32cf494cb8771944bee237a9b91fc9c1424ac4b777b" + [[package]] name = "tempfile" version = "3.23.0" @@ -4093,6 +4110,21 @@ dependencies = [ "tokio", ] +[[package]] +name = "toml" +version = "0.9.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0dc8b1fb61449e27716ec0e1bdf0f6b8f3e8f6b05391e8497b8b6d7804ea6d8" +dependencies = [ + "indexmap 2.12.0", + "serde_core", + "serde_spanned", + "toml_datetime", + "toml_parser", + "toml_writer", + "winnow", +] + [[package]] name = "toml_datetime" version = "0.7.3" @@ -4123,6 +4155,12 @@ dependencies = [ "winnow", ] +[[package]] +name = "toml_writer" +version = "1.0.6+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ab16f14aed21ee8bfd8ec22513f7287cd4a91aa92e44edfe2c17ddd004e92607" + [[package]] name = "tracing" version = "0.1.41" @@ -4198,6 +4236,21 @@ dependencies = [ "tracing-serde", ] +[[package]] +name = "trybuild" +version = "1.0.115" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f614c21bd3a61bad9501d75cbb7686f00386c806d7f456778432c25cf86948a" +dependencies = [ + "glob", + "serde", + "serde_derive", + "serde_json", + "target-triple", + "termcolor", + "toml", +] + [[package]] name = "tungstenite" version = "0.24.0" diff --git a/book/src/chapters/custom_messages.md b/book/src/chapters/custom_messages.md index 12fa9f45..b15fba89 100644 --- a/book/src/chapters/custom_messages.md +++ b/book/src/chapters/custom_messages.md @@ -2,10 +2,10 @@ ros-z supports two approaches for defining custom message types: -| Approach | Definition | Best For | -|----------|------------|----------| -| **Rust-Native** | Write Rust structs directly | Prototyping, ros-z-only systems | -| **Schema-Generated** | Write `.msg`/`.srv` files, generate Rust | Production, ROS 2 interop | +| Approach | Definition | Best For | +| -------------------- | ---------------------------------------- | ------------------------------- | +| **Rust-Native** | Write Rust structs directly | Prototyping, ros-z-only systems | +| **Schema-Generated** | Write `.msg`/`.srv` files, generate Rust | Production, ROS 2 interop | ```mermaid flowchart TD @@ -21,38 +21,38 @@ flowchart TD ## Rust-Native Messages -**Define messages directly in Rust by implementing required traits.** This approach is fast for prototyping but only works between ros-z nodes. +**Define messages directly in Rust and derive their schema metadata.** This approach is fast for prototyping, and plain named structs can still participate in the standard ROS 2 type description service. ```admonish warning -Rust-Native messages use `TypeHash::zero()` and won't interoperate with ROS 2 C++/Python nodes. +Rust-native messages defined with `#[derive(MessageTypeInfo)]` are limited to ROS 2 schema-compatible shapes. If you need `Option`, enums, or other ros-z-only schema extensions, use `#[derive(ExtendedMessageTypeInfo)]` plus the parallel extended type description service instead of the standard ROS 2 type description service. ``` ### Workflow of Rust-Native Messages ```mermaid graph LR - A[Define Struct] --> B[Impl MessageTypeInfo] + A[Define Struct] --> B[Derive MessageTypeInfo] B --> C[Add Serde Traits] - C --> D[Impl WithTypeInfo] + C --> D[Impl ZMessage] D --> E[Use in Pub/Sub] ``` ### Required Traits -| Trait | Purpose | Key Method | -|-------|---------|------------| -| **MessageTypeInfo** | Type identification | `type_name()`, `type_hash()` | -| **WithTypeInfo** | ros-z integration | `type_info()` | -| **Serialize/Deserialize** | Data encoding | From `serde` | +| Trait | Purpose | Key Method | +| ------------------------- | ------------------------------------ | ------------------------------------------------ | +| **MessageTypeInfo** | Type identification + runtime schema | `type_name()`, `type_hash()`, `message_schema()` | +| **Serialize/Deserialize** | Data encoding | From `serde` | +| **ZMessage** | ros-z serialization path | `type Serdes = SerdeCdrSerdes` | ### Message Example ```rust,ignore -use ros_z::{MessageTypeInfo, entity::TypeHash}; -use ros_z::ros_msg::WithTypeInfo; +use ros_z::MessageTypeInfo; use serde::{Serialize, Deserialize}; -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Serialize, Deserialize, MessageTypeInfo)] +#[ros_msg(type_name = "my_msgs/msg/RobotStatus")] struct RobotStatus { battery_level: f32, position_x: f32, @@ -60,19 +60,21 @@ struct RobotStatus { is_moving: bool, } -impl MessageTypeInfo for RobotStatus { - fn type_name() -> &'static str { - "my_msgs::msg::dds_::RobotStatus_" - } - - fn type_hash() -> TypeHash { - TypeHash::zero() // ros-z-to-ros-z only - } +impl ros_z::msg::ZMessage for RobotStatus { + type Serdes = ros_z::msg::SerdeCdrSerdes; } - -impl WithTypeInfo for RobotStatus {} ``` +`MessageTypeInfo` derive in core `ros-z` intentionally supports only ROS 2 schema-compatible named +structs: primitive numeric/bool types, `String`, `Vec`, fixed arrays `[T; N]`, and nested +message types. Tuple structs, unit structs, enums, `Option`, maps, `usize`, `isize`, and other +Rust-only shapes are rejected at compile time. + +For richer serde shapes such as `Option` or enums, use `ros_z::ExtendedMessageTypeInfo`. +It keeps normal `message_schema()` support for types that are still ROS 2 compatible, and uses a +separate `~get_extended_type_description` service for extended-only schemas when the publisher node +is created with `.with_extended_type_description_service()`. + ### Service Example ```rust,ignore @@ -105,6 +107,9 @@ impl ZService for NavigateTo { See the `z_custom_message` example: +When a publisher node enables the type description service, derived custom message types +automatically register their runtime schema so dynamic subscribers can discover them. + ```bash # Terminal 1: Router cargo run --example zenoh_router @@ -272,14 +277,14 @@ ROS_Z_MSG_PATH="./my_robot_msgs" cargo build ## Comparison -| Feature | Rust-Native | Schema-Generated | -|---------|-------------|------------------| -| **Definition** | Rust structs | `.msg`/`.srv` files | -| **Type Hashes** | `TypeHash::zero()` | Proper RIHS01 hashes | -| **Standard Type Refs** | Manual | Automatic (`geometry_msgs`, etc.) | -| **ROS 2 Interop** | No | Partial (messages yes, services limited) | -| **Setup Complexity** | Low | Medium (build.rs required) | -| **Best For** | Prototyping | Production | +| Feature | Rust-Native | Schema-Generated | +| ---------------------- | ------------------ | ---------------------------------------- | +| **Definition** | Rust structs | `.msg`/`.srv` files | +| **Type Hashes** | `TypeHash::zero()` | Proper RIHS01 hashes | +| **Standard Type Refs** | Manual | Automatic (`geometry_msgs`, etc.) | +| **ROS 2 Interop** | No | Partial (messages yes, services limited) | +| **Setup Complexity** | Low | Medium (build.rs required) | +| **Best For** | Prototyping | Production | --- diff --git a/book/src/chapters/keyexpr_formats.md b/book/src/chapters/keyexpr_formats.md index 6ad35746..dabe5441 100644 --- a/book/src/chapters/keyexpr_formats.md +++ b/book/src/chapters/keyexpr_formats.md @@ -62,6 +62,12 @@ robot/sensors/camera/** # Topic /robot/sensors/camera - Using `zenoh-bridge-ros2dds` - Integrating with CycloneDDS or FastDDS nodes via Zenoh +**Discovery consequences:** + +- `Ros2Dds` graph discovery can identify publishers/subscribers/services by topic or service name, but its liveliness data does not provide publisher node identity. +- Topic-based graph helpers such as publisher/subscriber matching work with `Ros2Dds`. +- Node-based discovery and automatic schema discovery via `create_dyn_sub_auto()` are not supported with `Ros2Dds`, because the publishing node cannot be identified from discovery data. + ## Key Expression Behavior (IMPORTANT) Understanding how topic names are converted to key expressions is critical for debugging: diff --git a/crates/rmw-zenoh-rs/src/rmw.rs b/crates/rmw-zenoh-rs/src/rmw.rs index 200e2c6d..e62f6247 100644 --- a/crates/rmw-zenoh-rs/src/rmw.rs +++ b/crates/rmw-zenoh-rs/src/rmw.rs @@ -1271,6 +1271,7 @@ pub extern "C" fn rmw_create_service( let service_impl = crate::service::ServiceImpl { inner: zserver, + pending: std::collections::HashMap::new(), service_name: service_name_cstr, request_ts: service_type_support, response_ts: service_type_support, diff --git a/crates/rmw-zenoh-rs/src/service.rs b/crates/rmw-zenoh-rs/src/service.rs index 10cfa98b..d948d81c 100644 --- a/crates/rmw-zenoh-rs/src/service.rs +++ b/crates/rmw-zenoh-rs/src/service.rs @@ -1,3 +1,4 @@ +use std::collections::HashMap; use std::ffi::CString; use std::sync::Mutex; use std::sync::atomic::{AtomicI64, Ordering}; @@ -89,15 +90,11 @@ impl ClientImpl { tracing::debug!( "[ClientImpl::take_response] Attempting to take response, rx has {} items", - if self.inner.rx().is_empty() { - 0 - } else { - self.inner.rx().len() - } + if self.inner.rmw_has_responses() { 1 } else { 0 } ); // Try to receive a response - if let Ok(sample) = self.inner.rx().try_recv() { + if let Some(sample) = self.inner.rmw_try_take_response_sample()? { tracing::debug!("[ClientImpl::take_response] Got response sample"); let payload = sample.payload(); @@ -172,6 +169,8 @@ impl ClientImpl { /// Service implementation for RMW pub struct ServiceImpl { pub inner: ros_z::service::ZServer, + pub pending: + HashMap>, pub service_name: CString, pub request_ts: crate::type_support::ServiceTypeSupport, pub response_ts: crate::type_support::ServiceTypeSupport, @@ -195,69 +194,23 @@ impl ServiceImpl { *taken = false; } - // Try to receive a request from the raw receiver - if let Some(query) = self.inner.try_queue().and_then(|q| q.try_recv()) { - // Get the payload bytes - let bytes = if let Some(payload) = query.payload() { - payload.to_bytes().to_vec() - } else { - return Ok(()); - }; - - // Extract attachment from query to get GID, sequence number, and timestamp - let key = if let Some(attachment_bytes) = query.attachment() { - match ros_z::attachment::Attachment::try_from(attachment_bytes) { - Ok(attachment) => { - let key: ros_z::service::QueryKey = attachment.into(); - tracing::debug!( - "[ServiceImpl::take_request] Got request with sn: {}, gid: {:?}", - key.sn, - key.gid - ); - key - } - Err(e) => { - tracing::warn!("Failed to extract attachment from query: {}", e); - // Fallback to placeholder - ros_z::service::QueryKey { - gid: [0u8; 16], - sn: 0i64, - } - } - } - } else { - tracing::warn!("No attachment in query, using placeholder QueryKey"); - ros_z::service::QueryKey { - gid: [0u8; 16], - sn: 0i64, - } - }; - - // Extract timestamp from attachment - let source_timestamp = if let Some(attachment_bytes) = query.attachment() { - match ros_z::attachment::Attachment::try_from(attachment_bytes) { - Ok(attachment) => attachment.source_timestamp, - Err(_) => 0, - } - } else { - 0 - }; + if let Some(request_ctx) = self.inner.try_take_request()? { + let request_id = request_ctx.id().clone(); + let source_timestamp = 0; + let (request_msg, reply) = request_ctx.into_parts(); + let bytes = request_msg.0; // Set received_timestamp to current time let received_timestamp = std::time::SystemTime::now() .duration_since(std::time::UNIX_EPOCH) .map_or(0, |v| v.as_nanos() as i64); - // Store the query for later response + // Store the reply context for later response tracing::debug!( - "[ServiceImpl::take_request] Storing query with key sn:{}, inserting into map", - key.sn - ); - self.inner.map_insert(key.clone(), query); - tracing::debug!( - "[ServiceImpl::take_request] Map now has {} entries", - self.inner.map_len() + "[ServiceImpl::take_request] Storing reply context with sn:{}", + request_id.sequence_number ); + self.pending.insert(request_id.clone(), reply); // Deserialize into the provided request buffer using request MessageTypeSupport unsafe { @@ -269,9 +222,8 @@ impl ServiceImpl { // Fill request_header with sequence info and timestamps if !request_header.is_null() { unsafe { - (*request_header).request_id.sequence_number = key.sn; - // Copy GID from key - for (i, &byte) in key.gid.iter().enumerate() { + (*request_header).request_id.sequence_number = request_id.sequence_number; + for (i, &byte) in request_id.writer_guid.iter().enumerate() { if i < 16 { (*request_header).request_id.writer_guid[i] = byte; } @@ -293,42 +245,39 @@ impl ServiceImpl { request_header: *const rmw_request_id_t, response: *const c_void, ) -> Result<()> { - // Extract QueryKey from request_header - let key = unsafe { + let request_id = unsafe { let mut gid = [0u8; 16]; gid.copy_from_slice(&(*request_header).writer_guid); - ros_z::service::QueryKey { - gid, - sn: (*request_header).sequence_number, + ros_z::service::RequestId { + writer_guid: gid, + sequence_number: (*request_header).sequence_number, } }; tracing::debug!( "[ServiceImpl::send_response] Sending response for key sn:{}, gid:{:?}", - key.sn, - key.gid - ); - tracing::debug!( - "[ServiceImpl::send_response] Map has {} entries before send_response", - self.inner.map_len() + request_id.sequence_number, + request_id.writer_guid ); // Create RosMessage Response from the raw pointer using response MessageTypeSupport let resp = crate::msg::RosMessage::new(response, self.response_ts.response); - // Send response - match self.inner.send_response(&resp, &key) { - Ok(_) => { - tracing::debug!("[ServiceImpl::send_response] Response sent successfully"); - Ok(()) - } - Err(e) => { - tracing::error!( - "[ServiceImpl::send_response] Failed to send response: {}", - e - ); - Err(e) - } + match self.pending.remove(&request_id) { + Some(reply) => match reply.reply_blocking(&resp) { + Ok(_) => { + tracing::debug!("[ServiceImpl::send_response] Response sent successfully"); + Ok(()) + } + Err(e) => { + tracing::error!( + "[ServiceImpl::send_response] Failed to send response: {}", + e + ); + Err(e) + } + }, + None => Err(zenoh::Error::from("Pending request not found")), } } } @@ -337,7 +286,7 @@ impl Waitable for ClientImpl { fn is_ready(&self) -> bool { // Acquire fence to ensure we see the latest channel state from other threads std::sync::atomic::fence(std::sync::atomic::Ordering::Acquire); - !self.inner.rx().is_empty() + self.inner.rmw_has_responses() } } diff --git a/crates/ros-z-bridge/src/bridge.rs b/crates/ros-z-bridge/src/bridge.rs index dc4b7515..084e8f05 100644 --- a/crates/ros-z-bridge/src/bridge.rs +++ b/crates/ros-z-bridge/src/bridge.rs @@ -206,7 +206,7 @@ impl Bridge { let key = BridgeKey { topic: ep.topic.clone(), type_name: type_info.name.clone(), - kind: ep.kind, + kind: ep.kind.into(), }; if event.appeared { diff --git a/crates/ros-z-bridge/src/discovery.rs b/crates/ros-z-bridge/src/discovery.rs index 55e3c285..ff1e1e1c 100644 --- a/crates/ros-z-bridge/src/discovery.rs +++ b/crates/ros-z-bridge/src/discovery.rs @@ -135,22 +135,22 @@ pub fn start_discovery( mod tests { use super::*; use ros_z_protocol::{ - EntityKind, TypeHash, TypeInfo, + EndpointKind, TypeHash, TypeInfo, entity::{EndpointEntity, NodeEntity}, }; fn make_endpoint(hash: TypeHash) -> Entity { Entity::Endpoint(EndpointEntity { id: 1, - node: NodeEntity { + node: Some(NodeEntity { domain_id: 0, z_id: "1234567890abcdef1234567890abcdef".parse().unwrap(), id: 0, name: "test_node".to_string(), namespace: "/".to_string(), enclave: String::new(), - }, - kind: EntityKind::Publisher, + }), + kind: EndpointKind::Publisher, topic: "/chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs::msg::dds_::String_", hash)), qos: Default::default(), @@ -187,15 +187,15 @@ mod tests { fn classify_endpoint_no_type_info_returns_none() { let entity = Entity::Endpoint(EndpointEntity { id: 2, - node: NodeEntity { + node: Some(NodeEntity { domain_id: 0, z_id: "1234567890abcdef1234567890abcdef".parse().unwrap(), id: 0, name: "no_type_node".to_string(), namespace: "/".to_string(), enclave: String::new(), - }, - kind: EntityKind::Publisher, + }), + kind: EndpointKind::Publisher, topic: "/anon".to_string(), type_info: None, qos: Default::default(), diff --git a/crates/ros-z-bridge/src/forwarder.rs b/crates/ros-z-bridge/src/forwarder.rs index ec10ef46..92afb44d 100644 --- a/crates/ros-z-bridge/src/forwarder.rs +++ b/crates/ros-z-bridge/src/forwarder.rs @@ -97,7 +97,7 @@ pub fn start_service_forwarder( let target_ke = target_ke_arc.clone(); let proxy_ke_log = proxy_ke_log.clone(); // Extract payload and attachment from the query. - // The attachment carries the QueryKey (sequence number, writer GUID) + // The attachment carries the request identity (sequence number, writer GUID) // required by ros-z service.rs — it must be forwarded verbatim. let payload: Option = query.payload().cloned(); let attachment: Option = query.attachment().cloned(); diff --git a/crates/ros-z-console/src/app/render/services.rs b/crates/ros-z-console/src/app/render/services.rs index 3423d38e..b44b305e 100644 --- a/crates/ros-z-console/src/app/render/services.rs +++ b/crates/ros-z-console/src/app/render/services.rs @@ -100,10 +100,15 @@ impl App { for (idx, entity) in server_entities.iter().enumerate() { if let Some(endpoint) = entity_get_endpoint(entity) { detail.push_str(&format!(" Server {}:\n", idx + 1)); - detail.push_str(&format!( - " Node: {}/{}\n", - endpoint.node.namespace, endpoint.node.name - )); + match endpoint.node.as_ref() { + Some(node) => { + detail.push_str(&format!( + " Node: {}/{}\n", + node.namespace, node.name + )); + } + None => detail.push_str(" Node: unknown\n"), + } detail.push_str(&format_qos_detail(&endpoint.qos)); detail.push_str("\n\n"); } @@ -131,10 +136,15 @@ impl App { for (idx, entity) in client_entities.iter().enumerate() { if let Some(endpoint) = entity_get_endpoint(entity) { detail.push_str(&format!(" Client {}:\n", idx + 1)); - detail.push_str(&format!( - " Node: {}/{}\n", - endpoint.node.namespace, endpoint.node.name - )); + match endpoint.node.as_ref() { + Some(node) => { + detail.push_str(&format!( + " Node: {}/{}\n", + node.namespace, node.name + )); + } + None => detail.push_str(" Node: unknown\n"), + } detail.push_str(&format_qos_detail(&endpoint.qos)); detail.push_str("\n\n"); } diff --git a/crates/ros-z-console/src/app/render/topics.rs b/crates/ros-z-console/src/app/render/topics.rs index 898e4245..b7c9cf8d 100644 --- a/crates/ros-z-console/src/app/render/topics.rs +++ b/crates/ros-z-console/src/app/render/topics.rs @@ -145,10 +145,15 @@ impl App { for (idx, entity) in pub_entities.iter().enumerate() { if let Some(endpoint) = entity_get_endpoint(entity) { detail.push_str(&format!(" Publisher {}:\n", idx + 1)); - detail.push_str(&format!( - " Node: {}/{}\n", - endpoint.node.namespace, endpoint.node.name - )); + match endpoint.node.as_ref() { + Some(node) => { + detail.push_str(&format!( + " Node: {}/{}\n", + node.namespace, node.name + )); + } + None => detail.push_str(" Node: unknown\n"), + } if let Some(ti) = &endpoint.type_info { let hash_str = ti.hash.to_rihs_string(); @@ -187,10 +192,15 @@ impl App { for (idx, entity) in sub_entities.iter().enumerate() { if let Some(endpoint) = entity_get_endpoint(entity) { detail.push_str(&format!(" Subscriber {}:\n", idx + 1)); - detail.push_str(&format!( - " Node: {}/{}\n", - endpoint.node.namespace, endpoint.node.name - )); + match endpoint.node.as_ref() { + Some(node) => { + detail.push_str(&format!( + " Node: {}/{}\n", + node.namespace, node.name + )); + } + None => detail.push_str(" Node: unknown\n"), + } if let Some(ti) = &endpoint.type_info { let hash_str = ti.hash.to_rihs_string(); diff --git a/crates/ros-z-console/src/core/dynamic_subscriber.rs b/crates/ros-z-console/src/core/dynamic_subscriber.rs deleted file mode 100644 index 4d5a55e9..00000000 --- a/crates/ros-z-console/src/core/dynamic_subscriber.rs +++ /dev/null @@ -1,141 +0,0 @@ -//! Dynamic topic subscriber for any ROS message type -//! -//! Provides automatic schema discovery and message reception for topics -//! without compile-time knowledge of message types. - -use std::{sync::Arc, time::Duration}; - -use flume::Receiver; -use ros_z::{ - dynamic::{DynamicMessage, DynamicSerdeCdrSerdes, MessageSchema}, - node::ZNode, - pubsub::ZSub, -}; -use zenoh::sample::Sample; - -/// Manages subscription to a single topic with dynamic message types -pub struct DynamicTopicSubscriber { - /// Topic name being subscribed to - #[allow(dead_code)] - pub topic: String, - /// Discovered message schema - schema: Arc, - /// RIHS01 type hash for compatibility verification - type_hash: String, - /// Channel for receiving messages asynchronously - message_rx: Receiver, - /// Subscriber handle (kept alive to maintain subscription) - _subscriber: Arc>, -} - -impl DynamicTopicSubscriber { - /// Create a new dynamic subscriber with automatic schema discovery - /// - /// # Arguments - /// - /// * `node` - ROS node with type description service enabled - /// * `topic` - Topic name to subscribe to (e.g., "/chatter") - /// * `discovery_timeout` - Maximum time to wait for schema discovery - /// - /// # Errors - /// - /// Returns error if: - /// - No publishers found on topic within timeout - /// - Schema discovery fails - /// - Subscriber creation fails - pub async fn new( - node: &ZNode, - topic: &str, - discovery_timeout: Duration, - ) -> Result> { - // Use the node's auto-discovery method to get subscriber and schema - let (subscriber, schema) = node.create_dyn_sub_auto(topic, discovery_timeout).await?; - - // Compute type hash for informational purposes - use ros_z::dynamic::MessageSchemaTypeDescription; - let type_hash = schema - .compute_type_hash() - .map(|h| h.to_rihs_string()) - .unwrap_or_else(|_| "unknown".to_string()); - - // Create channel for async message handling - let (tx, rx) = flume::unbounded(); - - // Spawn background task to forward messages to channel - let subscriber = Arc::new(subscriber); - let sub_clone = subscriber.clone(); - tokio::task::spawn_blocking(move || { - loop { - match sub_clone.recv() { - Ok(msg) => { - if tx.send(msg).is_err() { - // Receiver dropped, exit task - break; - } - } - Err(e) => { - tracing::warn!("Subscriber recv error: {}", e); - break; - } - } - } - }); - - Ok(Self { - topic: topic.to_string(), - schema, - type_hash, - message_rx: rx, - _subscriber: subscriber, - }) - } - - /// Get the discovered message schema - pub fn schema(&self) -> &MessageSchema { - &self.schema - } - - /// Get the RIHS01 type hash - pub fn type_hash(&self) -> &str { - &self.type_hash - } - - /// Receive the next message (blocking) - /// - /// Blocks until a message is available or the channel is closed. - #[allow(dead_code)] - pub fn recv(&self) -> Result { - self.message_rx.recv() - } - - /// Try to receive a message without blocking - /// - /// Returns `Ok(Some(msg))` if a message is available, - /// `Ok(None)` if no message is ready, - /// or `Err` if the channel is closed. - pub fn try_recv(&self) -> Result, flume::TryRecvError> { - match self.message_rx.try_recv() { - Ok(msg) => Ok(Some(msg)), - Err(flume::TryRecvError::Empty) => Ok(None), - Err(e) => Err(e), - } - } - - /// Check if there are any messages waiting - #[allow(dead_code)] - pub fn has_messages(&self) -> bool { - !self.message_rx.is_empty() - } - - /// Get the number of messages currently buffered - #[allow(dead_code)] - pub fn message_count(&self) -> usize { - self.message_rx.len() - } -} - -#[cfg(test)] -mod tests { - // Note: Integration tests would require a running ROS system - // Unit tests are limited for this component -} diff --git a/crates/ros-z-console/src/core/engine.rs b/crates/ros-z-console/src/core/engine.rs index af8977a5..be40d4c8 100644 --- a/crates/ros-z-console/src/core/engine.rs +++ b/crates/ros-z-console/src/core/engine.rs @@ -8,12 +8,10 @@ use std::{ }; use parking_lot::Mutex; -use ros_z::{Builder, context::ZContext, graph::Graph, node::ZNode}; +use ros_z::{Builder, context::ZContext, dynamic::DynSubBuilder, graph::Graph, node::ZNode}; use tokio::sync::broadcast; -use super::{ - dynamic_subscriber::DynamicTopicSubscriber, events::SystemEvent, metrics::MetricsCollector, -}; +use super::{events::SystemEvent, metrics::MetricsCollector}; #[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] pub enum Backend { @@ -133,12 +131,14 @@ impl CoreEngine { /// # Errors /// /// Returns error if schema discovery fails or subscriber creation fails - pub async fn create_dynamic_subscriber( + pub async fn create_dynamic_subscriber_builder( &self, topic: &str, discovery_timeout: Duration, - ) -> Result> { - DynamicTopicSubscriber::new(&self.node, topic, discovery_timeout).await + ) -> Result> { + self.node + .create_dyn_sub_auto(topic, discovery_timeout) + .await } pub async fn start_monitoring(&self) { diff --git a/crates/ros-z-console/src/core/message_formatter.rs b/crates/ros-z-console/src/core/message_formatter.rs index 896636bc..6a83a3da 100644 --- a/crates/ros-z-console/src/core/message_formatter.rs +++ b/crates/ros-z-console/src/core/message_formatter.rs @@ -2,7 +2,9 @@ //! //! Provides JSON and human-readable text formatting for dynamic messages. -use ros_z::dynamic::{DynamicMessage, DynamicValue}; +use ros_z::dynamic::{ + DynamicMessage, DynamicNamedValue, DynamicValue, EnumPayloadValue, EnumValue, +}; use serde_json; /// Convert a DynamicMessage to a JSON value @@ -47,12 +49,45 @@ pub fn dynamic_value_to_json(value: &DynamicValue) -> serde_json::Value { ) } DynamicValue::Message(msg) => dynamic_message_to_json(msg), + DynamicValue::Optional(None) => serde_json::Value::Null, + DynamicValue::Optional(Some(value)) => dynamic_value_to_json(value), + DynamicValue::Enum(value) => enum_value_to_json(value), DynamicValue::Array(arr) => { serde_json::Value::Array(arr.iter().map(dynamic_value_to_json).collect()) } } } +fn enum_value_to_json(value: &EnumValue) -> serde_json::Value { + let mut fields = serde_json::Map::new(); + fields.insert( + "variant_index".to_string(), + serde_json::Value::Number(value.variant_index.into()), + ); + fields.insert( + "variant_name".to_string(), + serde_json::Value::String(value.variant_name.clone()), + ); + fields.insert("payload".to_string(), enum_payload_to_json(&value.payload)); + serde_json::Value::Object(fields) +} + +fn enum_payload_to_json(payload: &EnumPayloadValue) -> serde_json::Value { + match payload { + EnumPayloadValue::Unit => serde_json::Value::Null, + EnumPayloadValue::Newtype(value) => dynamic_value_to_json(value), + EnumPayloadValue::Tuple(values) => { + serde_json::Value::Array(values.iter().map(dynamic_value_to_json).collect()) + } + EnumPayloadValue::Struct(fields) => serde_json::Value::Object( + fields + .iter() + .map(|field| (field.name.clone(), dynamic_value_to_json(&field.value))) + .collect(), + ), + } +} + /// Format a DynamicMessage as human-readable indented text /// /// Produces output like: @@ -121,6 +156,11 @@ fn format_value_pretty(output: &mut String, name: &str, value: &DynamicValue, in format_value_pretty(output, n, v, indent + 1); } } + DynamicValue::Optional(None) => { + output.push_str(&format!("{}{}: null\n", prefix, name)); + } + DynamicValue::Optional(Some(value)) => format_value_pretty(output, name, value, indent), + DynamicValue::Enum(value) => format_enum_value_pretty(output, name, value, indent), DynamicValue::Array(arr) => { if arr.is_empty() { output.push_str(&format!("{}{}[]: []\n", prefix, name)); @@ -134,10 +174,59 @@ fn format_value_pretty(output: &mut String, name: &str, value: &DynamicValue, in } } +fn format_enum_value_pretty(output: &mut String, name: &str, value: &EnumValue, indent: usize) { + let prefix = " ".repeat(indent); + output.push_str(&format!("{}{}:\n", prefix, name)); + output.push_str(&format!("{} variant: {}\n", prefix, value.variant_name)); + format_enum_payload_pretty(output, &value.payload, indent + 1); +} + +fn format_enum_payload_pretty(output: &mut String, payload: &EnumPayloadValue, indent: usize) { + let prefix = " ".repeat(indent); + + match payload { + EnumPayloadValue::Unit => { + output.push_str(&format!("{}payload: null\n", prefix)); + } + EnumPayloadValue::Newtype(value) => { + format_value_pretty(output, "payload", value, indent); + } + EnumPayloadValue::Tuple(values) => { + if values.is_empty() { + output.push_str(&format!("{}payload[]: []\n", prefix)); + } else { + output.push_str(&format!("{}payload[{}]:\n", prefix, values.len())); + for (index, value) in values.iter().enumerate() { + format_value_pretty(output, &format!("[{}]", index), value, indent + 1); + } + } + } + EnumPayloadValue::Struct(fields) => { + if fields.is_empty() { + output.push_str(&format!("{}payload: {{}}\n", prefix)); + } else { + output.push_str(&format!("{}payload:\n", prefix)); + format_named_fields_pretty(output, fields, indent + 1); + } + } + } +} + +fn format_named_fields_pretty(output: &mut String, fields: &[DynamicNamedValue], indent: usize) { + for field in fields { + format_value_pretty(output, &field.name, &field.value, indent); + } +} + #[cfg(test)] mod tests { + use std::sync::Arc; + use super::*; - use ros_z::dynamic::{FieldType, MessageSchema}; + use ros_z::dynamic::{ + EnumPayloadValue, EnumSchema, EnumValue, EnumVariantSchema, FieldSchema, FieldType, + MessageSchema, + }; #[test] fn test_json_primitives() { @@ -204,4 +293,67 @@ mod tests { assert!(formatted.contains("linear:")); assert!(formatted.contains(" x: 1")); } + + #[test] + fn test_json_optional_and_enum_values() { + let optional = DynamicValue::Optional(Some(Box::new(DynamicValue::Int32(7)))); + assert_eq!(dynamic_value_to_json(&optional), serde_json::json!(7)); + + let enum_value = DynamicValue::Enum(EnumValue::new( + 1, + "Error", + EnumPayloadValue::Struct(vec![crate::dynamic::DynamicNamedValue { + name: "code".to_string(), + value: DynamicValue::Uint32(42), + }]), + )); + let json = dynamic_value_to_json(&enum_value); + assert_eq!(json["variant_index"], 1); + assert_eq!(json["variant_name"], "Error"); + assert_eq!(json["payload"]["code"], 42); + } + + #[test] + fn test_pretty_format_optional_and_enum_fields() { + let status_schema = Arc::new(EnumSchema::new( + "test_msgs/msg/Status", + vec![ + EnumVariantSchema::new("Idle", crate::dynamic::EnumPayloadSchema::Unit), + EnumVariantSchema::new( + "Error", + crate::dynamic::EnumPayloadSchema::Struct(vec![FieldSchema::new( + "code", + FieldType::Uint32, + )]), + ), + ], + )); + + let schema = MessageSchema::builder("test_msgs/msg/State") + .field("nickname", FieldType::Optional(Box::new(FieldType::String))) + .field("status", FieldType::Enum(status_schema)) + .build() + .unwrap(); + + let mut msg = DynamicMessage::new(&schema); + msg.set("nickname", Some("ros-z".to_string())).unwrap(); + msg.set( + "status", + EnumValue::new( + 1, + "Error", + EnumPayloadValue::Struct(vec![crate::dynamic::DynamicNamedValue { + name: "code".to_string(), + value: DynamicValue::Uint32(5), + }]), + ), + ) + .unwrap(); + + let formatted = format_message_pretty(&msg); + assert!(formatted.contains("nickname: \"ros-z\"")); + assert!(formatted.contains("status:")); + assert!(formatted.contains("variant: Error")); + assert!(formatted.contains("code: 5")); + } } diff --git a/crates/ros-z-console/src/core/mod.rs b/crates/ros-z-console/src/core/mod.rs index 585ee335..68f1d92e 100644 --- a/crates/ros-z-console/src/core/mod.rs +++ b/crates/ros-z-console/src/core/mod.rs @@ -1,4 +1,3 @@ -pub mod dynamic_subscriber; pub mod engine; pub mod events; pub mod logger; diff --git a/crates/ros-z-console/src/export.rs b/crates/ros-z-console/src/export.rs index 43259675..a2867ad6 100644 --- a/crates/ros-z-console/src/export.rs +++ b/crates/ros-z-console/src/export.rs @@ -69,8 +69,10 @@ async fn export_dot( // Publishers -> Topic for entity in graph.get_entities_by_topic(EntityKind::Publisher, &topic) { - if let Some(endpoint) = entity_get_endpoint(&entity) { - let node_id = format!("{}/{}", endpoint.node.namespace, endpoint.node.name); + if let Some(endpoint) = entity_get_endpoint(&entity) + && let Some(node) = endpoint.node.as_ref() + { + let node_id = format!("{}/{}", node.namespace, node.name); dot.push_str(&format!( " \"{}\" -> \"topic:{}\" [color=blue];\n", node_id, topic @@ -80,8 +82,10 @@ async fn export_dot( // Topic -> Subscribers for entity in graph.get_entities_by_topic(EntityKind::Subscription, &topic) { - if let Some(endpoint) = entity_get_endpoint(&entity) { - let node_id = format!("{}/{}", endpoint.node.namespace, endpoint.node.name); + if let Some(endpoint) = entity_get_endpoint(&entity) + && let Some(node) = endpoint.node.as_ref() + { + let node_id = format!("{}/{}", node.namespace, node.name); dot.push_str(&format!( " \"topic:{}\" -> \"{}\" [color=green];\n", topic, node_id diff --git a/crates/ros-z-console/src/modes/headless.rs b/crates/ros-z-console/src/modes/headless.rs index 7e3377f1..567b198c 100644 --- a/crates/ros-z-console/src/modes/headless.rs +++ b/crates/ros-z-console/src/modes/headless.rs @@ -1,9 +1,12 @@ use chrono::Utc; +use ros_z::{ + Builder, + dynamic::{DynSub, MessageSchemaTypeDescription}, +}; use serde::Serialize; use std::{collections::HashMap, time::Duration}; use crate::core::{ - dynamic_subscriber::DynamicTopicSubscriber, engine::CoreEngine, message_formatter::{dynamic_message_to_json, format_message_pretty}, }; @@ -40,7 +43,7 @@ pub async fn run_headless_mode( } // Create dynamic subscribers for echo topics - let mut subscribers: HashMap = HashMap::new(); + let mut subscribers: HashMap = HashMap::new(); if !echo_topics.is_empty() { tracing::info!( @@ -50,28 +53,43 @@ pub async fn run_headless_mode( for topic in echo_topics { match core - .create_dynamic_subscriber(&topic, Duration::from_secs(5)) + .create_dynamic_subscriber_builder(&topic, Duration::from_secs(5)) .await { Ok(sub) => { + let sub = match sub.build() { + Ok(sub) => sub, + Err(e) => { + eprintln!("Failed to build subscriber for {}: {}", topic, e); + continue; + } + }; + let Some(schema) = sub.schema() else { + eprintln!( + "Failed to inspect schema for {}: missing dynamic schema", + topic + ); + continue; + }; + let type_hash = schema + .compute_type_hash() + .map(|hash| hash.to_rihs_string()) + .unwrap_or_else(|_| "unknown".to_string()); if json { // Output schema info let schema_info = serde_json::json!({ "event": "topic_subscribed", "topic": topic, - "type_name": sub.schema().type_name, - "type_hash": sub.type_hash(), - "fields": sub.schema().field_names().collect::>(), + "type_name": schema.type_name, + "type_hash": type_hash, + "fields": schema.field_names().collect::>(), }); println!("{}", serde_json::to_string(&schema_info)?); } else { println!("\n=== Subscribed to {} ===", topic); - println!("Type: {}", sub.schema().type_name); - println!("Hash: {}", sub.type_hash()); - println!( - "Fields: {:?}", - sub.schema().field_names().collect::>() - ); + println!("Type: {}", schema.type_name); + println!("Hash: {}", type_hash); + println!("Fields: {:?}", schema.field_names().collect::>()); println!(); } subscribers.insert(topic.clone(), sub); @@ -117,20 +135,27 @@ pub async fn run_headless_mode( // Handle echo messages _ = async { for (topic, subscriber) in &subscribers { - if let Ok(Some(msg)) = subscriber.try_recv() { - if json { - let msg_json = serde_json::json!({ - "event": "message_received", - "topic": topic, - "type": msg.schema().type_name, - "data": dynamic_message_to_json(&msg), - }); - if let Ok(json_str) = serde_json::to_string(&msg_json) { - println!("{}", json_str); + if let Some(result) = subscriber.try_recv() { + match result { + Ok(msg) => { + if json { + let msg_json = serde_json::json!({ + "event": "message_received", + "topic": topic, + "type": msg.schema().type_name, + "data": dynamic_message_to_json(&msg), + }); + if let Ok(json_str) = serde_json::to_string(&msg_json) { + println!("{}", json_str); + } + } else { + println!("\n=== {} ===", topic); + print!("{}", format_message_pretty(&msg)); + } + } + Err(error) => { + eprintln!("Failed to receive from {}: {}", topic, error); } - } else { - println!("\n=== {} ===", topic); - print!("{}", format_message_pretty(&msg)); } } } diff --git a/crates/ros-z-derive/src/lib.rs b/crates/ros-z-derive/src/lib.rs index 667ab782..ac6cd3bf 100644 --- a/crates/ros-z-derive/src/lib.rs +++ b/crates/ros-z-derive/src/lib.rs @@ -1,18 +1,51 @@ -//! Derive macros for Python message bridge traits +//! Derive macros for ros-z traits. //! -//! Provides `FromPyMessage` and `IntoPyMessage` derive macros for automatic -//! conversion between Python msgspec structs and Rust ROS message types. +//! Provides: +//! - `MessageTypeInfo` for Rust-native message schema generation +//! - `FromPyMessage` and `IntoPyMessage` for Python bridge conversion #![allow(clippy::collapsible_if)] use proc_macro::TokenStream; use quote::quote; use syn::{ - Attribute, Data, DeriveInput, Fields, GenericArgument, Ident, Lit, Meta, PathArguments, Type, - parse_macro_input, + Attribute, Data, DeriveInput, Expr, Fields, GenericArgument, Ident, LitStr, PathArguments, + Type, parse_macro_input, }; -/// Derive macro for extracting Rust messages from Python objects +type TokenStream2 = proc_macro2::TokenStream; + +/// Derive macro for implementing ros-z message metadata and dynamic schema generation. +/// +/// # Example +/// ```ignore +/// #[derive(MessageTypeInfo)] +/// #[ros_msg(type_name = "custom_msgs/msg/RobotStatus")] +/// pub struct RobotStatus { +/// pub battery_percentage: f64, +/// pub is_moving: bool, +/// } +/// ``` +#[proc_macro_derive(MessageTypeInfo, attributes(ros_msg))] +pub fn derive_message_type_info(input: TokenStream) -> TokenStream { + let input = parse_macro_input!(input as DeriveInput); + match impl_standard_message_type_info(&input) { + Ok(tokens) => tokens.into(), + Err(err) => err.to_compile_error().into(), + } +} + +/// Derive macro for implementing ros-z extended message schema generation. +#[proc_macro_derive(ExtendedMessageTypeInfo, attributes(ros_msg))] +pub fn derive_extended_message_type_info(input: TokenStream) -> TokenStream { + let input = parse_macro_input!(input as DeriveInput); + match impl_message_type_info(&input) { + Ok(tokens) => tokens.into(), + Err(err) => err.to_compile_error().into(), + } +} + +/// Derive macro for extracting Rust messages from Python objects. /// /// # Example /// ```ignore @@ -31,7 +64,7 @@ pub fn derive_from_py_message(input: TokenStream) -> TokenStream { } } -/// Derive macro for constructing Python objects from Rust messages +/// Derive macro for constructing Python objects from Rust messages. /// /// # Example /// ```ignore @@ -50,7 +83,365 @@ pub fn derive_into_py_message(input: TokenStream) -> TokenStream { } } -fn impl_from_py_message(input: &DeriveInput) -> syn::Result { +fn impl_message_type_info(input: &DeriveInput) -> syn::Result { + let name = &input.ident; + + if !input.generics.params.is_empty() { + return Err(syn::Error::new_spanned( + &input.generics, + "ExtendedMessageTypeInfo derive does not support generic types in v1", + )); + } + + let attrs = parse_ros_msg_args(&input.attrs)?; + let canonical_type_name = attrs.type_name.ok_or_else(|| { + syn::Error::new_spanned( + input, + "ExtendedMessageTypeInfo derive requires #[ros_msg(type_name = \"my_pkg/msg/MyType\")]", + ) + })?; + let type_name_lit = LitStr::new(&canonical_type_name, proc_macro2::Span::call_site()); + let (package, _kind, message_name) = parse_canonical_type_name(&canonical_type_name)?; + let dds_type_name = canonical_to_dds_name(&canonical_type_name)?; + let package_lit = LitStr::new(&package, proc_macro2::Span::call_site()); + let message_name_lit = LitStr::new(&message_name, proc_macro2::Span::call_site()); + let dds_type_name_lit = LitStr::new(&dds_type_name, proc_macro2::Span::call_site()); + + match &input.data { + Data::Struct(data) => impl_message_type_info_for_struct( + name, + data, + &type_name_lit, + &package_lit, + &message_name_lit, + &dds_type_name_lit, + ), + Data::Enum(data) => impl_message_type_info_for_enum( + name, + data, + &type_name_lit, + &package_lit, + &message_name_lit, + &dds_type_name_lit, + ), + Data::Union(_) => Err(syn::Error::new_spanned( + input, + "ExtendedMessageTypeInfo derive does not support unions", + )), + } +} + +fn impl_standard_message_type_info(input: &DeriveInput) -> syn::Result { + let name = &input.ident; + + if !input.generics.params.is_empty() { + return Err(syn::Error::new_spanned( + &input.generics, + "MessageTypeInfo derive does not support generic types in v1", + )); + } + + let attrs = parse_ros_msg_args(&input.attrs)?; + let canonical_type_name = attrs.type_name.ok_or_else(|| { + syn::Error::new_spanned( + input, + "MessageTypeInfo derive requires #[ros_msg(type_name = \"my_pkg/msg/MyType\")]", + ) + })?; + let type_name_lit = LitStr::new(&canonical_type_name, proc_macro2::Span::call_site()); + let (package, _kind, message_name) = parse_canonical_type_name(&canonical_type_name)?; + let dds_type_name = canonical_to_dds_name(&canonical_type_name)?; + let package_lit = LitStr::new(&package, proc_macro2::Span::call_site()); + let message_name_lit = LitStr::new(&message_name, proc_macro2::Span::call_site()); + let dds_type_name_lit = LitStr::new(&dds_type_name, proc_macro2::Span::call_site()); + + let Data::Struct(data) = &input.data else { + return Err(syn::Error::new_spanned( + input, + "MessageTypeInfo derive only supports named structs in v1", + )); + }; + + let Fields::Named(fields) = &data.fields else { + let message = match &data.fields { + Fields::Unnamed(_) => "MessageTypeInfo derive does not support tuple structs in v1", + Fields::Unit => "MessageTypeInfo derive does not support unit structs in v1", + Fields::Named(_) => unreachable!(), + }; + return Err(syn::Error::new_spanned(input, message)); + }; + + let schema_fields = fields + .named + .iter() + .map(generate_standard_message_field_schema_tokens) + .collect::>>()?; + + let message_type_hash_impl = standard_message_type_hash_impl_tokens(); + + Ok(quote! { + impl ::ros_z::MessageTypeInfo for #name { + fn type_name() -> &'static str { + #dds_type_name_lit + } + + #message_type_hash_impl + + fn message_schema() -> Option<::std::sync::Arc<::ros_z::dynamic::MessageSchema>> { + static SCHEMA: ::std::sync::OnceLock<::std::sync::Arc<::ros_z::dynamic::MessageSchema>> = + ::std::sync::OnceLock::new(); + + Some( + SCHEMA + .get_or_init(|| { + ::std::sync::Arc::new(::ros_z::dynamic::MessageSchema { + type_name: #type_name_lit.to_string(), + package: #package_lit.to_string(), + name: #message_name_lit.to_string(), + fields: ::std::vec![#(#schema_fields),*], + type_hash: None, + }) + }) + .clone(), + ) + } + } + + impl ::ros_z::WithTypeInfo for #name {} + }) +} + +fn impl_message_type_info_for_struct( + name: &Ident, + data: &syn::DataStruct, + type_name_lit: &LitStr, + package_lit: &LitStr, + message_name_lit: &LitStr, + dds_type_name_lit: &LitStr, +) -> syn::Result { + let message_type_hash_impl = extended_message_type_hash_impl_tokens(); + + let Fields::Named(fields) = &data.fields else { + let message = match &data.fields { + Fields::Unnamed(_) => { + "ExtendedMessageTypeInfo derive does not support tuple structs in v1" + } + Fields::Unit => "ExtendedMessageTypeInfo derive does not support unit structs in v1", + Fields::Named(_) => unreachable!(), + }; + return Err(syn::Error::new_spanned(name, message)); + }; + + let schema_fields = fields + .named + .iter() + .map(generate_message_field_schema_tokens) + .collect::>>()?; + + Ok(quote! { + impl ::ros_z::ExtendedMessageTypeInfo for #name { + fn extended_message_schema() -> ::std::sync::Arc<::ros_z::dynamic::MessageSchema> { + static SCHEMA: ::std::sync::OnceLock<::std::sync::Arc<::ros_z::dynamic::MessageSchema>> = + ::std::sync::OnceLock::new(); + + SCHEMA + .get_or_init(|| { + ::std::sync::Arc::new(::ros_z::dynamic::MessageSchema { + type_name: #type_name_lit.to_string(), + package: #package_lit.to_string(), + name: #message_name_lit.to_string(), + fields: ::std::vec![#(#schema_fields),*], + type_hash: None, + }) + }) + .clone() + } + } + + impl ::ros_z::MessageTypeInfo for #name { + fn type_name() -> &'static str { + #dds_type_name_lit + } + + #message_type_hash_impl + + fn message_schema() -> Option<::std::sync::Arc<::ros_z::dynamic::MessageSchema>> { + let schema = ::extended_message_schema(); + if schema.uses_extended_types() { + None + } else { + Some(schema) + } + } + + fn register_type_extensions(node: &::ros_z::node::ZNode) -> ::std::result::Result<(), ::std::string::String> { + let schema = ::extended_message_schema(); + if schema.uses_extended_types() { + ::ros_z::extended_schema::register_type::(node) + } else { + Ok(()) + } + } + } + + impl ::ros_z::WithTypeInfo for #name {} + }) +} + +fn impl_message_type_info_for_enum( + name: &Ident, + data: &syn::DataEnum, + type_name_lit: &LitStr, + package_lit: &LitStr, + message_name_lit: &LitStr, + dds_type_name_lit: &LitStr, +) -> syn::Result { + let message_type_hash_impl = extended_message_type_hash_impl_tokens(); + + if data.variants.is_empty() { + return Err(syn::Error::new_spanned( + name, + "ExtendedMessageTypeInfo derive requires enums to have at least one variant", + )); + } + + let variant_tokens = data + .variants + .iter() + .map(generate_enum_variant_schema_tokens) + .collect::>>()?; + + Ok(quote! { + impl #name { + fn __ros_z_enum_schema() -> ::std::sync::Arc<::ros_z::dynamic::EnumSchema> { + static ENUM_SCHEMA: ::std::sync::OnceLock<::std::sync::Arc<::ros_z::dynamic::EnumSchema>> = + ::std::sync::OnceLock::new(); + + ENUM_SCHEMA + .get_or_init(|| { + ::std::sync::Arc::new(::ros_z::dynamic::EnumSchema { + type_name: #type_name_lit.to_string(), + variants: ::std::vec![#(#variant_tokens),*], + }) + }) + .clone() + } + } + + impl ::ros_z::ExtendedMessageTypeInfo for #name { + fn extended_message_schema() -> ::std::sync::Arc<::ros_z::dynamic::MessageSchema> { + static SCHEMA: ::std::sync::OnceLock<::std::sync::Arc<::ros_z::dynamic::MessageSchema>> = + ::std::sync::OnceLock::new(); + + SCHEMA + .get_or_init(|| { + ::std::sync::Arc::new(::ros_z::dynamic::MessageSchema { + type_name: #type_name_lit.to_string(), + package: #package_lit.to_string(), + name: #message_name_lit.to_string(), + fields: ::std::vec![ + ::ros_z::dynamic::FieldSchema::new( + "value", + ::ros_z::dynamic::FieldType::Enum(Self::__ros_z_enum_schema()), + ) + ], + type_hash: None, + }) + }) + .clone() + } + + fn extended_field_type() -> ::ros_z::dynamic::FieldType { + ::ros_z::dynamic::FieldType::Enum(Self::__ros_z_enum_schema()) + } + } + + impl ::ros_z::MessageTypeInfo for #name { + fn type_name() -> &'static str { + #dds_type_name_lit + } + + #message_type_hash_impl + + fn message_schema() -> Option<::std::sync::Arc<::ros_z::dynamic::MessageSchema>> { + None + } + + fn register_type_extensions(node: &::ros_z::node::ZNode) -> ::std::result::Result<(), ::std::string::String> { + ::ros_z::extended_schema::register_type::(node) + } + } + + impl ::ros_z::WithTypeInfo for #name {} + }) +} + +fn standard_message_type_hash_impl_tokens() -> TokenStream2 { + quote! { + fn type_hash() -> ::ros_z::entity::TypeHash { + let zero = ::ros_z::entity::TypeHash::zero(); + if zero.to_rihs_string() == "TypeHashNotSupported" { + return zero; + } + + static TYPE_HASH: ::std::sync::OnceLock<::ros_z::entity::TypeHash> = + ::std::sync::OnceLock::new(); + + TYPE_HASH + .get_or_init(|| { + use ::ros_z::dynamic::MessageSchemaTypeDescription; + + let schema = Self::message_schema() + .expect("derived message schema must be available"); + let rihs = schema + .compute_type_hash() + .expect("derived message schema must produce a type hash") + .to_rihs_string(); + + ::ros_z::entity::TypeHash::from_rihs_string(&rihs) + .expect("derived message hash must be a valid RIHS01 string") + }) + .clone() + } + } +} + +fn extended_message_type_hash_impl_tokens() -> TokenStream2 { + quote! { + fn type_hash() -> ::ros_z::entity::TypeHash { + let zero = ::ros_z::entity::TypeHash::zero(); + if zero.to_rihs_string() == "TypeHashNotSupported" { + return zero; + } + + static TYPE_HASH: ::std::sync::OnceLock<::ros_z::entity::TypeHash> = + ::std::sync::OnceLock::new(); + + TYPE_HASH + .get_or_init(|| { + let schema = ::extended_message_schema(); + let hash = if schema.uses_extended_types() { + ::ros_z::extended_schema::compute_extended_type_hash(&schema) + .expect("extended message schema must produce a type hash") + .to_rihs_string() + } else { + use ::ros_z::dynamic::MessageSchemaTypeDescription; + + schema + .compute_type_hash() + .expect("standard-compatible extended schema must produce a standard type hash") + .to_rihs_string() + }; + + ::ros_z::entity::TypeHash::from_rihs_string(&hash) + .expect("extended message hash must be a valid RIHS01 string") + }) + .clone() + } + } +} + +fn impl_from_py_message(input: &DeriveInput) -> syn::Result { let name = &input.ident; let Data::Struct(ref data) = input.data else { @@ -67,17 +458,14 @@ fn impl_from_py_message(input: &DeriveInput) -> syn::Result = fields + let field_extractions: Vec = fields .named .iter() .map(|f| { let field_name = f.ident.as_ref().unwrap(); - let field_name_str = field_name_to_py_attr(field_name); + let field_name_str = field_name_to_attr(field_name); let field_type = &f.ty; - - // Check for #[ros_msg(zbuf)] attribute - let use_zbuf = has_ros_msg_attr(&f.attrs, "zbuf"); - + let use_zbuf = parse_ros_msg_args(&f.attrs)?.zbuf; generate_field_extraction(field_name, &field_name_str, field_type, use_zbuf) }) .collect::>>()?; @@ -94,7 +482,7 @@ fn impl_from_py_message(input: &DeriveInput) -> syn::Result syn::Result { +fn impl_into_py_message(input: &DeriveInput) -> syn::Result { let name = &input.ident; let Data::Struct(ref data) = input.data else { @@ -111,20 +499,16 @@ fn impl_into_py_message(input: &DeriveInput) -> syn::Result = fields + let field_constructions: Vec = fields .named .iter() .map(|f| { let field_name = f.ident.as_ref().unwrap(); - let field_name_str = field_name_to_py_attr(field_name); + let field_name_str = field_name_to_attr(field_name); let field_type = &f.ty; - - // Check for #[ros_msg(zbuf)] attribute - let use_zbuf = has_ros_msg_attr(&f.attrs, "zbuf"); - + let use_zbuf = parse_ros_msg_args(&f.attrs)?.zbuf; generate_field_construction(field_name, &field_name_str, field_type, use_zbuf) }) .collect::>>()?; @@ -147,29 +531,341 @@ fn impl_into_py_message(input: &DeriveInput) -> syn::Result syn::Result { + let field_name = field + .ident + .as_ref() + .ok_or_else(|| syn::Error::new_spanned(field, "named fields are required"))?; + let field_name_str = field_name_to_attr(field_name); + let field_type = generate_standard_message_field_type_tokens(&field.ty)?; + + Ok(quote! { + ::ros_z::dynamic::FieldSchema::new(#field_name_str, #field_type) + }) +} + +fn generate_standard_message_field_type_tokens(ty: &Type) -> syn::Result { + match ty { + Type::Path(type_path) => { + if type_path.qself.is_some() { + return unsupported_message_type( + ty, + "qualified self types are not supported in v1", + ); + } + + let last_segment = type_path.path.segments.last().ok_or_else(|| { + syn::Error::new_spanned( + ty, + "unsupported field type for ExtendedMessageTypeInfo derive", + ) + })?; + let ident_str = last_segment.ident.to_string(); + + match ident_str.as_str() { + "bool" => Ok(quote! { ::ros_z::dynamic::FieldType::Bool }), + "i8" => Ok(quote! { ::ros_z::dynamic::FieldType::Int8 }), + "u8" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint8 }), + "i16" => Ok(quote! { ::ros_z::dynamic::FieldType::Int16 }), + "u16" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint16 }), + "i32" => Ok(quote! { ::ros_z::dynamic::FieldType::Int32 }), + "u32" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint32 }), + "i64" => Ok(quote! { ::ros_z::dynamic::FieldType::Int64 }), + "u64" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint64 }), + "f32" => Ok(quote! { ::ros_z::dynamic::FieldType::Float32 }), + "f64" => Ok(quote! { ::ros_z::dynamic::FieldType::Float64 }), + "String" => Ok(quote! { ::ros_z::dynamic::FieldType::String }), + "usize" | "isize" => unsupported_message_type( + ty, + "usize and isize are not supported by MessageTypeInfo derive in v1", + ), + "Option" => unsupported_message_type( + ty, + "Option fields are not supported by MessageTypeInfo derive in v1", + ), + "HashMap" | "BTreeMap" => unsupported_message_type( + ty, + "map fields are not supported by MessageTypeInfo derive in v1", + ), + "Vec" => { + let PathArguments::AngleBracketed(args) = &last_segment.arguments else { + return unsupported_message_type( + ty, + "Vec fields must specify an element type", + ); + }; + let Some(GenericArgument::Type(inner)) = args.args.first() else { + return unsupported_message_type( + ty, + "Vec fields must specify an element type", + ); + }; + let inner_tokens = generate_standard_message_field_type_tokens(inner)?; + Ok(quote! { + ::ros_z::dynamic::FieldType::Sequence(::std::boxed::Box::new(#inner_tokens)) + }) + } + _ => Ok(quote! { + ::ros_z::dynamic::FieldType::Message( + <#ty as ::ros_z::MessageTypeInfo>::message_schema() + .expect("derived nested message schema must be available") + ) + }), + } + } + Type::Array(array) => { + let len = match &array.len { + Expr::Lit(expr_lit) => match &expr_lit.lit { + syn::Lit::Int(value) => value.base10_parse::()?, + _ => { + return unsupported_message_type( + ty, + "array lengths must be integer literals for MessageTypeInfo derive", + ); + } + }, + _ => { + return unsupported_message_type( + ty, + "array lengths must be integer literals for MessageTypeInfo derive", + ); + } + }; + + let inner_tokens = generate_standard_message_field_type_tokens(&array.elem)?; + Ok(quote! { + ::ros_z::dynamic::FieldType::Array(::std::boxed::Box::new(#inner_tokens), #len) + }) + } + Type::Tuple(_) => unsupported_message_type( + ty, + "tuple fields are not supported by MessageTypeInfo derive in v1", + ), + _ => unsupported_message_type( + ty, + "unsupported field type for MessageTypeInfo derive in v1", + ), + } +} + +fn generate_message_field_schema_tokens(field: &syn::Field) -> syn::Result { + let field_name = field + .ident + .as_ref() + .ok_or_else(|| syn::Error::new_spanned(field, "named fields are required"))?; + let field_name_str = field_name_to_attr(field_name); + let field_type = generate_message_field_type_tokens(&field.ty)?; + + Ok(quote! { + ::ros_z::dynamic::FieldSchema::new(#field_name_str, #field_type) + }) +} + +fn generate_message_field_type_tokens(ty: &Type) -> syn::Result { + match ty { + Type::Path(type_path) => { + if type_path.qself.is_some() { + return unsupported_message_type( + ty, + "qualified self types are not supported by ExtendedMessageTypeInfo derive in v1", + ); + } + + let last_segment = type_path.path.segments.last().ok_or_else(|| { + syn::Error::new_spanned( + ty, + "unsupported field type for ExtendedMessageTypeInfo derive", + ) + })?; + let ident_str = last_segment.ident.to_string(); + + match ident_str.as_str() { + "bool" => Ok(quote! { ::ros_z::dynamic::FieldType::Bool }), + "i8" => Ok(quote! { ::ros_z::dynamic::FieldType::Int8 }), + "u8" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint8 }), + "i16" => Ok(quote! { ::ros_z::dynamic::FieldType::Int16 }), + "u16" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint16 }), + "i32" => Ok(quote! { ::ros_z::dynamic::FieldType::Int32 }), + "u32" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint32 }), + "i64" => Ok(quote! { ::ros_z::dynamic::FieldType::Int64 }), + "u64" => Ok(quote! { ::ros_z::dynamic::FieldType::Uint64 }), + "f32" => Ok(quote! { ::ros_z::dynamic::FieldType::Float32 }), + "f64" => Ok(quote! { ::ros_z::dynamic::FieldType::Float64 }), + "String" => Ok(quote! { ::ros_z::dynamic::FieldType::String }), + "usize" | "isize" => unsupported_message_type( + ty, + "usize and isize are not supported by ExtendedMessageTypeInfo derive in v1", + ), + "HashMap" | "BTreeMap" => unsupported_message_type( + ty, + "map fields are not supported by ExtendedMessageTypeInfo derive in v1", + ), + "Option" => { + let PathArguments::AngleBracketed(args) = &last_segment.arguments else { + return unsupported_message_type( + ty, + "Option fields must specify an inner type", + ); + }; + let Some(GenericArgument::Type(inner)) = args.args.first() else { + return unsupported_message_type( + ty, + "Option fields must specify an inner type", + ); + }; + let inner_tokens = generate_message_field_type_tokens(inner)?; + Ok(quote! { + ::ros_z::dynamic::FieldType::Optional(::std::boxed::Box::new(#inner_tokens)) + }) + } + "Vec" => { + let PathArguments::AngleBracketed(args) = &last_segment.arguments else { + return unsupported_message_type( + ty, + "Vec fields must specify an element type", + ); + }; + let Some(GenericArgument::Type(inner)) = args.args.first() else { + return unsupported_message_type( + ty, + "Vec fields must specify an element type", + ); + }; + let inner_tokens = generate_message_field_type_tokens(inner)?; + Ok(quote! { + ::ros_z::dynamic::FieldType::Sequence(::std::boxed::Box::new(#inner_tokens)) + }) + } + _ => Ok(quote! { + <#ty as ::ros_z::ExtendedMessageTypeInfo>::extended_field_type() + }), + } + } + Type::Array(array) => { + let len = match &array.len { + Expr::Lit(expr_lit) => match &expr_lit.lit { + syn::Lit::Int(value) => value.base10_parse::()?, + _ => { + return unsupported_message_type( + ty, + "array lengths must be integer literals for MessageTypeInfo derive", + ); + } + }, + _ => { + return unsupported_message_type( + ty, + "array lengths must be integer literals for MessageTypeInfo derive", + ); + } + }; + + let inner_tokens = generate_message_field_type_tokens(&array.elem)?; + Ok(quote! { + ::ros_z::dynamic::FieldType::Array(::std::boxed::Box::new(#inner_tokens), #len) + }) + } + Type::Tuple(_) => unsupported_message_type( + ty, + "tuple fields are not supported by ExtendedMessageTypeInfo derive in v1", + ), + _ => unsupported_message_type( + ty, + "unsupported field type for ExtendedMessageTypeInfo derive in v1", + ), + } +} + +fn generate_enum_variant_schema_tokens(variant: &syn::Variant) -> syn::Result { + let variant_name = variant.ident.to_string(); + let payload = match &variant.fields { + Fields::Unit => quote! { ::ros_z::dynamic::EnumPayloadSchema::Unit }, + Fields::Unnamed(fields) if fields.unnamed.len() == 1 => { + let field_type = generate_message_field_type_tokens(&fields.unnamed[0].ty)?; + quote! { + ::ros_z::dynamic::EnumPayloadSchema::Newtype(::std::boxed::Box::new(#field_type)) + } + } + Fields::Unnamed(fields) => { + let field_types = fields + .unnamed + .iter() + .map(|field| generate_message_field_type_tokens(&field.ty)) + .collect::>>()?; + quote! { + ::ros_z::dynamic::EnumPayloadSchema::Tuple(::std::vec![#(#field_types),*]) + } + } + Fields::Named(fields) => { + let field_schemas = fields + .named + .iter() + .map(generate_message_field_schema_tokens) + .collect::>>()?; + quote! { + ::ros_z::dynamic::EnumPayloadSchema::Struct(::std::vec![#(#field_schemas),*]) + } + } + }; + + Ok(quote! { + ::ros_z::dynamic::EnumVariantSchema::new(#variant_name, #payload) + }) +} + +fn unsupported_message_type(node: &T, message: &str) -> syn::Result +where + T: quote::ToTokens, +{ + Err(syn::Error::new_spanned(node, message)) +} + +fn parse_canonical_type_name(type_name: &str) -> syn::Result<(String, String, String)> { + let parts: Vec<_> = type_name.split('/').collect(); + if parts.len() != 3 { + return Err(syn::Error::new( + proc_macro2::Span::call_site(), + "ros_msg type_name must look like \"my_pkg/msg/MyType\"", + )); + } + + match parts[1] { + "msg" | "srv" | "action" => Ok(( + parts[0].to_string(), + parts[1].to_string(), + parts[2].to_string(), + )), + _ => Err(syn::Error::new( + proc_macro2::Span::call_site(), + "ros_msg type_name kind must be one of: msg, srv, action", + )), + } +} + +fn canonical_to_dds_name(type_name: &str) -> syn::Result { + let (package, kind, name) = parse_canonical_type_name(type_name)?; + Ok(format!("{package}::{kind}::dds_::{name}_")) +} + +/// Generate extraction code for a single field. fn generate_field_extraction( field_name: &Ident, field_name_str: &str, field_type: &Type, use_zbuf: bool, -) -> syn::Result { - // Handle ZBuf fields specially - try zero-copy paths first +) -> syn::Result { if use_zbuf { return Ok(quote! { #field_name: { - use ::pyo3::types::{PyBytesMethods, PyByteArrayMethods}; + use ::pyo3::types::{PyByteArrayMethods, PyBytesMethods}; let py_attr = obj.getattr(#field_name_str)?; - // Try ZBufView first - clone is cheap (ref-counted ZSlices) if let Ok(view) = py_attr.downcast::<::ros_z::zbuf_view::ZBufView>() { view.borrow().zbuf().clone() } else if let Ok(bytes) = py_attr.downcast::<::pyo3::types::PyBytes>() { ::ros_z::ZBuf::from(bytes.as_bytes().to_vec()) } else if let Ok(bytearray) = py_attr.downcast::<::pyo3::types::PyByteArray>() { - // SAFETY: We immediately copy the data ::ros_z::ZBuf::from(unsafe { bytearray.as_bytes() }.to_vec()) } else { - // Fallback for lists (slow path) let bytes: Vec = py_attr.extract()?; ::ros_z::ZBuf::from(bytes) } @@ -177,32 +873,22 @@ fn generate_field_extraction( }); } - // Analyze the type match classify_type(field_type) { - TypeClass::Primitive => Ok(quote! { - #field_name: obj.getattr(#field_name_str)?.extract()? - }), - - TypeClass::String => Ok(quote! { + TypeClass::Primitive | TypeClass::String => Ok(quote! { #field_name: obj.getattr(#field_name_str)?.extract()? }), - TypeClass::Vec(inner) => { let inner_class = classify_type(&inner); match inner_class { - // Special case for Vec - use buffer protocol for performance TypeClass::Primitive if is_u8_type(&inner) => Ok(quote! { #field_name: { - use ::pyo3::types::{PyBytesMethods, PyByteArrayMethods}; + use ::pyo3::types::{PyByteArrayMethods, PyBytesMethods}; let py_attr = obj.getattr(#field_name_str)?; - // Try bytes/bytearray first (fast path - buffer protocol) if let Ok(bytes) = py_attr.downcast::<::pyo3::types::PyBytes>() { bytes.as_bytes().to_vec() } else if let Ok(bytearray) = py_attr.downcast::<::pyo3::types::PyByteArray>() { - // SAFETY: We immediately copy the data unsafe { bytearray.as_bytes() }.to_vec() } else { - // Fallback for lists (slow path) py_attr.extract()? } } @@ -210,84 +896,63 @@ fn generate_field_extraction( TypeClass::Primitive | TypeClass::String => Ok(quote! { #field_name: obj.getattr(#field_name_str)?.extract()? }), - _ => { - // Vec of nested messages - recursively extract - Ok(quote! { - #field_name: { - use ::pyo3::types::PyListMethods; - let py_list = obj.getattr(#field_name_str)?; - let mut vec = Vec::new(); - for item in py_list.iter()? { - vec.push(<#inner as ::ros_z::python_bridge::FromPyMessage>::from_py(&item?)?); - } - vec + _ => Ok(quote! { + #field_name: { + use ::pyo3::types::PyListMethods; + let py_list = obj.getattr(#field_name_str)?; + let mut vec = Vec::new(); + for item in py_list.iter()? { + vec.push(<#inner as ::ros_z::python_bridge::FromPyMessage>::from_py(&item?)?); } - }) - } + vec + } + }), } } - TypeClass::Array(inner, size) => { let inner_class = classify_type(&inner); match inner_class { - TypeClass::Primitive | TypeClass::String => { - // For primitive arrays, use zeroed memory for initialization - // This works for any array size - Ok(quote! { - #field_name: { - let v: Vec<_> = obj.getattr(#field_name_str)?.extract()?; - // Use zeroed memory for arrays larger than 32 elements - // SAFETY: All primitive numeric types and bool are valid when zeroed - let mut arr: #field_type = unsafe { ::std::mem::zeroed() }; - let len = ::std::cmp::min(v.len(), #size); - arr[..len].copy_from_slice(&v[..len]); - arr - } - }) - } - _ => { - // Fixed array of nested messages - Ok(quote! { - #field_name: { - use ::pyo3::types::PyListMethods; - let py_list = obj.getattr(#field_name_str)?; - let mut arr: #field_type = ::std::array::from_fn(|_| Default::default()); - for (i, item) in py_list.iter()?.enumerate().take(#size) { - arr[i] = <#inner as ::ros_z::python_bridge::FromPyMessage>::from_py(&item?)?; - } - arr + TypeClass::Primitive | TypeClass::String => Ok(quote! { + #field_name: { + let v: Vec<_> = obj.getattr(#field_name_str)?.extract()?; + let mut arr: #field_type = unsafe { ::std::mem::zeroed() }; + let len = ::std::cmp::min(v.len(), #size); + arr[..len].copy_from_slice(&v[..len]); + arr + } + }), + _ => Ok(quote! { + #field_name: { + use ::pyo3::types::PyListMethods; + let py_list = obj.getattr(#field_name_str)?; + let mut arr: #field_type = ::std::array::from_fn(|_| Default::default()); + for (i, item) in py_list.iter()?.enumerate().take(#size) { + arr[i] = <#inner as ::ros_z::python_bridge::FromPyMessage>::from_py(&item?)?; } - }) - } + arr + } + }), } } - - TypeClass::Nested => { - // Nested message - check for None and use Default if None - Ok(quote! { - #field_name: { - let py_attr = obj.getattr(#field_name_str)?; - if py_attr.is_none() { - Default::default() - } else { - <#field_type as ::ros_z::python_bridge::FromPyMessage>::from_py(&py_attr)? - } + TypeClass::Nested => Ok(quote! { + #field_name: { + let py_attr = obj.getattr(#field_name_str)?; + if py_attr.is_none() { + Default::default() + } else { + <#field_type as ::ros_z::python_bridge::FromPyMessage>::from_py(&py_attr)? } - }) - } - + } + }), TypeClass::ZBuf => Ok(quote! { #field_name: { - use ::pyo3::types::{PyBytesMethods, PyByteArrayMethods}; + use ::pyo3::types::{PyByteArrayMethods, PyBytesMethods}; let py_attr = obj.getattr(#field_name_str)?; - // Try bytes/bytearray first (fast path - buffer protocol) let bytes: Vec = if let Ok(bytes) = py_attr.downcast::<::pyo3::types::PyBytes>() { bytes.as_bytes().to_vec() } else if let Ok(bytearray) = py_attr.downcast::<::pyo3::types::PyByteArray>() { - // SAFETY: We immediately copy the data unsafe { bytearray.as_bytes() }.to_vec() } else { - // Fallback for lists (slow path) py_attr.extract()? }; ::ros_z::ZBuf::from(bytes) @@ -296,19 +961,16 @@ fn generate_field_extraction( } } -/// Generate construction code for a single field (Rust -> Python) +/// Generate construction code for a single field (Rust -> Python). fn generate_field_construction( field_name: &Ident, field_name_str: &str, field_type: &Type, use_zbuf: bool, -) -> syn::Result { - // Handle ZBuf fields specially - create zero-copy view using buffer protocol +) -> syn::Result { if use_zbuf { return Ok(quote! { { - // Create a ZBufView which implements buffer protocol for zero-copy access - // Python can use memoryview(zbuf_view) to get zero-copy access to the data let zbuf_view = ::ros_z::zbuf_view::ZBufView::new(self.#field_name.clone()); let py_view = ::pyo3::Py::new(py, zbuf_view)?; kwargs.set_item(#field_name_str, py_view)?; @@ -320,15 +982,12 @@ fn generate_field_construction( TypeClass::Primitive => Ok(quote! { kwargs.set_item(#field_name_str, self.#field_name)?; }), - TypeClass::String => Ok(quote! { kwargs.set_item(#field_name_str, &self.#field_name)?; }), - TypeClass::Vec(inner) => { let inner_class = classify_type(&inner); match inner_class { - // Special case for Vec - output as bytes for performance TypeClass::Primitive if is_u8_type(&inner) => Ok(quote! { { let py_bytes = ::pyo3::types::PyBytes::new_bound(py, &self.#field_name); @@ -338,58 +997,46 @@ fn generate_field_construction( TypeClass::Primitive | TypeClass::String => Ok(quote! { kwargs.set_item(#field_name_str, &self.#field_name)?; }), - _ => { - // Vec of nested messages - Ok(quote! { - { - use ::pyo3::types::PyListMethods; - let py_list = ::pyo3::types::PyList::empty_bound(py); - for item in &self.#field_name { - py_list.append( - <#inner as ::ros_z::python_bridge::IntoPyMessage>::into_py_message(item, py)? - )?; - } - kwargs.set_item(#field_name_str, py_list)?; + _ => Ok(quote! { + { + use ::pyo3::types::PyListMethods; + let py_list = ::pyo3::types::PyList::empty_bound(py); + for item in &self.#field_name { + py_list.append( + <#inner as ::ros_z::python_bridge::IntoPyMessage>::into_py_message(item, py)? + )?; } - }) - } + kwargs.set_item(#field_name_str, py_list)?; + } + }), } } - TypeClass::Array(inner, _) => { let inner_class = classify_type(&inner); match inner_class { TypeClass::Primitive | TypeClass::String => Ok(quote! { kwargs.set_item(#field_name_str, self.#field_name.to_vec())?; }), - _ => { - // Array of nested messages - Ok(quote! { - { - use ::pyo3::types::PyListMethods; - let py_list = ::pyo3::types::PyList::empty_bound(py); - for item in &self.#field_name { - py_list.append( - <#inner as ::ros_z::python_bridge::IntoPyMessage>::into_py_message(item, py)? - )?; - } - kwargs.set_item(#field_name_str, py_list)?; + _ => Ok(quote! { + { + use ::pyo3::types::PyListMethods; + let py_list = ::pyo3::types::PyList::empty_bound(py); + for item in &self.#field_name { + py_list.append( + <#inner as ::ros_z::python_bridge::IntoPyMessage>::into_py_message(item, py)? + )?; } - }) - } + kwargs.set_item(#field_name_str, py_list)?; + } + }), } } - - TypeClass::Nested => { - // Nested message - convert using trait - Ok(quote! { - kwargs.set_item( - #field_name_str, - <#field_type as ::ros_z::python_bridge::IntoPyMessage>::into_py_message(&self.#field_name, py)? - )?; - }) - } - + TypeClass::Nested => Ok(quote! { + kwargs.set_item( + #field_name_str, + <#field_type as ::ros_z::python_bridge::IntoPyMessage>::into_py_message(&self.#field_name, py)? + )?; + }), TypeClass::ZBuf => Ok(quote! { { use ::zenoh_buffers::buffer::SplitBuffer; @@ -401,7 +1048,7 @@ fn generate_field_construction( } } -/// Type classification for code generation +/// Type classification for Python conversion code generation. #[derive(Debug)] enum TypeClass { Primitive, @@ -412,15 +1059,13 @@ enum TypeClass { ZBuf, } -/// Classify a type for code generation purposes +/// Classify a type for Python conversion code generation purposes. fn classify_type(ty: &Type) -> TypeClass { if let Type::Path(type_path) = ty { let segments = &type_path.path.segments; if let Some(last_segment) = segments.last() { - let ident = &last_segment.ident; - let ident_str = ident.to_string(); + let ident_str = last_segment.ident.to_string(); - // Check for primitives if matches!( ident_str.as_str(), "bool" @@ -438,17 +1083,14 @@ fn classify_type(ty: &Type) -> TypeClass { return TypeClass::Primitive; } - // Check for String if ident_str == "String" { return TypeClass::String; } - // Check for ZBuf if ident_str == "ZBuf" { return TypeClass::ZBuf; } - // Check for Vec if ident_str == "Vec" { if let PathArguments::AngleBracketed(args) = &last_segment.arguments { if let Some(GenericArgument::Type(inner)) = args.args.first() { @@ -459,10 +1101,9 @@ fn classify_type(ty: &Type) -> TypeClass { } } - // Check for arrays if let Type::Array(arr) = ty { - if let syn::Expr::Lit(lit) = &arr.len { - if let Lit::Int(int_lit) = &lit.lit { + if let Expr::Lit(lit) = &arr.len { + if let syn::Lit::Int(int_lit) = &lit.lit { if let Ok(size) = int_lit.base10_parse::() { return TypeClass::Array(Box::new((*arr.elem).clone()), size); } @@ -470,44 +1111,56 @@ fn classify_type(ty: &Type) -> TypeClass { } } - // Default to nested message TypeClass::Nested } -/// Check if a field has a specific ros_msg attribute -fn has_ros_msg_attr(attrs: &[Attribute], attr_name: &str) -> bool { +#[derive(Default)] +struct RosMsgArgs { + module: Option, + type_name: Option, + zbuf: bool, +} + +fn parse_ros_msg_args(attrs: &[Attribute]) -> syn::Result { + let mut parsed = RosMsgArgs::default(); + for attr in attrs { - if attr.path().is_ident("ros_msg") { - if let Ok(meta) = attr.parse_args::() { - if meta == attr_name { - return true; - } - } + if !attr.path().is_ident("ros_msg") { + continue; } + + attr.parse_nested_meta(|meta| { + if meta.path.is_ident("module") { + let value = meta.value()?.parse::()?; + parsed.module = Some(value.value()); + return Ok(()); + } + + if meta.path.is_ident("type_name") { + let value = meta.value()?.parse::()?; + parsed.type_name = Some(value.value()); + return Ok(()); + } + + if meta.path.is_ident("zbuf") { + parsed.zbuf = true; + return Ok(()); + } + + Err(meta + .error("unsupported ros_msg attribute, expected one of: module, type_name, zbuf")) + })?; } - false + + Ok(parsed) } -/// Extract module path from #[ros_msg(module = "...")] attribute fn extract_module_path(attrs: &[Attribute]) -> syn::Result { - for attr in attrs { - if attr.path().is_ident("ros_msg") { - if let Ok(Meta::NameValue(nv)) = attr.parse_args() { - if nv.path.is_ident("module") { - if let syn::Expr::Lit(lit) = &nv.value { - if let Lit::Str(s) = &lit.lit { - return Ok(s.value()); - } - } - } - } - } - } - // Default module path - Ok("ros_z_msgs_py.types".to_string()) + Ok(parse_ros_msg_args(attrs)? + .module + .unwrap_or_else(|| "ros_z_msgs_py.types".to_string())) } -/// Check if a type is u8 fn is_u8_type(ty: &Type) -> bool { if let Type::Path(type_path) = ty { if let Some(last_segment) = type_path.path.segments.last() { @@ -517,11 +1170,8 @@ fn is_u8_type(ty: &Type) -> bool { false } -/// Convert Rust field name to Python attribute name -/// Handles r#type -> type conversion for Rust keywords -fn field_name_to_py_attr(ident: &Ident) -> String { +fn field_name_to_attr(ident: &Ident) -> String { let name = ident.to_string(); - // Strip r# prefix used for Rust keywords if let Some(stripped) = name.strip_prefix("r#") { stripped.to_string() } else { diff --git a/crates/ros-z-protocol/src/entity.rs b/crates/ros-z-protocol/src/entity.rs index 00757637..d6a2654e 100644 --- a/crates/ros-z-protocol/src/entity.rs +++ b/crates/ros-z-protocol/src/entity.rs @@ -54,7 +54,7 @@ impl Display for TopicKE { } /// ROS 2 node entity. -#[derive(Default, Debug, Hash, Clone, PartialEq, Eq)] +#[derive(Debug, Hash, Clone, PartialEq, Eq)] pub struct NodeEntity { pub domain_id: usize, pub z_id: ZenohId, @@ -85,9 +85,8 @@ impl NodeEntity { } /// ROS 2 entity kind (node, publisher, subscription, service, client). -#[derive(Default, Debug, Hash, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Hash, Clone, Copy, PartialEq, Eq)] pub enum EntityKind { - #[default] Node, Publisher, Subscription, @@ -122,6 +121,65 @@ impl core::str::FromStr for EntityKind { } } +/// ROS 2 endpoint kind (publisher, subscription, service, client). +#[derive(Debug, Hash, Clone, Copy, PartialEq, Eq)] +pub enum EndpointKind { + Publisher, + Subscription, + Service, + Client, +} + +impl Display for EndpointKind { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + EndpointKind::Publisher => write!(f, "MP"), + EndpointKind::Subscription => write!(f, "MS"), + EndpointKind::Service => write!(f, "SS"), + EndpointKind::Client => write!(f, "SC"), + } + } +} + +impl core::str::FromStr for EndpointKind { + type Err = &'static str; + + fn from_str(s: &str) -> Result { + match s { + "MP" => Ok(EndpointKind::Publisher), + "MS" => Ok(EndpointKind::Subscription), + "SS" => Ok(EndpointKind::Service), + "SC" => Ok(EndpointKind::Client), + _ => Err("Invalid endpoint kind"), + } + } +} + +impl From for EntityKind { + fn from(kind: EndpointKind) -> Self { + match kind { + EndpointKind::Publisher => EntityKind::Publisher, + EndpointKind::Subscription => EntityKind::Subscription, + EndpointKind::Service => EntityKind::Service, + EndpointKind::Client => EntityKind::Client, + } + } +} + +impl TryFrom for EndpointKind { + type Error = &'static str; + + fn try_from(kind: EntityKind) -> Result { + match kind { + EntityKind::Node => Err("Node is not a valid endpoint kind"), + EntityKind::Publisher => Ok(EndpointKind::Publisher), + EntityKind::Subscription => Ok(EndpointKind::Subscription), + EntityKind::Service => Ok(EndpointKind::Service), + EntityKind::Client => Ok(EndpointKind::Client), + } + } +} + /// Type hash (RIHS format). #[derive(Debug, Hash, PartialEq, Eq, Clone)] pub struct TypeHash { @@ -224,16 +282,22 @@ impl TypeInfo { } /// ROS 2 endpoint entity (publisher, subscription, service, client). -#[derive(Default, Debug, Hash, PartialEq, Eq, Clone)] +#[derive(Debug, Hash, PartialEq, Eq, Clone)] pub struct EndpointEntity { pub id: usize, - pub node: NodeEntity, - pub kind: EntityKind, + pub node: Option, + pub kind: EndpointKind, pub topic: String, pub type_info: Option, pub qos: QosProfile, } +impl EndpointEntity { + pub fn entity_kind(&self) -> EntityKind { + self.kind.into() + } +} + /// Generic ROS 2 entity (node or endpoint). #[derive(Debug, Clone, PartialEq, Eq)] pub enum Entity { diff --git a/crates/ros-z-protocol/src/format/rmw_zenoh.rs b/crates/ros-z-protocol/src/format/rmw_zenoh.rs index a0c78639..fe26ba81 100644 --- a/crates/ros-z-protocol/src/format/rmw_zenoh.rs +++ b/crates/ros-z-protocol/src/format/rmw_zenoh.rs @@ -10,8 +10,8 @@ use zenoh::{key_expr::KeyExpr, session::ZenohId, Result}; use crate::{ entity::{ - EndpointEntity, Entity, EntityConversionError, EntityKind, LivelinessKE, NodeEntity, - TopicKE, TypeHash, TypeInfo, + EndpointEntity, EndpointKind, Entity, EntityConversionError, EntityKind, LivelinessKE, + NodeEntity, TopicKE, TypeHash, TypeInfo, }, qos::QosProfile, }; @@ -33,9 +33,20 @@ impl KeyExprFormatter for RmwZenohFormatter { const ADMIN_SPACE: &'static str = "@ros2_lv"; fn topic_key_expr(entity: &EndpointEntity) -> Result { - let domain_id = entity.node.domain_id; + let EndpointEntity { + node: Some(node), + topic, + type_info, + .. + } = entity + else { + return Err(zenoh::Error::from( + "rmw-zenoh endpoint keys require node identity", + )); + }; + let domain_id = node.domain_id; let topic = { - let s = &entity.topic; + let s = topic.as_str(); let s = s.strip_prefix('/').unwrap_or(s); let s = s.strip_suffix('/').unwrap_or(s); @@ -50,14 +61,14 @@ impl KeyExprFormatter for RmwZenohFormatter { s.to_string() }; - let type_info = entity.type_info.as_ref().map_or( - format!("{EMPTY_TOPIC_TYPE}/{EMPTY_TOPIC_HASH}"), - |x| { - let type_name = Self::demangle_name(&x.name); - let type_hash = Self::demangle_name(&x.hash.to_string()); - format!("{type_name}/{type_hash}") - }, - ); + let type_info = + type_info + .as_ref() + .map_or(format!("{EMPTY_TOPIC_TYPE}/{EMPTY_TOPIC_HASH}"), |x| { + let type_name = Self::demangle_name(&x.name); + let type_hash = Self::demangle_name(&x.hash.to_string()); + format!("{type_name}/{type_hash}") + }); Ok(TopicKE::new( format!("{domain_id}/{topic}/{type_info}").try_into()?, @@ -68,19 +79,24 @@ impl KeyExprFormatter for RmwZenohFormatter { let EndpointEntity { id, node: - NodeEntity { + Some(NodeEntity { domain_id, z_id, id: node_id, name: node_name, namespace: node_namespace, enclave: _, - }, + }), kind, topic: topic_name, type_info, qos, - } = entity; + } = entity + else { + return Err(zenoh::Error::from( + "rmw-zenoh liveliness requires node identity", + )); + }; let node_namespace = if node_namespace.is_empty() { EMPTY_PLACEHOLDER.to_string() @@ -220,8 +236,8 @@ impl KeyExprFormatter for RmwZenohFormatter { Entity::Endpoint(EndpointEntity { id: entity_id, - node, - kind: entity_kind, + node: Some(node), + kind: EndpointKind::try_from(entity_kind).map_err(|_| ParsingError)?, topic: topic_name, type_info, qos, @@ -244,7 +260,7 @@ impl KeyExprFormatter for RmwZenohFormatter { #[cfg(test)] mod tests { use super::*; - use crate::entity::{EndpointEntity, EntityKind, NodeEntity, TypeInfo}; + use crate::entity::{EndpointEntity, EndpointKind, NodeEntity, TypeInfo}; use crate::qos::{QosDurability, QosHistory, QosProfile, QosReliability}; #[test] @@ -303,8 +319,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -349,8 +365,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -417,8 +433,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Subscription, + node: Some(node), + kind: EndpointKind::Subscription, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -450,8 +466,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "add_two_ints".to_string(), type_info: Some(TypeInfo::new( "example_interfaces/srv/AddTwoInts", @@ -486,8 +502,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Client, + node: Some(node), + kind: EndpointKind::Client, topic: "add_two_ints".to_string(), type_info: Some(TypeInfo::new( "example_interfaces/srv/AddTwoInts", @@ -570,8 +586,8 @@ mod tests { // Service with slashes in name (common pattern: /node_name/service_name) let entity = EndpointEntity { id: 10, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "/talker/get_type_description".to_string(), type_info: Some(TypeInfo::new( "type_description_interfaces::srv::dds_::GetTypeDescription_", @@ -612,8 +628,8 @@ mod tests { let entity = EndpointEntity { id: 11, - node, - kind: EntityKind::Client, + node: Some(node), + kind: EndpointKind::Client, topic: "/my_service/sub_service/action".to_string(), type_info: Some(TypeInfo::new( "example_interfaces/srv/AddTwoInts", @@ -650,8 +666,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "/ns/topic".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -691,8 +707,8 @@ mod tests { let entity = EndpointEntity { id: 2, - node, - kind: EntityKind::Subscription, + node: Some(node), + kind: EndpointKind::Subscription, topic: "/robot/sensor/data".to_string(), type_info: Some(TypeInfo::new("sensor_msgs/msg/Image", TypeHash::zero())), qos: QosProfile::default(), @@ -732,8 +748,8 @@ mod tests { let entity = EndpointEntity { id: 3, - node, - kind: EntityKind::Publisher, // Actions use pub/sub for feedback/status + node: Some(node), + kind: EndpointKind::Publisher, // Actions use pub/sub for feedback/status topic: "/fibonacci/_action/send_goal".to_string(), type_info: Some(TypeInfo::new( "action_tutorials_interfaces::action::dds_::Fibonacci_SendGoal_", @@ -774,8 +790,8 @@ mod tests { // Service with leading and trailing slashes let entity = EndpointEntity { id: 4, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "/my_service/".to_string(), type_info: Some(TypeInfo::new( "example_interfaces/srv/Trigger", @@ -815,8 +831,8 @@ mod tests { let entity = EndpointEntity { id: 5, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: None, // No type info qos: QosProfile::default(), @@ -853,8 +869,8 @@ mod tests { let entity = EndpointEntity { id: 6, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), // Type name with mangled slashes (as stored internally) type_info: Some(TypeInfo::new("std_msgs%msg%String", TypeHash::zero())), @@ -892,8 +908,8 @@ mod tests { let entity = EndpointEntity { id: 10, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "/talker/get_type_description".to_string(), type_info: Some(TypeInfo::new( "type_description_interfaces::srv::dds_::GetTypeDescription_", @@ -928,8 +944,8 @@ mod tests { let entity = EndpointEntity { id: 7, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "/data/temperature".to_string(), type_info: Some(TypeInfo::new( "sensor_msgs/msg/Temperature", @@ -976,8 +992,8 @@ mod tests { let entity = EndpointEntity { id: 8, - node, - kind: EntityKind::Subscription, + node: Some(node), + kind: EndpointKind::Subscription, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -1015,8 +1031,8 @@ mod tests { let entity = EndpointEntity { id: 9, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -1049,8 +1065,8 @@ mod tests { let entity = EndpointEntity { id: 10, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "image".to_string(), type_info: Some(TypeInfo::new( "sensor_msgs/msg/Image", @@ -1097,8 +1113,8 @@ mod tests { let entity = EndpointEntity { id: 11, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos, @@ -1141,8 +1157,8 @@ mod tests { let original = EndpointEntity { id: 12, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "/topic/name".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -1154,8 +1170,14 @@ mod tests { if let Entity::Endpoint(parsed_entity) = parsed { assert_eq!(parsed_entity.id, original.id); assert_eq!(parsed_entity.kind, original.kind); - assert_eq!(parsed_entity.node.name, original.node.name); - assert_eq!(parsed_entity.node.namespace, original.node.namespace); + assert_eq!( + parsed_entity.node.as_ref().unwrap().name, + original.node.as_ref().unwrap().name + ); + assert_eq!( + parsed_entity.node.as_ref().unwrap().namespace, + original.node.as_ref().unwrap().namespace + ); // Topic name should be reconstructed (slashes demangled) assert_eq!(parsed_entity.topic, "/topic/name"); } else { @@ -1178,8 +1200,8 @@ mod tests { let original = EndpointEntity { id: 13, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "/my/service".to_string(), type_info: Some(TypeInfo::new( "example_interfaces::srv::dds_::AddTwoInts_", @@ -1193,7 +1215,7 @@ mod tests { if let Entity::Endpoint(parsed_entity) = parsed { assert_eq!(parsed_entity.id, original.id); - assert_eq!(parsed_entity.kind, EntityKind::Service); + assert_eq!(parsed_entity.kind, EndpointKind::Service); assert_eq!(parsed_entity.topic, "/my/service"); assert_eq!( parsed_entity.type_info.as_ref().unwrap().name, @@ -1219,8 +1241,8 @@ mod tests { let original = EndpointEntity { id: 14, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "test".to_string(), type_info: None, qos: QosProfile::default(), @@ -1282,8 +1304,8 @@ mod tests { let entity = EndpointEntity { id: 16, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "simple_topic".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -1318,8 +1340,8 @@ mod tests { let entity = EndpointEntity { id: 17, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "/a//b".to_string(), // Consecutive slashes type_info: Some(TypeInfo::new("std_srvs/srv/Trigger", TypeHash::zero())), qos: QosProfile::default(), @@ -1348,8 +1370,8 @@ mod tests { let entity = EndpointEntity { id: 18, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), // DDS type name with ::dds_:: namespace type_info: Some(TypeInfo::new( @@ -1386,8 +1408,8 @@ mod tests { let long_topic = "/very/long/topic/name/with/many/segments/for/testing/purposes"; let entity = EndpointEntity { id: 19, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: long_topic.to_string(), type_info: Some(TypeInfo::new("std_srvs/srv/Trigger", TypeHash::zero())), qos: QosProfile::default(), @@ -1473,8 +1495,8 @@ mod kani_proofs { }; let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "/kani_topic".to_string(), type_info: Some(TypeInfo { name: "std_msgs/msg/String".to_string(), @@ -1492,8 +1514,11 @@ mod kani_proofs { let parsed = RmwZenohFormatter::parse_liveliness(&ke).expect("parse_liveliness"); if let crate::entity::Entity::Endpoint(ep) = parsed { - kani::assert(ep.node.domain_id == domain_id, "domain_id preserved"); - kani::assert(ep.kind == EntityKind::Publisher, "entity kind preserved"); + kani::assert( + ep.node.as_ref().unwrap().domain_id == domain_id, + "domain_id preserved", + ); + kani::assert(ep.kind == EndpointKind::Publisher, "entity kind preserved"); } else { kani::assert(false, "expected Endpoint entity"); } diff --git a/crates/ros-z-protocol/src/format/ros2dds.rs b/crates/ros-z-protocol/src/format/ros2dds.rs index 642b7975..ca09190a 100644 --- a/crates/ros-z-protocol/src/format/ros2dds.rs +++ b/crates/ros-z-protocol/src/format/ros2dds.rs @@ -12,7 +12,7 @@ use zenoh::{key_expr::KeyExpr, session::ZenohId, Result}; use crate::{ entity::{ - EndpointEntity, Entity, EntityConversionError, EntityKind, LivelinessKE, NodeEntity, + EndpointEntity, EndpointKind, Entity, EntityConversionError, LivelinessKE, NodeEntity, TopicKE, TypeHash, TypeInfo, }, qos::{QosDurability, QosHistory, QosProfile, QosReliability}, @@ -48,11 +48,10 @@ impl KeyExprFormatter for Ros2DdsFormatter { fn liveliness_key_expr(entity: &EndpointEntity, zid: &ZenohId) -> Result { // ros2dds format: @//@ros2_lv///[/] let kind = match entity.kind { - EntityKind::Publisher => "MP", - EntityKind::Subscription => "MS", - EntityKind::Service => "SS", - EntityKind::Client => "SC", - EntityKind::Node => "NN", // ros2dds doesn't actually expose node tokens + EndpointKind::Publisher => "MP", + EndpointKind::Subscription => "MS", + EndpointKind::Service => "SS", + EndpointKind::Client => "SC", }; // Escape slashes in topic name @@ -72,7 +71,7 @@ impl KeyExprFormatter for Ros2DdsFormatter { // QoS encoding for pub/sub only let qos_str = match entity.kind { - EntityKind::Publisher | EntityKind::Subscription => { + EndpointKind::Publisher | EndpointKind::Subscription => { format!("/{}", Self::encode_qos(&entity.qos, false)) } _ => String::new(), @@ -119,13 +118,13 @@ impl KeyExprFormatter for Ros2DdsFormatter { // Entity kind let kind_str = iter.next().ok_or(MissingEntityKind)?; let kind = match kind_str { - "MP" => EntityKind::Publisher, - "MS" => EntityKind::Subscription, - "SS" => EntityKind::Service, - "SC" => EntityKind::Client, + "MP" => EndpointKind::Publisher, + "MS" => EndpointKind::Subscription, + "SS" => EndpointKind::Service, + "SC" => EndpointKind::Client, "AS" | "AC" => { // Action server/client - map to Service for now - EntityKind::Service + EndpointKind::Service } _ => return Err(zenoh::Error::from(ParsingError)), }; @@ -146,16 +145,6 @@ impl KeyExprFormatter for Ros2DdsFormatter { QosProfile::default() }; - // Create a placeholder node (ros2dds doesn't include node info in liveliness) - let node = NodeEntity { - id: 0, - domain_id: 0, - z_id, - name: String::new(), - namespace: String::new(), - enclave: String::new(), - }; - let type_info = if type_name.is_empty() || type_name == "unknown" { None } else { @@ -165,9 +154,11 @@ impl KeyExprFormatter for Ros2DdsFormatter { }) }; + let _ = z_id; + Ok(Entity::Endpoint(EndpointEntity { id: 0, - node, + node: None, kind, topic, type_info, @@ -417,8 +408,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -446,8 +437,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Publisher, + node: Some(node), + kind: EndpointKind::Publisher, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -505,8 +496,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Subscription, + node: Some(node), + kind: EndpointKind::Subscription, topic: "chatter".to_string(), type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), qos: QosProfile::default(), @@ -523,6 +514,39 @@ mod tests { ); } + #[test] + fn test_parse_liveliness_restores_absolute_topic_name() { + let zid: zenoh::session::ZenohId = "1234567890abcdef1234567890abcdef".parse().unwrap(); + let node = NodeEntity::new( + 0, + zid, + 1, + "test_node".to_string(), + "/".to_string(), + String::new(), + ); + + let entity = EndpointEntity { + id: 1, + node: Some(node), + kind: EndpointKind::Publisher, + topic: "/chatter".to_string(), + type_info: Some(TypeInfo::new("std_msgs/msg/String", TypeHash::zero())), + qos: QosProfile::default(), + }; + + let liveliness_ke = Ros2DdsFormatter::liveliness_key_expr(&entity, &zid).unwrap(); + let parsed = Ros2DdsFormatter::parse_liveliness(&liveliness_ke).unwrap(); + + match parsed { + Entity::Endpoint(endpoint) => { + assert_eq!(endpoint.topic, "/chatter"); + assert!(endpoint.node.is_none()); + } + other => panic!("expected endpoint entity, got {:?}", other), + } + } + /// Test service server liveliness key expression #[test] fn test_service_liveliness_key_expr() { @@ -538,8 +562,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Service, + node: Some(node), + kind: EndpointKind::Service, topic: "add_two_ints".to_string(), type_info: Some(TypeInfo::new( "example_interfaces/srv/AddTwoInts", @@ -585,8 +609,8 @@ mod tests { let entity = EndpointEntity { id: 1, - node, - kind: EntityKind::Client, + node: Some(node), + kind: EndpointKind::Client, topic: "add_two_ints".to_string(), type_info: Some(TypeInfo::new( "example_interfaces/srv/AddTwoInts", diff --git a/crates/ros-z-protocol/src/lib.rs b/crates/ros-z-protocol/src/lib.rs index 2c6d3079..06312bee 100644 --- a/crates/ros-z-protocol/src/lib.rs +++ b/crates/ros-z-protocol/src/lib.rs @@ -34,8 +34,8 @@ //! //! let entity = EndpointEntity { //! id: 1, -//! node, -//! kind: EntityKind::Publisher, +//! node: Some(node), +//! kind: EndpointKind::Publisher, //! topic: "/chatter".to_string(), //! type_info: None, //! qos: Default::default(), @@ -53,7 +53,9 @@ pub mod entity; pub mod format; pub mod qos; -pub use entity::{EndpointEntity, Entity, EntityKind, NodeEntity, TypeHash, TypeInfo}; +pub use entity::{ + EndpointEntity, EndpointKind, Entity, EntityKind, NodeEntity, TypeHash, TypeInfo, +}; #[cfg(feature = "rmw-zenoh")] pub use format::rmw_zenoh::RmwZenohFormatter; #[cfg(feature = "ros2dds")] diff --git a/crates/ros-z-protocol/tests/key_expr.rs b/crates/ros-z-protocol/tests/key_expr.rs index 9a34e182..8d74c01a 100644 --- a/crates/ros-z-protocol/tests/key_expr.rs +++ b/crates/ros-z-protocol/tests/key_expr.rs @@ -6,7 +6,7 @@ //! return Err rather than panicking. use ros_z_protocol::{ - entity::{EndpointEntity, EntityKind, NodeEntity, TypeHash, TypeInfo}, + entity::{EndpointEntity, EndpointKind, NodeEntity, TypeHash, TypeInfo}, format::{rmw_zenoh::RmwZenohFormatter, KeyExprFormatter}, qos::{QosDurability, QosHistory, QosProfile, QosReliability}, }; @@ -27,10 +27,10 @@ fn default_node(domain_id: usize) -> NodeEntity { } } -fn endpoint_entity(domain_id: usize, kind: EntityKind, topic: &str) -> EndpointEntity { +fn endpoint_entity(domain_id: usize, kind: EndpointKind, topic: &str) -> EndpointEntity { EndpointEntity { id: 42, - node: default_node(domain_id), + node: Some(default_node(domain_id)), kind, topic: topic.to_string(), type_info: Some(TypeInfo { @@ -56,7 +56,7 @@ fn parse_liveliness(ke_str: &str) -> zenoh::Result/// @@ -252,7 +261,7 @@ fn test_topic_key_expr_format() { #[test] fn test_topic_key_expr_preserves_internal_slashes() { - let entity = endpoint_entity(0, EntityKind::Publisher, "/ns/topic"); + let entity = endpoint_entity(0, EndpointKind::Publisher, "/ns/topic"); let ke = RmwZenohFormatter::topic_key_expr(&entity).unwrap(); let ke_str = ke.to_string(); // Internal slashes must be preserved (not mangled) diff --git a/crates/ros-z-py/src/service.rs b/crates/ros-z-py/src/service.rs index 11c3a8b0..43186aaa 100644 --- a/crates/ros-z-py/src/service.rs +++ b/crates/ros-z-py/src/service.rs @@ -1,7 +1,7 @@ use crate::traits::{RawClient, RawServer}; use pyo3::prelude::*; use pyo3::types::PyDict; -use ros_z::service::QueryKey; +use ros_z::service::RequestId; use std::time::Duration; /// Python wrapper for service client @@ -27,56 +27,21 @@ impl PyZClient { #[allow(unsafe_op_in_unsafe_fn)] #[pymethods] impl PyZClient { - /// Send a service request - unsafe fn send_request(&self, py: Python, data: &Bound<'_, PyAny>) -> PyResult<()> { + /// Call a service request and wait for its response. + #[pyo3(signature = (data, timeout=None))] + unsafe fn call( + &self, + py: Python, + data: &Bound<'_, PyAny>, + timeout: Option, + ) -> PyResult { let cdr_bytes = ros_z_msgs::serialize_to_cdr(&self.request_type_name, data.py(), data)?; - - py.allow_threads(|| { - self.inner - .send_request_serialized(&cdr_bytes) - .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string())) - }) - } - - /// Receive a service response (blocking) - #[pyo3(signature = (timeout=None))] - unsafe fn take_response(&self, py: Python, timeout: Option) -> PyResult> { let timeout_duration = timeout.map(Duration::from_secs_f64); - let result = py.allow_threads(|| self.inner.take_response_serialized(timeout_duration)); - - match result { - Ok(cdr_bytes) => { - let obj = - ros_z_msgs::deserialize_from_cdr(&self.response_type_name, py, &cdr_bytes)?; - Ok(Some(obj)) - } - Err(e) => { - let err_str = e.to_string(); - if err_str.contains("timeout") - || err_str.contains("Timeout") - || err_str.contains("timed out") - || err_str.contains("No sample available") - { - Ok(None) - } else { - Err(pyo3::exceptions::PyRuntimeError::new_err(err_str)) - } - } - } - } - - /// Try to receive a response without blocking - unsafe fn try_take_response(&self, py: Python) -> PyResult> { - match self.inner.try_take_response_serialized() { - Ok(Some(cdr_bytes)) => { - let obj = - ros_z_msgs::deserialize_from_cdr(&self.response_type_name, py, &cdr_bytes)?; - Ok(Some(obj)) - } - Ok(None) => Ok(None), - Err(e) => Err(pyo3::exceptions::PyRuntimeError::new_err(e.to_string())), - } + let cdr_bytes = py + .allow_threads(|| self.inner.call_serialized(&cdr_bytes, timeout_duration)) + .map_err(|e| pyo3::exceptions::PyRuntimeError::new_err(e.to_string()))?; + ros_z_msgs::deserialize_from_cdr(&self.response_type_name, py, &cdr_bytes) } /// Get the service type name (for debugging) @@ -127,8 +92,8 @@ impl PyZServer { let obj = ros_z_msgs::deserialize_from_cdr(&self.request_type_name, py, &cdr_bytes)?; let request_id = PyDict::new_bound(py); - request_id.set_item("sn", key.sn)?; - request_id.set_item("gid", key.gid.to_vec())?; + request_id.set_item("sn", key.sequence_number)?; + request_id.set_item("gid", key.writer_guid.to_vec())?; Ok((request_id.into(), obj)) } @@ -147,7 +112,10 @@ impl PyZServer { let gid_vec: Vec = request_id.get_item("gid")?.unwrap().extract()?; let mut gid = [0u8; 16]; gid.copy_from_slice(&gid_vec[..16]); - let key = QueryKey { sn, gid }; + let key = RequestId { + sequence_number: sn, + writer_guid: gid, + }; py.allow_threads(|| { let inner = self diff --git a/crates/ros-z-py/src/traits.rs b/crates/ros-z-py/src/traits.rs index 2dffc261..5a1e1865 100644 --- a/crates/ros-z-py/src/traits.rs +++ b/crates/ros-z-py/src/traits.rs @@ -93,19 +93,17 @@ impl RawSubscriber for GenericSubWrapper { // -- Generic service wrappers using RawBytesService -- -use ros_z::service::{QueryKey, ZClient, ZServer}; +use ros_z::service::{RequestId, ServiceReply, ZClient, ZServer}; /// Type-erased client trait for Python interop pub(crate) trait RawClient: Send + Sync { - fn send_request_serialized(&self, data: &[u8]) -> Result<()>; - fn take_response_serialized(&self, timeout: Option) -> Result>; - fn try_take_response_serialized(&self) -> Result>>; + fn call_serialized(&self, data: &[u8], timeout: Option) -> Result>; } /// Type-erased server trait for Python interop pub(crate) trait RawServer: Send + Sync { - fn take_request_serialized(&self) -> Result<(QueryKey, Vec)>; - fn send_response_serialized(&self, data: &[u8], key: &QueryKey) -> Result<()>; + fn take_request_serialized(&self) -> Result<(RequestId, Vec)>; + fn send_response_serialized(&self, data: &[u8], request_id: &RequestId) -> Result<()>; } /// Generic client wrapper using RawBytesService @@ -120,85 +118,72 @@ impl GenericClientWrapper { } impl RawClient for GenericClientWrapper { - fn send_request_serialized(&self, data: &[u8]) -> Result<()> { + fn call_serialized(&self, data: &[u8], timeout: Option) -> Result> { let request = RawBytesMessage(data.to_vec()); + let timeout = timeout.unwrap_or(Duration::from_secs(3600)); let rt = tokio::runtime::Handle::try_current() .or_else(|_| tokio::runtime::Runtime::new().map(|rt| rt.handle().clone()))?; - rt.block_on(async { + let response = rt.block_on(async { self.inner - .send_request(&request) + .call_or_timeout(&request, timeout) .await - .map_err(|e| anyhow::anyhow!("Failed to send request: {}", e)) - }) - } - - fn take_response_serialized(&self, timeout: Option) -> Result> { - let timeout_duration = timeout.unwrap_or(Duration::from_secs(3600)); - let response = self - .inner - .take_response_timeout(timeout_duration) - .map_err(|e| anyhow::anyhow!("Failed to receive response: {}", e))?; + .map_err(|e| anyhow::anyhow!("Failed to call service: {}", e)) + })?; Ok(response.0) } - - fn try_take_response_serialized(&self) -> Result>> { - match self.inner.take_response_timeout(Duration::from_millis(1)) { - Ok(response) => Ok(Some(response.0)), - Err(e) => { - let err_str = e.to_string(); - if err_str.contains("timeout") - || err_str.contains("Timeout") - || err_str.contains("No sample available") - { - Ok(None) - } else { - Err(anyhow::anyhow!("Failed to receive response: {}", e)) - } - } - } - } } /// Generic server wrapper using RawBytesService pub struct GenericServerWrapper { inner: std::sync::Mutex>, + pending: std::sync::Mutex>>, } impl GenericServerWrapper { pub fn new(inner: ZServer) -> Self { Self { inner: std::sync::Mutex::new(inner), + pending: std::sync::Mutex::new(std::collections::HashMap::new()), } } } impl RawServer for GenericServerWrapper { - fn take_request_serialized(&self) -> Result<(QueryKey, Vec)> { + fn take_request_serialized(&self) -> Result<(RequestId, Vec)> { let mut server = self .inner .lock() .map_err(|e| anyhow::anyhow!("Failed to lock server: {}", e))?; - let (key, request) = server + let request = server .take_request() .map_err(|e| anyhow::anyhow!("Failed to receive request: {}", e))?; + let (request, reply) = request.into_parts(); + let request_id = reply.id().clone(); + + self.pending + .lock() + .map_err(|e| anyhow::anyhow!("Failed to lock pending replies: {}", e))? + .insert(request_id.clone(), reply); - Ok((key, request.0)) + Ok((request_id, request.0)) } - fn send_response_serialized(&self, data: &[u8], key: &QueryKey) -> Result<()> { + fn send_response_serialized(&self, data: &[u8], request_id: &RequestId) -> Result<()> { let response = RawBytesMessage(data.to_vec()); - let mut server = self - .inner + let reply = self + .pending .lock() - .map_err(|e| anyhow::anyhow!("Failed to lock server: {}", e))?; + .map_err(|e| anyhow::anyhow!("Failed to lock pending replies: {}", e))? + .remove(request_id) + .ok_or_else(|| anyhow::anyhow!("Unknown request id"))?; - server - .send_response(&response, key) + reply + .reply_blocking(&response) .map_err(|e| anyhow::anyhow!("Failed to send response: {}", e)) } } diff --git a/crates/ros-z-tests/tests/common/mod.rs b/crates/ros-z-tests/tests/common/mod.rs index 966715a2..d7640d30 100644 --- a/crates/ros-z-tests/tests/common/mod.rs +++ b/crates/ros-z-tests/tests/common/mod.rs @@ -209,8 +209,9 @@ pub fn wait_for_service_ready( let rt = tokio::runtime::Runtime::new()?; let result = rt.block_on(async { - client.send_request(&test_request).await?; - client.take_response_timeout(Duration::from_millis(500)) + client + .call_or_timeout(&test_request, Duration::from_millis(500)) + .await }); if result.is_ok() { diff --git a/crates/ros-z-tests/tests/dds_interop.rs b/crates/ros-z-tests/tests/dds_interop.rs index bdc1f9af..b4980909 100644 --- a/crates/ros-z-tests/tests/dds_interop.rs +++ b/crates/ros-z-tests/tests/dds_interop.rs @@ -275,12 +275,7 @@ fn test_ros2_dds_service_server_to_ros_z_client() { let req = AddTwoIntsRequest { a: 15, b: 27 }; println!("Sending request: {} + {}", req.a, req.b); - client - .send_request(&req) - .await - .expect("Failed to send request"); - - match client.take_response_timeout(Duration::from_secs(10)) { + match client.call_or_timeout(&req, Duration::from_secs(10)).await { Ok(resp) => { println!("Received response: {}", resp.sum); assert_eq!(resp.sum, 42, "Expected 15 + 27 = 42, got {}", resp.sum); @@ -342,12 +337,16 @@ fn test_ros_z_service_server_to_ros2_dds_client() { // Handle one request match server.async_take_request().await { - Ok((key, req)) => { - println!("Received request: {} + {}", req.a, req.b); - let resp = AddTwoIntsResponse { sum: req.a + req.b }; - server - .send_response(&resp, &key) - .expect("Failed to send response"); + Ok(req) => { + println!( + "Received request: {} + {}", + req.message().a, + req.message().b + ); + let resp = AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; + req.reply(&resp).await.expect("Failed to send response"); println!("Sent response: {}", resp.sum); } Err(e) => { diff --git a/crates/ros-z-tests/tests/humble_jazzy_bridge.rs b/crates/ros-z-tests/tests/humble_jazzy_bridge.rs index cb520b9d..d23fa52d 100644 --- a/crates/ros-z-tests/tests/humble_jazzy_bridge.rs +++ b/crates/ros-z-tests/tests/humble_jazzy_bridge.rs @@ -145,9 +145,8 @@ fn test_service_humble_server_jazzy_client() { .expect("failed to create client"); let req = ros_z_msgs::ros::example_interfaces::AddTwoIntsRequest { a: 3, b: 7 }; - client.send_request(&req).await?; // ctx / node / client are dropped here, inside the runtime - client.take_response_timeout(Duration::from_secs(20)) + client.call_or_timeout(&req, Duration::from_secs(20)).await }); let response = response.expect("did not receive service response within timeout"); @@ -187,13 +186,16 @@ fn test_service_jazzy_server_humble_client() { // Handle one request or give up after 10 s. let deadline = tokio::time::Instant::now() + Duration::from_secs(10); loop { - if let Ok((key, req)) = server.take_request() { - let resp = - ros_z_msgs::ros::example_interfaces::AddTwoIntsResponse { sum: req.a + req.b }; - let _ = server.send_response(&resp, &key); + if let Ok(req) = server.take_request() { + let resp = ros_z_msgs::ros::example_interfaces::AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; + let _ = req.reply_blocking(&resp); println!( "Handled Humble→Jazzy service call: {} + {} = {}", - req.a, req.b, resp.sum + req.message().a, + req.message().b, + resp.sum ); break; } diff --git a/crates/ros-z-tests/tests/parameter_tests.rs b/crates/ros-z-tests/tests/parameter_tests.rs index 88986fc6..5393b49a 100644 --- a/crates/ros-z-tests/tests/parameter_tests.rs +++ b/crates/ros-z-tests/tests/parameter_tests.rs @@ -519,15 +519,14 @@ mod service_tests { .build() .expect("get client"); - get_client - .send_request(&GetParametersRequest { - names: vec!["value".to_string()], - }) - .await - .expect("send"); - let resp: GetParametersResponse = get_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &GetParametersRequest { + names: vec!["value".to_string()], + }, + Duration::from_secs(5), + ) + .await .expect("response"); assert_eq!(resp.values.len(), 1); @@ -540,16 +539,15 @@ mod service_tests { .build() .expect("list client"); - list_client - .send_request(&ListParametersRequest { - prefixes: vec![], - depth: 0, - }) - .await - .expect("send"); - let list_resp: ListParametersResponse = list_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &ListParametersRequest { + prefixes: vec![], + depth: 0, + }, + Duration::from_secs(5), + ) + .await .expect("list response"); assert!(list_resp.result.names.contains(&"value".to_string())); @@ -564,18 +562,17 @@ mod service_tests { wire_value.r#type = 2; wire_value.integer_value = 42; - set_client - .send_request(&SetParametersRequest { - parameters: vec![rcl_interfaces::Parameter { - name: "value".to_string(), - value: wire_value, - }], - }) - .await - .expect("send"); - let set_resp: SetParametersResponse = set_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &SetParametersRequest { + parameters: vec![rcl_interfaces::Parameter { + name: "value".to_string(), + value: wire_value, + }], + }, + Duration::from_secs(5), + ) + .await .expect("set response"); assert_eq!(set_resp.results.len(), 1); @@ -591,15 +588,14 @@ mod service_tests { .build() .expect("types client"); - types_client - .send_request(&GetParameterTypesRequest { - names: vec!["value".to_string(), "nonexistent".to_string()], - }) - .await - .expect("send"); - let types_resp: GetParameterTypesResponse = types_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &GetParameterTypesRequest { + names: vec!["value".to_string(), "nonexistent".to_string()], + }, + Duration::from_secs(5), + ) + .await .expect("types response"); let type_bytes = types_resp.types.contiguous(); @@ -613,15 +609,14 @@ mod service_tests { .build() .expect("desc client"); - desc_client - .send_request(&DescribeParametersRequest { - names: vec!["value".to_string()], - }) - .await - .expect("send"); - let desc_resp: DescribeParametersResponse = desc_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &DescribeParametersRequest { + names: vec!["value".to_string()], + }, + Duration::from_secs(5), + ) + .await .expect("desc response"); assert_eq!(desc_resp.descriptors.len(), 1); @@ -689,15 +684,14 @@ mod service_tests { .build() .expect("atomic client"); - atomic_client - .send_request(&SetParametersAtomicallyRequest { - parameters: vec![make_int("a", 10), make_int("b", 20)], - }) - .await - .expect("send"); - let atomic_resp: SetParametersAtomicallyResponse = atomic_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &SetParametersAtomicallyRequest { + parameters: vec![make_int("a", 10), make_int("b", 20)], + }, + Duration::from_secs(5), + ) + .await .expect("atomic response"); assert!( @@ -716,15 +710,14 @@ mod service_tests { .build() .expect("get client"); - get_client - .send_request(&GetParametersRequest { - names: vec!["a".to_string(), "b".to_string()], - }) - .await - .expect("send"); - let get_resp: GetParametersResponse = get_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &GetParametersRequest { + names: vec!["a".to_string(), "b".to_string()], + }, + Duration::from_secs(5), + ) + .await .expect("get response"); assert_eq!(get_resp.values.len(), 2); @@ -777,15 +770,14 @@ mod service_tests { } }; - atomic_client - .send_request(&SetParametersAtomicallyRequest { - parameters: vec![make_int("a", 10), make_int("b", 20)], - }) - .await - .expect("send"); - let resp: SetParametersAtomicallyResponse = atomic_client - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout( + &SetParametersAtomicallyRequest { + parameters: vec![make_int("a", 10), make_int("b", 20)], + }, + Duration::from_secs(5), + ) + .await .expect("atomic response"); assert!( diff --git a/crates/ros-z-tests/tests/python_interop.rs b/crates/ros-z-tests/tests/python_interop.rs index 2d1c42e6..91b4ffde 100644 --- a/crates/ros-z-tests/tests/python_interop.rs +++ b/crates/ros-z-tests/tests/python_interop.rs @@ -243,11 +243,8 @@ fn test_python_server_rust_client() { let rt = tokio::runtime::Runtime::new().unwrap(); rt.block_on(async { client - .send_request(&request) + .call_or_timeout(&request, Duration::from_secs(5)) .await - .expect("Failed to send request"); - client - .take_response_timeout(Duration::from_secs(5)) .map(|resp| resp.sum) }) }) @@ -295,13 +292,17 @@ fn test_rust_server_python_client() { // Handle one request with polling let start = std::time::Instant::now(); while start.elapsed() < Duration::from_secs(10) { - if let Ok((key, req)) = server.take_request() { - let sum = req.a + req.b; - println!(" [rust] Received: {} + {} = {}", req.a, req.b, sum); + if let Ok(req) = server.take_request() { + let sum = req.message().a + req.message().b; + println!( + " [rust] Received: {} + {} = {}", + req.message().a, + req.message().b, + sum + ); let response = AddTwoIntsResponse { sum }; - server - .send_response(&response, &key) + req.reply_blocking(&response) .expect("Failed to send response"); break; } diff --git a/crates/ros-z-tests/tests/service_interop.rs b/crates/ros-z-tests/tests/service_interop.rs index 7b17034e..2d8fc936 100644 --- a/crates/ros-z-tests/tests/service_interop.rs +++ b/crates/ros-z-tests/tests/service_interop.rs @@ -30,7 +30,6 @@ fn test_ros_z_server_ros_z_client() { .build() .expect("Failed to create node"); - // Correct API: use tuple type and take_request/send_response pattern let mut zsrv = node .create_service::("add_two_ints_test1") .build() @@ -39,12 +38,17 @@ fn test_ros_z_server_ros_z_client() { println!("Server ready, waiting for requests..."); // Handle one request - if let Ok((key, req)) = zsrv.take_request() { - println!("Received request: {} + {}", req.a, req.b); - let resp = AddTwoIntsResponse { sum: req.a + req.b }; + if let Ok(req) = zsrv.take_request() { + println!( + "Received request: {} + {}", + req.message().a, + req.message().b + ); + let resp = AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; println!("Sending response: {}", resp.sum); - zsrv.send_response(&resp, &key) - .expect("Failed to send response"); + req.reply_blocking(&resp).expect("Failed to send response"); } }); @@ -72,13 +76,9 @@ fn test_ros_z_server_ros_z_client() { println!("Sending request..."); - let req = AddTwoIntsRequest { a: 5, b: 3 }; - zcli.send_request(&req) - .await - .expect("Failed to send request"); - let resp = zcli - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout(&AddTwoIntsRequest { a: 5, b: 3 }, Duration::from_secs(5)) + .await .expect("Failed to receive response"); println!("Received response: {}", resp.sum); @@ -115,11 +115,16 @@ fn test_ros_z_server_ros_z_client_multipart_name() { println!("Server ready (multi-part name), waiting for requests..."); - if let Ok((key, req)) = zsrv.take_request() { - println!("Received request: {} + {}", req.a, req.b); - let resp = AddTwoIntsResponse { sum: req.a + req.b }; - zsrv.send_response(&resp, &key) - .expect("Failed to send response"); + if let Ok(req) = zsrv.take_request() { + println!( + "Received request: {} + {}", + req.message().a, + req.message().b + ); + let resp = AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; + req.reply_blocking(&resp).expect("Failed to send response"); } }); @@ -143,13 +148,9 @@ fn test_ros_z_server_ros_z_client_multipart_name() { println!("Client ready, waiting..."); tokio::time::sleep(Duration::from_millis(500)).await; - let req = AddTwoIntsRequest { a: 7, b: 5 }; - zcli.send_request(&req) - .await - .expect("Failed to send request"); - let resp = zcli - .take_response_timeout(Duration::from_secs(5)) + .call_or_timeout(&AddTwoIntsRequest { a: 7, b: 5 }, Duration::from_secs(5)) + .await .expect("Failed to receive response"); println!("Received response: {}", resp.sum); @@ -192,12 +193,17 @@ fn test_ros_z_server_ros2_client() { println!("Server ready for ROS2 client..."); // Handle one request - if let Ok((key, req)) = zsrv.take_request() { - println!("Received request from ROS2: {} + {}", req.a, req.b); - let resp = AddTwoIntsResponse { sum: req.a + req.b }; + if let Ok(req) = zsrv.take_request() { + println!( + "Received request from ROS2: {} + {}", + req.message().a, + req.message().b + ); + let resp = AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; println!("Sending response: {}", resp.sum); - zsrv.send_response(&resp, &key) - .expect("Failed to send response"); + req.reply_blocking(&resp).expect("Failed to send response"); } }); @@ -260,12 +266,17 @@ fn test_ros_z_server_ros2_client_multipart() { println!("Server ready for ROS2 client (multi-part name)..."); - if let Ok((key, req)) = zsrv.take_request() { - println!("Received request from ROS2: {} + {}", req.a, req.b); - let resp = AddTwoIntsResponse { sum: req.a + req.b }; + if let Ok(req) = zsrv.take_request() { + println!( + "Received request from ROS2: {} + {}", + req.message().a, + req.message().b + ); + let resp = AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; println!("Sending response: {}", resp.sum); - zsrv.send_response(&resp, &key) - .expect("Failed to send response"); + req.reply_blocking(&resp).expect("Failed to send response"); } }); @@ -357,13 +368,12 @@ fn test_ros2_server_ros_z_client_multipart() { println!("Calling ROS2 multi-part server..."); - let req = AddTwoIntsRequest { a: 11, b: 13 }; - zcli.send_request(&req) - .await - .expect("Failed to send request"); - let resp = zcli - .take_response_timeout(std::time::Duration::from_secs(5)) + .call_or_timeout( + &AddTwoIntsRequest { a: 11, b: 13 }, + std::time::Duration::from_secs(5), + ) + .await .expect("Failed to receive response"); println!("Received response from ROS2: {}", resp.sum); @@ -440,13 +450,12 @@ fn test_ros2_server_ros_z_client() { println!("Calling ROS2 server..."); - let req = AddTwoIntsRequest { a: 15, b: 9 }; - zcli.send_request(&req) - .await - .expect("Failed to send request"); - let resp = zcli - .take_response_timeout(std::time::Duration::from_secs(5)) + .call_or_timeout( + &AddTwoIntsRequest { a: 15, b: 9 }, + std::time::Duration::from_secs(5), + ) + .await .expect("Failed to receive response"); println!("Received response from ROS2: {}", resp.sum); diff --git a/crates/ros-z-tests/tests/type_description_integration.rs b/crates/ros-z-tests/tests/type_description_integration.rs index a57d342e..33603d02 100644 --- a/crates/ros-z-tests/tests/type_description_integration.rs +++ b/crates/ros-z-tests/tests/type_description_integration.rs @@ -111,10 +111,13 @@ fn test_static_pub_dynamic_sub_with_type_discovery() { // Discover schema and create dynamic subscriber println!("Discovering schema from publisher..."); - let (subscriber, discovered_schema) = sub_node + let subscriber = sub_node .create_dyn_sub_auto("/test_topic", Duration::from_secs(15)) .await + .expect("Failed to create dynamic subscriber builder with auto-discovery") + .build() .expect("Failed to create dynamic subscriber with auto-discovery"); + let discovered_schema = subscriber.schema().expect("discovered schema"); println!("Discovered schema: {}", discovered_schema.type_name); assert_eq!(discovered_schema.type_name, "std_msgs/msg/String"); @@ -204,6 +207,7 @@ fn test_dynamic_pub_dynamic_sub_with_type_discovery() { // Create dynamic publisher (auto-registers schema) let publisher = pub_node .create_dyn_pub("/point_topic", schema.clone()) + .build() .expect("Failed to create dynamic publisher"); println!( @@ -252,10 +256,13 @@ fn test_dynamic_pub_dynamic_sub_with_type_discovery() { // Discover schema and create dynamic subscriber println!("Discovering schema from dynamic publisher..."); - let (subscriber, discovered_schema) = sub_node + let subscriber = sub_node .create_dyn_sub_auto("/point_topic", Duration::from_secs(15)) .await + .expect("Failed to create dynamic subscriber builder with auto-discovery") + .build() .expect("Failed to create dynamic subscriber with auto-discovery"); + let discovered_schema = subscriber.schema().expect("discovered schema"); println!("Discovered schema: {}", discovered_schema.type_name); assert_eq!(discovered_schema.type_name, "geometry_msgs/msg/Point"); @@ -341,6 +348,7 @@ fn test_dynamic_pub_static_sub() { // Create dynamic publisher let publisher = pub_node .create_dyn_pub("/dyn_to_static", schema.clone()) + .build() .expect("Failed to create dynamic publisher"); println!("Dynamic publisher created"); @@ -462,6 +470,7 @@ fn test_static_pub_dynamic_sub_known_schema() { let subscriber = sub_node .create_dyn_sub("/static_to_dyn", schema) + .build() .expect("Failed to create dynamic subscriber"); println!("Dynamic subscriber created with known schema"); @@ -555,6 +564,7 @@ fn test_multiple_publishers_schema_discovery() { let publisher1 = pub1_node .create_dyn_pub("/multi_pub_topic", schema.clone()) + .build() .expect("Failed to create publisher 1"); // Create second publisher with type description service @@ -568,6 +578,7 @@ fn test_multiple_publishers_schema_discovery() { let publisher2 = pub2_node .create_dyn_pub("/multi_pub_topic", schema.clone()) + .build() .expect("Failed to create publisher 2"); println!("Two publishers created on /multi_pub_topic"); @@ -584,10 +595,13 @@ fn test_multiple_publishers_schema_discovery() { .expect("Failed to create subscriber node"); println!("Discovering schema from publishers..."); - let (subscriber, discovered_schema) = sub_node + let subscriber = sub_node .create_dyn_sub_auto("/multi_pub_topic", Duration::from_secs(15)) .await + .expect("Failed to create subscriber builder with auto-discovery") + .build() .expect("Failed to create subscriber with auto-discovery"); + let discovered_schema = subscriber.schema().expect("discovered schema"); println!("Discovered schema: {}", discovered_schema.type_name); diff --git a/crates/ros-z-tests/tests/type_description_interop.rs b/crates/ros-z-tests/tests/type_description_interop.rs index 68814c89..a2b0c49b 100644 --- a/crates/ros-z-tests/tests/type_description_interop.rs +++ b/crates/ros-z-tests/tests/type_description_interop.rs @@ -143,14 +143,9 @@ fn test_get_type_description_from_ros2_talker() { include_type_sources: true, }; - println!("Calling GetTypeDescription for std_msgs/msg/String..."); - zcli.send_request(&req) - .await - .expect("Failed to send request"); - - println!("Waiting for response (timeout: 15s)..."); let resp = zcli - .take_response_timeout(Duration::from_secs(15)) + .call_or_timeout(&req, Duration::from_secs(15)) + .await .expect("Failed to receive response"); resp @@ -256,13 +251,9 @@ fn test_get_type_description_without_sources() { include_type_sources: false, }; - println!("Calling GetTypeDescription without hash..."); - zcli.send_request(&req) - .await - .expect("Failed to send request"); - let resp = zcli - .take_response_timeout(Duration::from_secs(15)) + .call_or_timeout(&req, Duration::from_secs(15)) + .await .expect("Failed to receive response"); resp @@ -388,12 +379,9 @@ fn test_dynamic_subscriber_from_type_description() { include_type_sources: false, }; - zcli.send_request(&req) - .await - .expect("Failed to send request"); - let resp = zcli - .take_response_timeout(Duration::from_secs(15)) + .call_or_timeout(&req, Duration::from_secs(15)) + .await .expect("Failed to receive GetTypeDescription response"); assert!( diff --git a/crates/ros-z/Cargo.toml b/crates/ros-z/Cargo.toml index f4bdb281..a1e78dfd 100644 --- a/crates/ros-z/Cargo.toml +++ b/crates/ros-z/Cargo.toml @@ -34,6 +34,7 @@ prost = { workspace = true, optional = true } pyo3 = { workspace = true, optional = true } ros-z-cdr = { workspace = true } ros-z-codegen = { workspace = true, optional = true } +ros-z-derive = { path = "../ros-z-derive" } ros-z-protocol = { path = "../ros-z-protocol", default-features = false, features = [ "std", ] } @@ -47,6 +48,7 @@ paste = "1.0" ros-z-msgs = { path = "../ros-z-msgs" } ros-z-codegen = { path = "../ros-z-codegen" } serial_test = "3.0" +trybuild = "1.0" [build-dependencies] serde = { workspace = true, features = ["serde_derive"] } diff --git a/crates/ros-z/examples/demo_nodes/add_two_ints_client.rs b/crates/ros-z/examples/demo_nodes/add_two_ints_client.rs index 742a7318..c451856b 100644 --- a/crates/ros-z/examples/demo_nodes/add_two_ints_client.rs +++ b/crates/ros-z/examples/demo_nodes/add_two_ints_client.rs @@ -34,15 +34,13 @@ pub fn run_add_two_ints_client(ctx: ZContext, a: i64, b: i64, async_mode: bool) // Wait for the response let resp = if async_mode { - tokio::runtime::Runtime::new().unwrap().block_on(async { - client.send_request(&req).await?; - client.async_take_response().await - })? + tokio::runtime::Runtime::new() + .unwrap() + .block_on(async { client.call(&req).await })? } else { tokio::runtime::Runtime::new() .unwrap() - .block_on(async { client.send_request(&req).await })?; - client.take_response_timeout(Duration::from_secs(5))? + .block_on(async { client.call_or_timeout(&req, Duration::from_secs(5)).await })? }; println!("Received response: {}", resp.sum); diff --git a/crates/ros-z/examples/demo_nodes/add_two_ints_server.rs b/crates/ros-z/examples/demo_nodes/add_two_ints_server.rs index c99dcc4d..ac7c9043 100644 --- a/crates/ros-z/examples/demo_nodes/add_two_ints_server.rs +++ b/crates/ros-z/examples/demo_nodes/add_two_ints_server.rs @@ -25,11 +25,15 @@ pub fn run_add_two_ints_server(ctx: ZContext, max_requests: Option) -> Re // ANCHOR: request_loop loop { // Wait for a request - let (key, req) = service.take_request()?; - println!("Incoming request\na: {} b: {}", req.a, req.b); + let req = service.take_request()?; + println!( + "Incoming request\na: {} b: {}", + req.message().a, + req.message().b + ); // Compute the sum - let sum = req.a + req.b; + let sum = req.message().a + req.message().b; // Create the response let resp = AddTwoIntsResponse { sum }; @@ -37,7 +41,7 @@ pub fn run_add_two_ints_server(ctx: ZContext, max_requests: Option) -> Re println!("Sending response: {}", resp.sum); // Send the response - service.send_response(&resp, &key)?; + req.reply_blocking(&resp)?; request_count += 1; diff --git a/crates/ros-z/examples/protobuf_demo/src/lib.rs b/crates/ros-z/examples/protobuf_demo/src/lib.rs index caa6bea3..8e3c901b 100644 --- a/crates/ros-z/examples/protobuf_demo/src/lib.rs +++ b/crates/ros-z/examples/protobuf_demo/src/lib.rs @@ -123,8 +123,9 @@ pub fn run_service_client( // Send request and wait for response using the shared runtime let response = rt.block_on(async { - client.send_request(&request).await?; - client.take_response_timeout(Duration::from_secs(5)) + client + .call_or_timeout(&request, Duration::from_secs(5)) + .await })?; if response.success { @@ -172,47 +173,50 @@ pub fn run_service_server( loop { match server.take_request() { - Ok((key, request)) => { + Ok(request) => { consecutive_errors = 0; // Reset error counter on success count += 1; println!( "[Server] Request {}: {} {} {}", - count, request.a, request.operation, request.b + count, + request.message().a, + request.message().operation, + request.message().b ); - let response = match request.operation.as_str() { + let response = match request.message().operation.as_str() { "add" => types::CalculateResponse { success: true, - result: request.a + request.b, + result: request.message().a + request.message().b, message: format!( "{} + {} = {}", - request.a, - request.b, - request.a + request.b + request.message().a, + request.message().b, + request.message().a + request.message().b ), }, "subtract" => types::CalculateResponse { success: true, - result: request.a - request.b, + result: request.message().a - request.message().b, message: format!( "{} - {} = {}", - request.a, - request.b, - request.a - request.b + request.message().a, + request.message().b, + request.message().a - request.message().b ), }, "multiply" => types::CalculateResponse { success: true, - result: request.a * request.b, + result: request.message().a * request.message().b, message: format!( "{} * {} = {}", - request.a, - request.b, - request.a * request.b + request.message().a, + request.message().b, + request.message().a * request.message().b ), }, "divide" => { - if request.b == 0.0 { + if request.message().b == 0.0 { types::CalculateResponse { success: false, result: 0.0, @@ -221,12 +225,12 @@ pub fn run_service_server( } else { types::CalculateResponse { success: true, - result: request.a / request.b, + result: request.message().a / request.message().b, message: format!( "{} / {} = {}", - request.a, - request.b, - request.a / request.b + request.message().a, + request.message().b, + request.message().a / request.message().b ), } } @@ -234,11 +238,14 @@ pub fn run_service_server( _ => types::CalculateResponse { success: false, result: 0.0, - message: format!("Error: Unknown operation '{}'", request.operation), + message: format!( + "Error: Unknown operation '{}'", + request.message().operation + ), }, }; - if let Err(e) = server.send_response(&response, &key) { + if let Err(e) = request.reply_blocking(&response) { eprintln!("[Server] Failed to send response: {}", e); consecutive_errors += 1; } diff --git a/crates/ros-z/examples/z_custom_message.rs b/crates/ros-z/examples/z_custom_message.rs index cbb99b4e..b00cd243 100644 --- a/crates/ros-z/examples/z_custom_message.rs +++ b/crates/ros-z/examples/z_custom_message.rs @@ -9,7 +9,8 @@ use ros_z::{ use serde::{Deserialize, Serialize}; // Custom message for pub/sub example -#[derive(Debug, Clone, Serialize, Deserialize, Default)] +#[derive(Debug, Clone, Serialize, Deserialize, Default, MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/RobotStatus")] pub struct RobotStatus { pub robot_id: String, pub battery_percentage: f64, @@ -18,66 +19,32 @@ pub struct RobotStatus { pub is_moving: bool, } -impl MessageTypeInfo for RobotStatus { - fn type_name() -> &'static str { - "custom_msgs::msg::dds_::RobotStatus_" - } - - fn type_hash() -> TypeHash { - TypeHash::zero() - } -} - -impl ros_z::WithTypeInfo for RobotStatus {} - impl ros_z::msg::ZMessage for RobotStatus { type Serdes = ros_z::msg::SerdeCdrSerdes; } // Custom service request -#[derive(Debug, Clone, Serialize, Deserialize, Default)] +#[derive(Debug, Clone, Serialize, Deserialize, Default, MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/srv/NavigateTo_Request")] pub struct NavigateToRequest { pub target_x: f64, pub target_y: f64, pub max_speed: f64, } -impl MessageTypeInfo for NavigateToRequest { - fn type_name() -> &'static str { - "custom_msgs::srv::dds_::NavigateTo_Request_" - } - - fn type_hash() -> TypeHash { - TypeHash::zero() - } -} - -impl ros_z::WithTypeInfo for NavigateToRequest {} - impl ros_z::msg::ZMessage for NavigateToRequest { type Serdes = ros_z::msg::SerdeCdrSerdes; } // Custom service response -#[derive(Debug, Clone, Serialize, Deserialize, Default)] +#[derive(Debug, Clone, Serialize, Deserialize, Default, MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/srv/NavigateTo_Response")] pub struct NavigateToResponse { pub success: bool, pub estimated_duration: f64, pub message: String, } -impl MessageTypeInfo for NavigateToResponse { - fn type_name() -> &'static str { - "custom_msgs::srv::dds_::NavigateTo_Response_" - } - - fn type_hash() -> TypeHash { - TypeHash::zero() - } -} - -impl ros_z::WithTypeInfo for NavigateToResponse {} - impl ros_z::msg::ZMessage for NavigateToResponse { type Serdes = ros_z::msg::SerdeCdrSerdes; } @@ -153,9 +120,14 @@ async fn run_status_publisher(robot_id: String) -> Result<()> { println!("Starting robot status publisher for robot: {robot_id}"); let ctx = ZContextBuilder::default().build()?; - let node = ctx.create_node("robot_status_publisher").build()?; + let node = ctx + .create_node("robot_status_publisher") + .with_type_description_service() + .build()?; let zpub = node.create_pub::("/robot_status").build()?; + println!("Type description service enabled for automatic schema registration"); + let mut position_x = 0.0; let mut position_y = 0.0; let mut battery = 100.0; @@ -233,19 +205,23 @@ pub fn run_navigation_server(ctx: ros_z::context::ZContext) -> Result<()> { println!("Navigation server ready, waiting for requests..."); loop { - if let Ok((request_id, request)) = zsrv.take_request() { + if let Ok(request) = zsrv.take_request() { println!( "Received navigation request: target=({:.1}, {:.1}), max_speed={:.1}", - request.target_x, request.target_y, request.max_speed + request.message().target_x, + request.message().target_y, + request.message().max_speed ); // Simulate path planning std::thread::sleep(Duration::from_millis(500)); - let distance = (request.target_x.powi(2) + request.target_y.powi(2)).sqrt(); - let duration = distance / request.max_speed; + let distance = + (request.message().target_x.powi(2) + request.message().target_y.powi(2)).sqrt(); + let duration = distance / request.message().max_speed; - let response = if request.max_speed > 0.0 && request.max_speed < 5.0 { + let response = if request.message().max_speed > 0.0 && request.message().max_speed < 5.0 + { NavigateToResponse { success: true, estimated_duration: duration, @@ -263,7 +239,7 @@ pub fn run_navigation_server(ctx: ros_z::context::ZContext) -> Result<()> { }; println!("Sending response: {:?}", response); - zsrv.send_response(&response, &request_id)?; + request.reply_blocking(&response)?; } std::thread::sleep(Duration::from_millis(100)); @@ -292,22 +268,14 @@ pub fn run_navigation_client( request.target_x, request.target_y, request.max_speed ); - tokio::runtime::Runtime::new() + let response = tokio::runtime::Runtime::new() .unwrap() - .block_on(async { zcli.send_request(&request).await })?; - - println!("Waiting for response..."); + .block_on(async { zcli.call(&request).await })?; - loop { - if let Ok(response) = zcli.take_response() { - println!("Received response:"); - println!("Success: {}", response.success); - println!("Duration: {:.2}s", response.estimated_duration); - println!("Message: {}", response.message); - break; - } - std::thread::sleep(Duration::from_millis(100)); - } + println!("Received response:"); + println!("Success: {}", response.success); + println!("Duration: {:.2}s", response.estimated_duration); + println!("Message: {}", response.message); Ok(()) } diff --git a/crates/ros-z/examples/z_pubsub_ros2dds.rs b/crates/ros-z/examples/z_pubsub_ros2dds.rs index 2429c9b7..482a1008 100644 --- a/crates/ros-z/examples/z_pubsub_ros2dds.rs +++ b/crates/ros-z/examples/z_pubsub_ros2dds.rs @@ -37,7 +37,7 @@ use ros_z::{ entity::{NodeEntity, TypeInfo}, }; use ros_z_cdr::{CdrSerializer, LittleEndian}; -use ros_z_protocol::entity::{EndpointEntity, EntityKind}; +use ros_z_protocol::entity::{EndpointEntity, EndpointKind}; use serde::{Deserialize, Serialize}; use zenoh::{Wait, key_expr::KeyExpr}; @@ -120,13 +120,13 @@ async fn main() -> ros_z::Result<()> { }; let entity_kind = match args.role.as_str() { - "talker" => EntityKind::Publisher, - _ => EntityKind::Subscription, + "talker" => EndpointKind::Publisher, + _ => EndpointKind::Subscription, }; let entity = EndpointEntity { id: 1, - node, + node: Some(node), kind: entity_kind, topic: topic.clone(), type_info: Some(type_info), diff --git a/crates/ros-z/examples/z_srvcli.rs b/crates/ros-z/examples/z_srvcli.rs index 2dda3b2a..95eb6938 100644 --- a/crates/ros-z/examples/z_srvcli.rs +++ b/crates/ros-z/examples/z_srvcli.rs @@ -66,13 +66,19 @@ pub fn run_server(ctx: ZContext) -> Result<()> { println!("AddTwoInts service server started, waiting for requests..."); loop { - let (key, req) = zsrv.take_request()?; - println!("Received request: {} + {}", req.a, req.b); + let req = zsrv.take_request()?; + println!( + "Received request: {} + {}", + req.message().a, + req.message().b + ); - let resp = AddTwoIntsResponse { sum: req.a + req.b }; + let resp = AddTwoIntsResponse { + sum: req.message().a + req.message().b, + }; println!("Sending response: {}", resp.sum); - zsrv.send_response(&resp, &key)?; + req.reply_blocking(&resp)?; } } @@ -85,8 +91,9 @@ pub async fn run_client(ctx: ZContext, a: i64, b: i64) -> Result<()> { let req = AddTwoIntsRequest { a, b }; println!("Sending request: {} + {}", req.a, req.b); - zcli.send_request(&req).await?; - let resp = zcli.take_response_timeout(std::time::Duration::from_secs(5))?; + let resp = zcli + .call_or_timeout(&req, std::time::Duration::from_secs(5)) + .await?; println!("Received response: {}", resp.sum); diff --git a/crates/ros-z/examples/z_srvcli_ros2dds.rs b/crates/ros-z/examples/z_srvcli_ros2dds.rs index 0b557bd3..7cdb4c9a 100644 --- a/crates/ros-z/examples/z_srvcli_ros2dds.rs +++ b/crates/ros-z/examples/z_srvcli_ros2dds.rs @@ -120,13 +120,13 @@ async fn run_server(ctx: ros_z::context::ZContext, args: &Args) -> ros_z::Result loop { // Wait for a request - let (key, req) = service.take_request()?; + let req = service.take_request()?; println!("Incoming request:"); - println!(" a: {}", req.a); - println!(" b: {}", req.b); + println!(" a: {}", req.message().a); + println!(" b: {}", req.message().b); // Compute the sum - let sum = req.a + req.b; + let sum = req.message().a + req.message().b; // Create the response let resp = AddTwoIntsResponse { sum }; @@ -134,7 +134,7 @@ async fn run_server(ctx: ros_z::context::ZContext, args: &Args) -> ros_z::Result println!("Sending response: {}\n", resp.sum); // Send the response - service.send_response(&resp, &key)?; + req.reply_blocking(&resp)?; request_count += 1; @@ -176,11 +176,7 @@ async fn run_client(ctx: ros_z::context::ZContext, args: &Args) -> ros_z::Result println!("Sending request #{}: a={}, b={}", i + 1, req.a, req.b); - // Send request - client.send_request(&req).await?; - - // Wait for response with timeout - match client.take_response_timeout(Duration::from_secs(5)) { + match client.call_or_timeout(&req, Duration::from_secs(5)).await { Ok(resp) => { println!("Received response: {} + {} = {}\n", req.a, req.b, resp.sum); } diff --git a/crates/ros-z/src/action/client.rs b/crates/ros-z/src/action/client.rs index 8d282e7c..872e722f 100644 --- a/crates/ros-z/src/action/client.rs +++ b/crates/ros-z/src/action/client.rs @@ -30,7 +30,7 @@ pub mod goal_state { /// /// # Examples /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # use ros_z::qos::QosProfile; /// # let node = todo!(); @@ -286,7 +286,7 @@ impl<'a, A: ZAction> Builder for ZActionClientBuilder<'a, A> { /// /// # Simple Goal Send/Receive /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # #[tokio::main] /// # async fn main() -> Result<()> { @@ -306,7 +306,7 @@ impl<'a, A: ZAction> Builder for ZActionClientBuilder<'a, A> { /// /// # Feedback Streaming /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # #[tokio::main] /// # async fn main() -> Result<()> { @@ -328,7 +328,7 @@ impl<'a, A: ZAction> Builder for ZActionClientBuilder<'a, A> { /// /// # Cancellation /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # #[tokio::main] /// # async fn main() -> Result<()> { @@ -403,7 +403,7 @@ impl ZActionClient { /// /// # Examples /// - /// ```no_run + /// ```ignore /// # use ros_z::action::*; /// # #[tokio::main] /// # async fn main() -> Result<()> { @@ -437,23 +437,13 @@ impl ZActionClient { // 3. Send goal request via service client let request = SendGoalRequest { goal_id, goal }; tracing::debug!("Sending goal request for goal_id: {:?}", goal_id); - if let Err(e) = self.goal_client.send_request(&request).await { - // Cleanup on send failure - self.goal_board.active_goals.remove(&goal_id); - return Err(e); - } - tracing::debug!("Goal request sent, waiting for response..."); - - // 4. Wait for response - let sample = self.goal_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - tracing::debug!( - "Received goal response payload: {} bytes: {:?}", - payload.len(), - payload - ); - let response = ::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string()))?; + let response = match self.goal_client.call(&request).await { + Ok(response) => response, + Err(error) => { + self.goal_board.active_goals.remove(&goal_id); + return Err(error); + } + }; // 5. Check if accepted if !response.accepted { @@ -476,12 +466,7 @@ impl ZActionClient { let goal_info = GoalInfo::new(goal_id); let request = CancelGoalServiceRequest { goal_info }; - self.cancel_client.send_request(&request).await?; - let sample = self.cancel_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - let response = ::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string()))?; - Ok(response) + self.cancel_client.call(&request).await } pub async fn cancel_all_goals(&self) -> Result { @@ -493,12 +478,7 @@ impl ZActionClient { }; let request = CancelGoalServiceRequest { goal_info }; - self.cancel_client.send_request(&request).await?; - let sample = self.cancel_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - let response = ::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string()))?; - Ok(response) + self.cancel_client.call(&request).await } pub fn feedback_stream(&self, goal_id: GoalId) -> Option> { @@ -523,53 +503,57 @@ impl ZActionClient { pub async fn get_result(&self, goal_id: GoalId) -> Result { let request = GetResultRequest { goal_id }; - self.result_client.send_request(&request).await?; - let sample = self.result_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - let response = as ZMessage>::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string()))?; + let response: GetResultResponse = self.result_client.call(&request).await?; Ok(response.result) } // FIXME: Check the necessity // Low-level methods for testing - pub async fn send_goal_request_low(&self, request: &SendGoalRequest) -> Result<()> { - self.goal_client.send_request(request).await + pub async fn send_goal_request_low( + &self, + request: &SendGoalRequest, + ) -> Result { + self.goal_client.call(request).await } // FIXME: Check the necessity pub async fn recv_goal_response_low(&self) -> Result { - let sample = self.goal_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - ::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string())) + Err(zenoh::Error::from( + "recv_goal_response_low is no longer available; use send_goal_request_low".to_string(), + )) } // FIXME: Check the necessity - pub async fn send_cancel_request_low(&self, request: &CancelGoalServiceRequest) -> Result<()> { - self.cancel_client.send_request(request).await + pub async fn send_cancel_request_low( + &self, + request: &CancelGoalServiceRequest, + ) -> Result { + self.cancel_client.call(request).await } // FIXME: Check the necessity pub async fn recv_cancel_response_low(&self) -> Result { - let sample = self.cancel_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - ::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string())) + Err(zenoh::Error::from( + "recv_cancel_response_low is no longer available; use send_cancel_request_low" + .to_string(), + )) } // FIXME: Check the necessity - pub async fn send_result_request_low(&self, request: &GetResultRequest) -> Result<()> { - self.result_client.send_request(request).await + pub async fn send_result_request_low( + &self, + request: &GetResultRequest, + ) -> Result> { + self.result_client.call(request).await } // FIXME: Check the necessity pub async fn recv_result_response_low(&self) -> Result> { - let sample = self.result_client.rx.recv_async().await?; - let payload = sample.payload().to_bytes(); - as ZMessage>::deserialize(&payload) - .map_err(|e| zenoh::Error::from(e.to_string())) + Err(zenoh::Error::from( + "recv_result_response_low is no longer available; use send_result_request_low" + .to_string(), + )) } } @@ -597,7 +581,7 @@ struct GoalChannels { /// /// # Examples /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # #[tokio::main] /// # async fn main() -> Result<()> { diff --git a/crates/ros-z/src/action/server.rs b/crates/ros-z/src/action/server.rs index 764f7296..9ca31153 100644 --- a/crates/ros-z/src/action/server.rs +++ b/crates/ros-z/src/action/server.rs @@ -123,7 +123,7 @@ impl Drop for ShutdownGuard { /// /// # Examples /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # use std::time::Duration; /// # let node = todo!(); @@ -676,7 +676,7 @@ impl ZActionServer { /// /// # Examples /// - /// ```no_run + /// ```ignore /// # use ros_z::action::*; /// # let server = todo!(); /// let server = server.with_handler(|executing| async move { @@ -717,7 +717,7 @@ impl ZActionServer { /// /// # Examples /// - /// ```no_run + /// ```ignore /// # use ros_z::action::*; /// # let server: ros_z::action::server::ZActionServer = todo!(); /// // Check and expire any goals that have passed their expiration time @@ -771,7 +771,7 @@ impl ZActionServer { /// /// # Examples /// - /// ```no_run + /// ```ignore /// # use ros_z::action::*; /// # use std::time::Duration; /// # let mut server: ros_z::action::server::ZActionServer = todo!(); @@ -827,7 +827,7 @@ pub type ExecutingGoal = GoalHandle; /// /// # Examples /// -/// ```no_run +/// ```ignore /// # use ros_z::action::*; /// # let server: std::sync::Arc> = todo!(); /// # async { diff --git a/crates/ros-z/src/config.rs b/crates/ros-z/src/config.rs index afe938c8..bb9dccb8 100644 --- a/crates/ros-z/src/config.rs +++ b/crates/ros-z/src/config.rs @@ -10,7 +10,10 @@ //! - Session-specific: 6 settings unique to peer mode //! //! # Example -//! ```rust +//! ```no_run +//! # use ros_z::config::{RouterConfigBuilder, router_config, session_config}; +//! # #[tokio::main] +//! # async fn main() -> zenoh::Result<()> { //! // Create router config //! let router_cfg = router_config()?; //! let router = zenoh::open(router_cfg).await?; @@ -23,6 +26,8 @@ //! let custom_router = RouterConfigBuilder::new() //! .with_listen_port(7448) //! .build_config()?; +//! # Ok(()) +//! # } //! ``` use serde::{Deserialize, Serialize}; @@ -286,9 +291,14 @@ fn build_config(overrides: &[ConfigOverride]) -> zenoh::Result { /// Create a router configuration matching rmw_zenoh_cpp defaults /// /// # Example -/// ```rust +/// ```no_run +/// # use ros_z::config::router_config; +/// # #[tokio::main] +/// # async fn main() -> zenoh::Result<()> { /// let config = router_config()?; /// let router = zenoh::open(config).await?; +/// # Ok(()) +/// # } /// ``` pub fn router_config() -> zenoh::Result { build_config(&router_overrides()) @@ -297,9 +307,14 @@ pub fn router_config() -> zenoh::Result { /// Create a session configuration matching rmw_zenoh_cpp defaults /// /// # Example -/// ```rust +/// ```no_run +/// # use ros_z::config::session_config; +/// # #[tokio::main] +/// # async fn main() -> zenoh::Result<()> { /// let config = session_config()?; /// let session = zenoh::open(config).await?; +/// # Ok(()) +/// # } /// ``` pub fn session_config() -> zenoh::Result { build_config(&session_overrides()) @@ -313,8 +328,10 @@ pub fn session_config() -> zenoh::Result { /// /// # Example /// ```rust +/// # use ros_z::config::{generate_json5, router_overrides}; /// let json5 = generate_json5(&router_overrides(), "Router Config"); /// std::fs::write("router_config.json5", json5)?; +/// # Ok::<(), std::io::Error>(()) /// ``` pub fn generate_json5(overrides: &[ConfigOverride], name: &str) -> String { use serde_json::Value as JsonValue; @@ -470,9 +487,11 @@ impl RouterConfigBuilder { /// /// # Example /// ```rust + /// # use ros_z::config::RouterConfigBuilder; /// let config = RouterConfigBuilder::new() /// .with_listen_port(7448) /// .build_config()?; + /// # Ok::<(), zenoh::Error>(()) /// ``` pub fn with_listen_port(mut self, port: u16) -> Self { if let Some(listen) = self @@ -489,9 +508,11 @@ impl RouterConfigBuilder { /// /// # Example /// ```rust + /// # use ros_z::config::RouterConfigBuilder; /// let config = RouterConfigBuilder::new() /// .with_listen_endpoint("tcp/0.0.0.0:7447") /// .build_config()?; + /// # Ok::<(), zenoh::Error>(()) /// ``` pub fn with_listen_endpoint(mut self, endpoint: &str) -> Self { if let Some(listen) = self @@ -510,6 +531,7 @@ impl RouterConfigBuilder { /// /// # Example /// ```rust + /// # use ros_z::config::RouterConfigBuilder; /// let config = RouterConfigBuilder::new() /// .with_override( /// "transport/unicast/max_sessions", @@ -517,6 +539,7 @@ impl RouterConfigBuilder { /// "Custom increased sessions" /// ) /// .build_config()?; + /// # Ok::<(), zenoh::Error>(()) /// ``` pub fn with_override(mut self, key: &'static str, value: Value, reason: &'static str) -> Self { if let Some(existing) = self.overrides.iter_mut().find(|o| o.key == key) { @@ -560,9 +583,11 @@ impl SessionConfigBuilder { /// /// # Example /// ```rust + /// # use ros_z::config::SessionConfigBuilder; /// let config = SessionConfigBuilder::new() /// .with_router_endpoint("tcp/192.168.1.100:7447") /// .build_config()?; + /// # Ok::<(), zenoh::Error>(()) /// ``` pub fn with_router_endpoint(mut self, endpoint: &str) -> Self { if let Some(connect) = self @@ -581,6 +606,7 @@ impl SessionConfigBuilder { /// /// # Example /// ```rust + /// # use ros_z::config::SessionConfigBuilder; /// let config = SessionConfigBuilder::new() /// .with_override( /// "queries_default_timeout", @@ -588,6 +614,7 @@ impl SessionConfigBuilder { /// "Increased timeout for slow network" /// ) /// .build_config()?; + /// # Ok::<(), zenoh::Error>(()) /// ``` pub fn with_override(mut self, key: &'static str, value: Value, reason: &'static str) -> Self { if let Some(existing) = self.overrides.iter_mut().find(|o| o.key == key) { diff --git a/crates/ros-z/src/context.rs b/crates/ros-z/src/context.rs index 0e177dba..3c1548d9 100644 --- a/crates/ros-z/src/context.rs +++ b/crates/ros-z/src/context.rs @@ -98,7 +98,7 @@ impl ZContextBuilder { /// Set the key expression format for ROS 2 entity mapping. /// /// # Example - /// ``` + /// ```ignore /// use ros_z::context::ZContextBuilder; /// use ros_z::Builder; /// use ros_z_protocol::KeyExprFormat; @@ -231,10 +231,12 @@ impl ZContextBuilder { /// use ros_z::context::ZContextBuilder; /// use ros_z::Builder; /// + /// # fn main() -> zenoh::Result<()> { /// let ctx = ZContextBuilder::default() - /// .with_router_endpoint("tcp/192.168.1.1:7447") - /// .build() - /// .expect("Failed to build context"); + /// .with_router_endpoint("tcp/192.168.1.1:7447")? + /// .build()?; + /// # Ok(()) + /// # } /// ``` pub fn with_router_endpoint>(mut self, endpoint: S) -> Result { let session_config = crate::config::SessionConfigBuilder::new() @@ -617,6 +619,7 @@ impl ZContext { keyexpr_format: self.keyexpr_format, clock: self.clock.clone(), enable_type_desc_service: false, + enable_extended_type_desc_service: false, enable_parameters: true, parameter_overrides: std::collections::HashMap::new(), } diff --git a/crates/ros-z/src/dynamic/discovery.rs b/crates/ros-z/src/dynamic/discovery.rs new file mode 100644 index 00000000..78877448 --- /dev/null +++ b/crates/ros-z/src/dynamic/discovery.rs @@ -0,0 +1,191 @@ +use std::time::Duration; +use std::{collections::BTreeSet, sync::Arc}; + +use crate::{ + dynamic::{DynamicError, MessageSchema}, + entity::{Entity, EntityKind}, + graph::Graph, + node::ZNode, + topic_name::qualify_topic_name, +}; + +use super::type_info::{ros_type_name_from_dds, schema_type_info_with_hash}; + +#[derive(Debug, Clone)] +pub struct DiscoveredTopicSchema { + pub qualified_topic: String, + pub schema: Arc, + pub type_hash: String, +} + +#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] +pub(crate) struct TopicSchemaCandidate { + pub node_name: String, + pub namespace: String, + pub type_name: String, + pub type_hash: String, +} + +pub(crate) fn collect_topic_schema_candidates_from_publishers( + publishers: &[Arc], + qualified_topic: &str, +) -> Result, DynamicError> { + let mut saw_missing_node_identity = false; + let mut saw_missing_type_info = false; + let mut candidates = BTreeSet::new(); + + for publisher in publishers { + let Entity::Endpoint(endpoint) = &**publisher else { + continue; + }; + let Some(node) = endpoint.node.as_ref() else { + saw_missing_node_identity = true; + continue; + }; + let Some(type_info) = endpoint.type_info.as_ref() else { + saw_missing_type_info = true; + continue; + }; + + candidates.insert(TopicSchemaCandidate { + node_name: node.name.clone(), + namespace: node.namespace.clone(), + type_name: ros_type_name_from_dds(&type_info.name), + type_hash: type_info.hash.to_rihs_string(), + }); + } + + if !candidates.is_empty() { + return Ok(candidates.into_iter().collect()); + } + + if saw_missing_node_identity { + return Err(DynamicError::MissingNodeIdentity { + topic: qualified_topic.to_string(), + }); + } + + if saw_missing_type_info { + return Err(DynamicError::SchemaNotFound(format!( + "No publishers with type information found for topic: {}", + qualified_topic + ))); + } + + Err(DynamicError::SchemaNotFound(format!( + "No usable publishers found for topic: {}", + qualified_topic + ))) +} + +pub(crate) fn collect_topic_schema_candidates( + graph: &Graph, + qualified_topic: &str, +) -> Result, DynamicError> { + let publishers = graph.get_entities_by_topic(EntityKind::Publisher, qualified_topic); + if publishers.is_empty() { + return Err(DynamicError::SchemaNotFound(format!( + "No publishers found for topic: {}", + qualified_topic + ))); + } + + collect_topic_schema_candidates_from_publishers(&publishers, qualified_topic) +} + +pub(crate) struct SchemaDiscovery<'a> { + node: &'a ZNode, + timeout: Duration, +} + +impl<'a> SchemaDiscovery<'a> { + pub(crate) fn new(node: &'a ZNode, timeout: Duration) -> Self { + Self { node, timeout } + } + + pub(crate) async fn discover( + &self, + topic: &str, + ) -> Result { + let qualified_topic = qualify_topic_name(topic, self.node.namespace(), self.node.name()) + .map_err(|error| { + DynamicError::SchemaNotFound(format!("Failed to qualify topic: {error}")) + })?; + let candidates = + collect_topic_schema_candidates(self.node.graph().as_ref(), &qualified_topic)?; + + let (schema, type_hash) = match self.try_standard(&candidates[..]).await { + Ok(result) => result, + Err(standard_error) => match self.try_extended(&candidates[..]).await { + Ok(result) => result, + Err(extended_error) => { + return Err(DynamicError::SchemaNotFound(format!( + "Schema discovery failed. Standard: {}. Extended: {}", + standard_error, extended_error + ))); + } + }, + }; + + Ok(DiscoveredTopicSchema { + qualified_topic, + schema, + type_hash, + }) + } + + async fn try_standard( + &self, + candidates: &[TopicSchemaCandidate], + ) -> Result<(Arc, String), DynamicError> { + let mut last_error = None; + + for candidate in candidates { + match super::type_description_query::query_type_description( + self.node, + candidate, + self.timeout, + false, + ) + .await + { + Ok(result) => return Ok(result), + Err(error) => last_error = Some(error), + } + } + + Err(last_error.unwrap_or_else(|| { + DynamicError::SchemaNotFound("No standard schema source succeeded".to_string()) + })) + } + + async fn try_extended( + &self, + candidates: &[TopicSchemaCandidate], + ) -> Result<(Arc, String), DynamicError> { + let mut last_error = None; + + for candidate in candidates { + match crate::extended_type_description_query::query_extended_type_description( + self.node, + candidate, + self.timeout, + ) + .await + { + Ok(result) => return Ok(result), + Err(error) => last_error = Some(error), + } + } + + Err(last_error.unwrap_or_else(|| { + DynamicError::SchemaNotFound("No extended schema source succeeded".to_string()) + })) + } +} + +pub(crate) fn discovered_schema_type_info( + discovered: &DiscoveredTopicSchema, +) -> crate::entity::TypeInfo { + schema_type_info_with_hash(&discovered.schema, &discovered.type_hash) +} diff --git a/crates/ros-z/src/dynamic/error.rs b/crates/ros-z/src/dynamic/error.rs index 80175412..4cce1415 100644 --- a/crates/ros-z/src/dynamic/error.rs +++ b/crates/ros-z/src/dynamic/error.rs @@ -41,6 +41,9 @@ pub enum DynamicError { /// Type description service call timed out — no response from the remote node. ServiceTimeout { node: String, service: String }, + /// Automatic topic-based schema discovery requires publisher node identity. + MissingNodeIdentity { topic: String }, + /// Invalid default value for field type InvalidDefaultValue { field: String, reason: String }, @@ -107,6 +110,13 @@ impl fmt::Display for DynamicError { node, service ) } + DynamicError::MissingNodeIdentity { topic } => { + write!( + f, + "automatic schema discovery for topic '{}' requires publisher node identity, which is unavailable from this backend/discovery format", + topic + ) + } DynamicError::InvalidDefaultValue { field, reason } => { write!(f, "Invalid default value for field '{}': {}", field, reason) } diff --git a/crates/ros-z/src/dynamic/mod.rs b/crates/ros-z/src/dynamic/mod.rs index 2e028604..c6fdf845 100644 --- a/crates/ros-z/src/dynamic/mod.rs +++ b/crates/ros-z/src/dynamic/mod.rs @@ -54,6 +54,7 @@ //! assert_eq!(decoded.get::("x")?, 1.0); //! ``` +pub(crate) mod discovery; pub mod error; pub mod message; pub mod registry; @@ -61,29 +62,39 @@ pub mod schema; pub mod serdes; pub mod serialization; pub mod type_description; -pub mod type_description_client; +pub mod type_description_query; pub mod type_description_service; +pub(crate) mod type_info; pub mod value; #[cfg(test)] mod tests; // Re-export main types +pub use discovery::DiscoveredTopicSchema; pub use error::DynamicError; pub use message::{DynamicMessage, DynamicMessageBuilder}; pub use registry::{SchemaRegistry, get_schema, has_schema, register_schema}; -pub use schema::{FieldSchema, FieldType, MessageSchema, MessageSchemaBuilder}; +pub use schema::{ + EnumPayloadSchema, EnumSchema, EnumVariantSchema, FieldSchema, FieldType, MessageSchema, + MessageSchemaBuilder, +}; pub use serdes::DynamicSerdeCdrSerdes; pub use serialization::SerializationFormat; pub use type_description::{MessageSchemaTypeDescription, type_description_msg_to_schema}; -pub use type_description_client::TypeDescriptionClient; +pub use type_description_query::schema_from_type_description_response; pub use type_description_service::{ GetTypeDescription, GetTypeDescriptionRequest, GetTypeDescriptionResponse, RegisteredSchema, TypeDescriptionService, TypeSource, WireField, WireFieldType, WireIndividualTypeDescription, WireKeyValue, WireTypeDescription, WireTypeSource, schema_to_wire_type_description, wire_to_schema_type_description, }; -pub use value::{DynamicValue, FromDynamic, IntoDynamic}; +pub use value::{ + DynamicNamedValue, DynamicValue, EnumPayloadValue, EnumValue, FromDynamic, IntoDynamic, +}; + +pub(crate) use discovery::{SchemaDiscovery, discovered_schema_type_info}; +pub(crate) use type_info::schema_type_info; use zenoh::sample::Sample; diff --git a/crates/ros-z/src/dynamic/schema.rs b/crates/ros-z/src/dynamic/schema.rs index ecef2af3..19ccab57 100644 --- a/crates/ros-z/src/dynamic/schema.rs +++ b/crates/ros-z/src/dynamic/schema.rs @@ -32,6 +32,10 @@ pub enum FieldType { // Compound /// Nested message type Message(Arc), + /// Optional value using serde/ros-z-cdr's `u32` presence tag encoding. + Optional(Box), + /// Tagged enum using serde/ros-z-cdr's `u32` variant index encoding. + Enum(Arc), // Collections /// Fixed-size array: `T[N]` @@ -52,6 +56,7 @@ impl FieldType { FieldType::Int64 | FieldType::Uint64 | FieldType::Float64 => Some(8), FieldType::Array(inner, len) => inner.fixed_size().map(|s| s * len), FieldType::Message(schema) => schema.fixed_cdr_size(), + FieldType::Optional(_) | FieldType::Enum(_) => None, // String, Sequence types are variable _ => None, } @@ -67,6 +72,7 @@ impl FieldType { FieldType::String | FieldType::BoundedString(_) => 4, // length prefix FieldType::Array(inner, _) => inner.alignment(), FieldType::Sequence(_) | FieldType::BoundedSequence(_, _) => 4, // length prefix + FieldType::Optional(_) | FieldType::Enum(_) => 4, FieldType::Message(schema) => schema.alignment(), } } @@ -113,14 +119,27 @@ impl FieldType { match self { FieldType::Array(inner, _) | FieldType::Sequence(inner) - | FieldType::BoundedSequence(inner, _) => Some(inner), + | FieldType::BoundedSequence(inner, _) + | FieldType::Optional(inner) => Some(inner), _ => None, } } + + /// Check whether this type uses ros-z extended schema features. + pub fn is_extended(&self) -> bool { + match self { + FieldType::Optional(_) | FieldType::Enum(_) => true, + FieldType::Message(schema) => schema.uses_extended_types(), + FieldType::Array(inner, _) + | FieldType::Sequence(inner) + | FieldType::BoundedSequence(inner, _) => inner.is_extended(), + _ => false, + } + } } /// Schema for a single message field. -#[derive(Clone, Debug)] +#[derive(Clone, Debug, PartialEq)] pub struct FieldSchema { /// Field name pub name: String, @@ -147,6 +166,79 @@ impl FieldSchema { } } +/// Schema for a serde enum. +#[derive(Clone, Debug, PartialEq)] +pub struct EnumSchema { + /// Canonical enum type name, usually matching the owning Rust type. + pub type_name: String, + /// Ordered list of enum variants in serde discriminant order. + pub variants: Vec, +} + +impl EnumSchema { + /// Create a new enum schema. + pub fn new(type_name: impl Into, variants: Vec) -> Self { + Self { + type_name: type_name.into(), + variants, + } + } + + /// Returns true if any payload uses ros-z extended schema features. + pub fn uses_extended_types(&self) -> bool { + self.variants + .iter() + .any(EnumVariantSchema::uses_extended_types) + } +} + +/// Schema for a single enum variant. +#[derive(Clone, Debug, PartialEq)] +pub struct EnumVariantSchema { + /// Variant name as exposed by serde. + pub name: String, + /// Payload shape for the variant. + pub payload: EnumPayloadSchema, +} + +impl EnumVariantSchema { + /// Create a new enum variant schema. + pub fn new(name: impl Into, payload: EnumPayloadSchema) -> Self { + Self { + name: name.into(), + payload, + } + } + + /// Returns true if the payload uses ros-z extended schema features. + pub fn uses_extended_types(&self) -> bool { + self.payload.uses_extended_types() + } +} + +/// Payload schema for a serde enum variant. +#[derive(Clone, Debug, PartialEq)] +pub enum EnumPayloadSchema { + Unit, + Newtype(Box), + Tuple(Vec), + Struct(Vec), +} + +impl EnumPayloadSchema { + /// Returns true if the payload uses ros-z extended schema features. + pub fn uses_extended_types(&self) -> bool { + match self { + EnumPayloadSchema::Unit => false, + EnumPayloadSchema::Newtype(field_type) => field_type.is_extended(), + EnumPayloadSchema::Tuple(field_types) => field_types.iter().any(FieldType::is_extended), + EnumPayloadSchema::Struct(fields) => { + fields.iter().any(|field| field.field_type.is_extended()) + } + } + } +} + /// Complete schema for a ROS 2 message type. #[derive(Clone, Debug)] pub struct MessageSchema { @@ -230,6 +322,13 @@ impl MessageSchema { pub fn field_names(&self) -> impl Iterator { self.fields.iter().map(|f| f.name.as_str()) } + + /// Returns true if any field relies on ros-z extended schema features. + pub fn uses_extended_types(&self) -> bool { + self.fields + .iter() + .any(|field| field.field_type.is_extended()) + } } impl PartialEq for MessageSchema { diff --git a/crates/ros-z/src/dynamic/serialization/cdr.rs b/crates/ros-z/src/dynamic/serialization/cdr.rs index 50529db4..e8005955 100644 --- a/crates/ros-z/src/dynamic/serialization/cdr.rs +++ b/crates/ros-z/src/dynamic/serialization/cdr.rs @@ -10,8 +10,8 @@ use zenoh_buffers::ZBuf; use crate::dynamic::error::DynamicError; use crate::dynamic::message::DynamicMessage; -use crate::dynamic::schema::{FieldType, MessageSchema}; -use crate::dynamic::value::DynamicValue; +use crate::dynamic::schema::{EnumPayloadSchema, EnumSchema, FieldType, MessageSchema}; +use crate::dynamic::value::{DynamicNamedValue, DynamicValue, EnumPayloadValue, EnumValue}; use super::CDR_HEADER_LE; @@ -86,6 +86,14 @@ fn serialize_value( (DynamicValue::Float64(v), FieldType::Float64) => writer.write_f64(*v), (DynamicValue::String(v), FieldType::String) => writer.write_string(v), (DynamicValue::String(v), FieldType::BoundedString(_)) => writer.write_string(v), + (DynamicValue::Optional(None), FieldType::Optional(_)) => writer.write_u32(0), + (DynamicValue::Optional(Some(inner)), FieldType::Optional(inner_type)) => { + writer.write_u32(1); + serialize_value(inner, inner_type, writer)?; + } + (DynamicValue::Enum(enum_value), FieldType::Enum(schema)) => { + serialize_enum_value(enum_value, schema, writer)?; + } // Fixed-size array (no length prefix) (DynamicValue::Array(values), FieldType::Array(inner, _len)) => { @@ -175,6 +183,19 @@ fn deserialize_value( FieldType::String | FieldType::BoundedString(_) => Ok(DynamicValue::String( reader.read_string().map_err(map_cdr_err)?, )), + FieldType::Optional(inner) => { + let tag = reader.read_u32().map_err(map_cdr_err)?; + match tag { + 0 => Ok(DynamicValue::Optional(None)), + 1 => Ok(DynamicValue::Optional(Some(Box::new(deserialize_value( + inner, reader, + )?)))), + other => Err(DynamicError::DeserializationError(format!( + "invalid option discriminant: {other}" + ))), + } + } + FieldType::Enum(schema) => Ok(DynamicValue::Enum(deserialize_enum_value(schema, reader)?)), // Fixed-size array FieldType::Array(inner, len) => { @@ -225,6 +246,131 @@ fn deserialize_value( } } +fn serialize_enum_value( + value: &EnumValue, + schema: &Arc, + writer: &mut CdrWriter, +) -> Result<(), DynamicError> { + let variant = schema + .variants + .get(value.variant_index as usize) + .ok_or_else(|| { + DynamicError::SerializationError(format!( + "enum variant index {} is out of bounds for {}", + value.variant_index, schema.type_name + )) + })?; + + if variant.name != value.variant_name { + return Err(DynamicError::SerializationError(format!( + "enum variant name mismatch for {}: schema={}, value={}", + schema.type_name, variant.name, value.variant_name + ))); + } + + writer.write_u32(value.variant_index); + serialize_enum_payload(&value.payload, &variant.payload, writer) +} + +fn serialize_enum_payload( + payload: &EnumPayloadValue, + schema: &EnumPayloadSchema, + writer: &mut CdrWriter, +) -> Result<(), DynamicError> { + match (payload, schema) { + (EnumPayloadValue::Unit, EnumPayloadSchema::Unit) => Ok(()), + (EnumPayloadValue::Newtype(value), EnumPayloadSchema::Newtype(field_type)) => { + serialize_value(value, field_type, writer) + } + (EnumPayloadValue::Tuple(values), EnumPayloadSchema::Tuple(field_types)) => { + if values.len() != field_types.len() { + return Err(DynamicError::SerializationError(format!( + "enum tuple payload length mismatch: expected {}, got {}", + field_types.len(), + values.len() + ))); + } + + for (value, field_type) in values.iter().zip(field_types.iter()) { + serialize_value(value, field_type, writer)?; + } + Ok(()) + } + (EnumPayloadValue::Struct(values), EnumPayloadSchema::Struct(fields)) => { + if values.len() != fields.len() { + return Err(DynamicError::SerializationError(format!( + "enum struct payload length mismatch: expected {}, got {}", + fields.len(), + values.len() + ))); + } + + for (value, field) in values.iter().zip(fields.iter()) { + if value.name != field.name { + return Err(DynamicError::SerializationError(format!( + "enum struct payload field mismatch: expected {}, got {}", + field.name, value.name + ))); + } + serialize_value(&value.value, &field.field_type, writer)?; + } + Ok(()) + } + _ => Err(DynamicError::SerializationError(format!( + "enum payload mismatch: payload={payload:?}, schema={schema:?}" + ))), + } +} + +fn deserialize_enum_value( + schema: &Arc, + reader: &mut CdrReader, +) -> Result { + let variant_index = reader.read_u32().map_err(map_cdr_err)?; + let variant = schema.variants.get(variant_index as usize).ok_or_else(|| { + DynamicError::DeserializationError(format!( + "enum variant index {} is out of bounds for {}", + variant_index, schema.type_name + )) + })?; + + let payload = deserialize_enum_payload(&variant.payload, reader)?; + Ok(EnumValue { + variant_index, + variant_name: variant.name.clone(), + payload, + }) +} + +fn deserialize_enum_payload( + schema: &EnumPayloadSchema, + reader: &mut CdrReader, +) -> Result { + match schema { + EnumPayloadSchema::Unit => Ok(EnumPayloadValue::Unit), + EnumPayloadSchema::Newtype(field_type) => Ok(EnumPayloadValue::Newtype(Box::new( + deserialize_value(field_type, reader)?, + ))), + EnumPayloadSchema::Tuple(field_types) => Ok(EnumPayloadValue::Tuple( + field_types + .iter() + .map(|field_type| deserialize_value(field_type, reader)) + .collect::, _>>()?, + )), + EnumPayloadSchema::Struct(fields) => Ok(EnumPayloadValue::Struct( + fields + .iter() + .map(|field| { + Ok(DynamicNamedValue { + name: field.name.clone(), + value: deserialize_value(&field.field_type, reader)?, + }) + }) + .collect::, DynamicError>>()?, + )), + } +} + /// Map ros-z-cdr errors to DynamicError. fn map_cdr_err(e: ros_z_cdr::Error) -> DynamicError { DynamicError::DeserializationError(e.to_string()) diff --git a/crates/ros-z/src/dynamic/tests/pubsub_tests.rs b/crates/ros-z/src/dynamic/tests/pubsub_tests.rs index 5b970ae5..bf2d39ac 100644 --- a/crates/ros-z/src/dynamic/tests/pubsub_tests.rs +++ b/crates/ros-z/src/dynamic/tests/pubsub_tests.rs @@ -166,7 +166,14 @@ fn test_zpub_builder_with_dyn_schema() { let session = zenoh::Wait::wait(zenoh::open(zenoh::Config::default())).unwrap(); let graph = std::sync::Arc::new(crate::graph::Graph::new(&session, 0).unwrap()); let builder: ZPubBuilder = ZPubBuilder { - entity: Default::default(), + entity: crate::entity::EndpointEntity { + id: 0, + node: None, + kind: crate::entity::EndpointKind::Publisher, + topic: String::new(), + type_info: None, + qos: ros_z_protocol::qos::QosProfile::default(), + }, session: std::sync::Arc::new(session), graph, clock: crate::time::ZClock::default(), @@ -199,7 +206,14 @@ fn test_zpub_builder_with_serdes_preserves_schema() { let session = zenoh::Wait::wait(zenoh::open(zenoh::Config::default())).unwrap(); let graph = std::sync::Arc::new(crate::graph::Graph::new(&session, 0).unwrap()); let builder: ZPubBuilder = ZPubBuilder { - entity: Default::default(), + entity: crate::entity::EndpointEntity { + id: 0, + node: None, + kind: crate::entity::EndpointKind::Publisher, + topic: String::new(), + type_info: None, + qos: ros_z_protocol::qos::QosProfile::default(), + }, session: std::sync::Arc::new(session), graph, clock: crate::time::ZClock::default(), diff --git a/crates/ros-z/src/dynamic/type_description.rs b/crates/ros-z/src/dynamic/type_description.rs index 0ee010ba..483a9813 100644 --- a/crates/ros-z/src/dynamic/type_description.rs +++ b/crates/ros-z/src/dynamic/type_description.rs @@ -66,6 +66,13 @@ impl MessageSchemaTypeDescription for MessageSchema { } fn to_type_description(&self) -> Result { + if self.uses_extended_types() { + return Err(DynamicError::SerializationError( + "optional and enum fields cannot be represented as standard ROS 2 type descriptions" + .to_string(), + )); + } + let fields = self .fields .iter() @@ -120,6 +127,7 @@ fn collect_field_type_references( | FieldType::BoundedSequence(inner, _) => { collect_field_type_references(inner, referenced, visited)?; } + FieldType::Optional(_) | FieldType::Enum(_) => {} _ => {} // Primitives have no references } Ok(()) @@ -164,6 +172,10 @@ fn field_type_to_description(field_type: &FieldType) -> Result Err(DynamicError::SerializationError( + "optional and enum fields are only available through ros-z extended schema discovery" + .to_string(), + )), // Fixed-size array FieldType::Array(inner, size) => { @@ -227,6 +239,10 @@ fn get_base_type_id(field_type: &FieldType) -> Result { FieldType::Float64 => Ok(TypeId::FLOAT64), FieldType::String | FieldType::BoundedString(_) => Ok(TypeId::STRING), FieldType::Message(_) => Ok(TypeId::NESTED_TYPE), + FieldType::Optional(_) | FieldType::Enum(_) => Err(DynamicError::SerializationError( + "optional and enum fields are only available through ros-z extended schema discovery" + .to_string(), + )), // For nested arrays/sequences, we get the innermost type FieldType::Array(inner, _) | FieldType::Sequence(inner) diff --git a/crates/ros-z/src/dynamic/type_description_client.rs b/crates/ros-z/src/dynamic/type_description_client.rs deleted file mode 100644 index 3779bcea..00000000 --- a/crates/ros-z/src/dynamic/type_description_client.rs +++ /dev/null @@ -1,609 +0,0 @@ -//! Type Description Client implementation. -//! -//! This module provides the `TypeDescriptionClient` for querying type descriptions -//! from remote nodes. It enables dynamic schema discovery by calling the -//! `GetTypeDescription` service on publisher nodes. -//! -//! # Architecture -//! -//! ```text -//! ┌──────────────────────────────────────────────────────────────┐ -//! │ TypeDescriptionClient │ -//! ├──────────────────────────────────────────────────────────────┤ -//! │ Methods: │ -//! │ ├── get_type_description(node, type_name) -> Response │ -//! │ ├── get_type_description_for_topic(topic) -> Schema │ -//! │ └── response_to_schema(response) -> MessageSchema │ -//! └──────────────────────────────────────────────────────────────┘ -//! │ -//! ▼ -//! ┌──────────────────────────────────────────────────────────────┐ -//! │ ZClient │ -//! │ (Zenoh Querier) │ -//! └──────────────────────────────────────────────────────────────┘ -//! ``` -//! -//! # Example -//! -//! ```rust,ignore -//! use ros_z::dynamic::TypeDescriptionClient; -//! -//! // Create client -//! let client = TypeDescriptionClient::new(node); -//! -//! // Query type description from a specific node -//! let response = client.get_type_description( -//! "talker", // node name -//! "", // namespace (root) -//! "std_msgs/msg/String", -//! "", // empty hash = no validation -//! false, // don't include sources -//! ).await?; -//! -//! // Convert to schema -//! let schema = TypeDescriptionClient::response_to_schema(&response)?; -//! ``` - -use std::sync::Arc; -use std::time::Duration; - -use tracing::{debug, warn}; -use zenoh::Session; - -use crate::context::GlobalCounter; -use crate::entity::{EndpointEntity, Entity, EntityKind, NodeEntity}; -use crate::graph::Graph; -use crate::service::{ZClient, ZClientBuilder}; -use crate::{Builder, ServiceTypeInfo}; - -use super::error::DynamicError; -use super::schema::MessageSchema; - -/// Normalize DDS type name to ROS 2 canonical format. -/// -/// Converts "std_msgs::msg::dds_::String_" to "std_msgs/msg/String" -fn normalize_type_name(dds_name: &str) -> String { - dds_name - .replace("::msg::dds_::", "/msg/") - .replace("::srv::dds_::", "/srv/") - .replace("::action::dds_::", "/action/") - .trim_end_matches('_') - .to_string() -} -use super::type_description::type_description_msg_to_schema; -use super::type_description_service::{ - GetTypeDescription, GetTypeDescriptionRequest, GetTypeDescriptionResponse, - wire_to_schema_type_description, -}; - -/// Client for querying type descriptions from remote nodes. -/// -/// This client enables dynamic schema discovery by: -/// - Querying specific nodes for their registered type descriptions -/// - Discovering publishers on a topic and querying their type descriptions -/// - Converting wire format responses to usable `MessageSchema` objects -pub struct TypeDescriptionClient { - session: Arc, - counter: Arc, - graph: Option>, - /// Default timeout for service calls - timeout: Duration, -} - -impl TypeDescriptionClient { - /// Create a new TypeDescriptionClient. - /// - /// # Arguments - /// - /// * `session` - The Zenoh session to use - /// * `counter` - Global counter for entity IDs - /// - /// # Returns - /// - /// A new `TypeDescriptionClient` instance. - pub fn new(session: Arc, counter: Arc) -> Self { - Self { - session, - counter, - graph: None, - timeout: Duration::from_secs(10), - } - } - - /// Create a TypeDescriptionClient with graph access for topic-based discovery. - /// - /// # Arguments - /// - /// * `session` - The Zenoh session to use - /// * `counter` - Global counter for entity IDs - /// * `graph` - The graph for entity discovery - pub fn with_graph( - session: Arc, - counter: Arc, - graph: Arc, - ) -> Self { - Self { - session, - counter, - graph: Some(graph), - timeout: Duration::from_secs(10), - } - } - - /// Set the default timeout for service calls. - pub fn with_timeout(mut self, timeout: Duration) -> Self { - self.timeout = timeout; - self - } - - /// Query type description from a specific node. - /// - /// This method creates a service client for the target node's - /// `get_type_description` service and queries for the specified type. - /// - /// # Arguments - /// - /// * `node_name` - Name of the target node - /// * `namespace` - Namespace of the target node (empty string for root namespace) - /// * `type_name` - Fully qualified type name (e.g., "std_msgs/msg/String") - /// * `type_hash` - Expected type hash for validation (empty string to skip) - /// * `include_sources` - Whether to include source files in the response - /// - /// # Returns - /// - /// The `GetTypeDescriptionResponse` from the remote node. - pub async fn get_type_description( - &self, - node_name: &str, - namespace: &str, - type_name: &str, - type_hash: &str, - include_sources: bool, - ) -> Result { - debug!( - "[TDC] Querying type description: node={}/{}, type={}", - namespace, node_name, type_name - ); - - // Build the absolute service name for this node. - let service_name = if namespace.is_empty() || namespace == "/" { - format!("/{}/get_type_description", node_name) - } else { - format!("{}/{}/get_type_description", namespace, node_name) - }; - - debug!( - "[TDC] Creating client for absolute service: {}", - service_name - ); - - // Use empty namespace since we're using an absolute service name (starts with /) - let client = self.create_client(&service_name, "")?; - - self.query_with_client( - &client, - node_name, - namespace, - type_name, - type_hash, - include_sources, - ) - .await - } - - /// Send a type description request via an already-built client and wait for the response. - /// - /// Separating client creation from the actual query allows callers to hoist Querier - /// creation out of tight loops, mirroring rmw_zenoh_cpp where the Querier lives for - /// the lifetime of the service client rather than being freshly created per request. - async fn query_with_client( - &self, - client: &ZClient, - node_name: &str, - namespace: &str, - type_name: &str, - type_hash: &str, - include_sources: bool, - ) -> Result { - let request = GetTypeDescriptionRequest { - type_name: type_name.to_string(), - type_hash: type_hash.to_string(), - include_type_sources: include_sources, - }; - - debug!( - "[TDC] Sending request to get_type_description: node={}/{}", - namespace, node_name - ); - - client - .send_request(&request) - .await - .map_err(|e| DynamicError::SerializationError(e.to_string()))?; - - // Map a channel/recv timeout to the dedicated ServiceTimeout variant so callers - // can distinguish "service didn't respond" from actual CDR decode failures. - let node_display = if namespace.is_empty() || namespace == "/" { - node_name.to_string() - } else { - format!("{}/{}", namespace, node_name) - }; - let service_display = if namespace.is_empty() || namespace == "/" { - format!("/{}/get_type_description", node_name) - } else { - format!("{}/{}/get_type_description", namespace, node_name) - }; - - let response = client.take_response_timeout(self.timeout).map_err(|_| { - DynamicError::ServiceTimeout { - node: node_display, - service: service_display, - } - })?; - - if response.successful { - debug!( - "[TDC] Got type description for: {}", - response.type_description.type_description.type_name - ); - } else { - warn!( - "[TDC] Type description query failed: {}", - response.failure_reason - ); - } - - Ok(response) - } - - /// Query type description from any node publishing to a topic. - /// - /// This method uses the graph to discover publishers on the specified topic, - /// then queries each publisher's node for the type description until one - /// succeeds. - /// - /// # Arguments - /// - /// * `topic` - The topic to discover type information for - /// * `timeout` - Maximum time to wait for discovery and query - /// - /// # Returns - /// - /// A tuple of (MessageSchema, type_hash) on success. - pub async fn get_type_description_for_topic( - &self, - topic: &str, - timeout: Duration, - ) -> Result<(Arc, String), DynamicError> { - let graph = self.graph.as_ref().ok_or_else(|| { - DynamicError::SerializationError( - "TypeDescriptionClient requires graph for topic-based discovery".to_string(), - ) - })?; - - debug!("[TDC] Discovering type description for topic: {}", topic); - - // ── Phase 1: Publisher discovery ───────────────────────────────────── - // Retry up to 5 × 500 ms if the graph hasn't seen any publishers yet. - let mut publishers = graph.get_entities_by_topic(EntityKind::Publisher, topic); - debug!( - "[TDC] Initial discovery found {} publishers for topic {}", - publishers.len(), - topic - ); - - if publishers.is_empty() { - debug!("[TDC] No publishers found initially, waiting for discovery..."); - for attempt in 1..=5 { - tokio::time::sleep(Duration::from_millis(500)).await; - publishers = graph.get_entities_by_topic(EntityKind::Publisher, topic); - debug!( - "[TDC] Discovery attempt {}: found {} publishers", - attempt, - publishers.len() - ); - if !publishers.is_empty() { - break; - } - } - - if publishers.is_empty() { - warn!( - "[TDC] No publishers found for topic {} after retries", - topic - ); - return Err(DynamicError::SchemaNotFound(format!( - "No publishers found for topic: {}", - topic - ))); - } - } - - // Extract type name and hash from the first publisher. - // All publishers on a topic share the same type in ROS 2. - let first_pub = publishers.first().ok_or_else(|| { - DynamicError::SchemaNotFound(format!("No publishers found for topic: {}", topic)) - })?; - let first_ep = match &**first_pub { - Entity::Endpoint(e) => e, - _ => { - return Err(DynamicError::SerializationError( - "Expected endpoint entity".to_string(), - )); - } - }; - let type_info = first_ep.type_info.as_ref().ok_or_else(|| { - DynamicError::SchemaNotFound(format!("Publisher on {} has no type information", topic)) - })?; - let type_name = normalize_type_name(&type_info.name); - let type_hash = type_info.hash.to_rihs_string(); - - debug!( - "[TDC] Found publisher for {}: node={}/{}, type={} (normalized from: {})", - topic, first_ep.node.namespace, first_ep.node.name, type_name, type_info.name - ); - - // ── Phase 2: Create one ZClient (Querier) per publisher endpoint ───── - // - // This is the key fix for the flaky race condition. In rmw_zenoh_cpp the - // Querier is created once when rmw_create_client is called and then reused - // for every send_request. By the time user code calls a service the - // Querier has been alive long enough to have received queryable - // advertisements from the router. - // - // The old code called create_client() inside the retry loop, meaning a - // brand-new Querier was created and immediately used. With AllComplete a - // fresh Querier that hasn't received advertisements yet resolves the GET - // instantly with zero replies, causing the 10 s take_response_timeout to - // expire before a reply ever arrives. - // - // By creating all Queriers here, before any GET is sent, we give them time - // to settle in Phase 3 below. - // (node_name, namespace, client) - let mut pub_clients: Vec<(String, String, ZClient)> = Vec::new(); - for publisher in &publishers { - let Entity::Endpoint(ep) = &**publisher else { - continue; - }; - let service_name = if ep.node.namespace.is_empty() || ep.node.namespace == "/" { - format!("/{}/get_type_description", ep.node.name) - } else { - format!( - "{}/{}/get_type_description", - ep.node.namespace, ep.node.name - ) - }; - match self.create_client(&service_name, "") { - Ok(c) => pub_clients.push((ep.node.name.clone(), ep.node.namespace.clone(), c)), - Err(e) => warn!("[TDC] Could not create client for {}: {}", service_name, e), - } - } - - if pub_clients.is_empty() { - return Err(DynamicError::SchemaNotFound(format!( - "Could not create any service clients for topic: {}", - topic - ))); - } - - // ── Phase 3: Let Queriers settle ───────────────────────────────────── - // - // Allow the newly-declared Queriers to receive queryable advertisements - // from the router. This mirrors the implicit settling time rmw_zenoh_cpp - // benefits from because rmw_create_client and rmw_send_request are called - // in different stages of a node's lifecycle. - tokio::time::sleep(Duration::from_millis(100)).await; - - // ── Phase 4: Retry loop using the pre-built clients ────────────────── - // - // With settled Queriers the happy path succeeds on the first attempt. - // The retry loop is a safety net for loaded CI systems or slow networks. - let deadline = tokio::time::Instant::now() + timeout; - let mut last_error = None; - - 'retry: loop { - for (node_name, namespace, client) in &pub_clients { - if tokio::time::Instant::now() > deadline { - break 'retry; - } - - match self - .query_with_client(client, node_name, namespace, &type_name, &type_hash, false) - .await - { - Ok(response) if response.successful => { - let schema = Self::response_to_schema(&response)?; - return Ok((schema, type_hash.clone())); - } - Ok(response) => { - // Definitive service failure (e.g. hash mismatch): no point retrying. - last_error = - Some(DynamicError::SerializationError(response.failure_reason)); - break 'retry; - } - Err(e @ DynamicError::ServiceTimeout { .. }) => { - // Transient: Querier may not yet have received the advertisement. - debug!("[TDC] Type description query timed out, retrying..."); - last_error = Some(e); - } - Err(e) => { - last_error = Some(e); - break 'retry; - } - } - } - - let remaining = deadline.saturating_duration_since(tokio::time::Instant::now()); - if remaining.is_zero() { - break; - } - - let delay = Duration::from_millis(500).min(remaining); - tokio::time::sleep(delay).await; - - // (Publisher list is fixed to the clients built in Phase 2; - // re-discovery on each retry is not needed once Queriers are settled.) - } - - Err(last_error.unwrap_or_else(|| { - DynamicError::SchemaNotFound(format!( - "Failed to get type description from any publisher on {}", - topic - )) - })) - } - - /// Convert a GetTypeDescriptionResponse to a MessageSchema. - /// - /// This method takes the wire format TypeDescription from a service response - /// and converts it to a runtime MessageSchema that can be used for dynamic - /// message handling. - /// - /// # Arguments - /// - /// * `response` - The response from a GetTypeDescription service call - /// - /// # Returns - /// - /// An `Arc` representing the type. - pub fn response_to_schema( - response: &GetTypeDescriptionResponse, - ) -> Result, DynamicError> { - if !response.successful { - return Err(DynamicError::SerializationError(format!( - "Response indicates failure: {}", - response.failure_reason - ))); - } - - // Convert wire format to ros-z-schema types - let type_desc_msg = wire_to_schema_type_description(&response.type_description); - - // Convert to MessageSchema - type_description_msg_to_schema(&type_desc_msg) - } - - /// Create a service client for the given service name. - fn create_client( - &self, - service_name: &str, - namespace: &str, - ) -> Result, DynamicError> { - // Create a temporary node entity for the client - let node_entity = NodeEntity::new( - 0, // domain_id - self.session.zid(), - self.counter.increment(), - "type_desc_client".to_string(), - namespace.to_string(), - String::new(), // enclave (empty, normalized to "%" in liveliness token) - ); - - let entity = EndpointEntity { - id: self.counter.increment(), - node: node_entity, - kind: EntityKind::Client, - topic: service_name.to_string(), - type_info: Some(GetTypeDescription::service_type_info()), - ..Default::default() - }; - - let builder: ZClientBuilder = ZClientBuilder { - entity, - session: self.session.clone(), - clock: crate::time::ZClock::default(), - keyexpr_format: ros_z_protocol::KeyExprFormat::default(), - _phantom_data: Default::default(), - }; - - builder - .build() - .map_err(|e| DynamicError::SerializationError(e.to_string())) - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::dynamic::schema::FieldType; - use crate::dynamic::type_description_service::{ - WireTypeDescription, schema_to_wire_type_description, - }; - - #[test] - fn test_response_to_schema_success() { - // Build a schema and convert to wire format - let original = MessageSchema::builder("std_msgs/msg/String") - .field("data", FieldType::String) - .build() - .unwrap(); - - let wire_td = schema_to_wire_type_description(&original).unwrap(); - - let response = GetTypeDescriptionResponse { - successful: true, - failure_reason: String::new(), - type_description: wire_td, - type_sources: vec![], - extra_information: vec![], - }; - - let schema = TypeDescriptionClient::response_to_schema(&response).unwrap(); - assert_eq!(schema.type_name, "std_msgs/msg/String"); - assert_eq!(schema.fields.len(), 1); - assert_eq!(schema.fields[0].name, "data"); - } - - #[test] - fn test_response_to_schema_failure() { - let response = GetTypeDescriptionResponse { - successful: false, - failure_reason: "Type not found".to_string(), - type_description: WireTypeDescription::default(), - type_sources: vec![], - extra_information: vec![], - }; - - let result = TypeDescriptionClient::response_to_schema(&response); - assert!(result.is_err()); - } - - #[test] - fn test_response_to_schema_nested() { - // Build a nested schema - let vector3 = MessageSchema::builder("geometry_msgs/msg/Vector3") - .field("x", FieldType::Float64) - .field("y", FieldType::Float64) - .field("z", FieldType::Float64) - .build() - .unwrap(); - - let twist = MessageSchema::builder("geometry_msgs/msg/Twist") - .field("linear", FieldType::Message(vector3.clone())) - .field("angular", FieldType::Message(vector3)) - .build() - .unwrap(); - - let wire_td = schema_to_wire_type_description(&twist).unwrap(); - - let response = GetTypeDescriptionResponse { - successful: true, - failure_reason: String::new(), - type_description: wire_td, - type_sources: vec![], - extra_information: vec![], - }; - - let schema = TypeDescriptionClient::response_to_schema(&response).unwrap(); - assert_eq!(schema.type_name, "geometry_msgs/msg/Twist"); - assert_eq!(schema.fields.len(), 2); - - // Verify nested types are resolved - if let FieldType::Message(nested) = &schema.fields[0].field_type { - assert_eq!(nested.type_name, "geometry_msgs/msg/Vector3"); - assert_eq!(nested.fields.len(), 3); - } else { - panic!("Expected Message type for linear field"); - } - } -} diff --git a/crates/ros-z/src/dynamic/type_description_query.rs b/crates/ros-z/src/dynamic/type_description_query.rs new file mode 100644 index 00000000..7eaf7d44 --- /dev/null +++ b/crates/ros-z/src/dynamic/type_description_query.rs @@ -0,0 +1,220 @@ +//! Standard type-description protocol helpers. + +use std::{sync::Arc, time::Duration}; + +use tracing::{debug, warn}; + +use crate::{Builder, node::ZNode}; + +#[cfg(test)] +use super::discovery::collect_topic_schema_candidates_from_publishers; +use super::type_description::type_description_msg_to_schema; +use super::type_description_service::{ + GetTypeDescription, GetTypeDescriptionRequest, GetTypeDescriptionResponse, + wire_to_schema_type_description, +}; +use super::{discovery::TopicSchemaCandidate, error::DynamicError, schema::MessageSchema}; + +pub(crate) async fn query_type_description( + node: &ZNode, + candidate: &TopicSchemaCandidate, + timeout: Duration, + include_sources: bool, +) -> Result<(Arc, String), DynamicError> { + debug!( + "[TDC] Querying type description: node={}/{}, type={}", + candidate.namespace, candidate.node_name, candidate.type_name + ); + + let service_name = if candidate.namespace.is_empty() || candidate.namespace == "/" { + format!("/{}/get_type_description", candidate.node_name) + } else { + format!( + "{}/{}/get_type_description", + candidate.namespace, candidate.node_name + ) + }; + + let client = node + .create_client::(&service_name) + .build() + .map_err(|e| DynamicError::SerializationError(e.to_string()))?; + let request = GetTypeDescriptionRequest { + type_name: candidate.type_name.clone(), + type_hash: candidate.type_hash.clone(), + include_type_sources: include_sources, + }; + + let response = client + .call_or_timeout(&request, timeout) + .await + .map_err(|_| DynamicError::ServiceTimeout { + node: if candidate.namespace.is_empty() || candidate.namespace == "/" { + candidate.node_name.clone() + } else { + format!("{}/{}", candidate.namespace, candidate.node_name) + }, + service: service_name, + })?; + + if response.successful { + let schema = schema_from_type_description_response(&response)?; + Ok((schema, candidate.type_hash.clone())) + } else { + warn!( + "[TDC] Type description query failed: {}", + response.failure_reason + ); + Err(DynamicError::SerializationError(response.failure_reason)) + } +} + +pub fn schema_from_type_description_response( + response: &GetTypeDescriptionResponse, +) -> Result, DynamicError> { + if !response.successful { + return Err(DynamicError::SerializationError(format!( + "Response indicates failure: {}", + response.failure_reason + ))); + } + + let type_desc_msg = wire_to_schema_type_description(&response.type_description); + type_description_msg_to_schema(&type_desc_msg) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::dynamic::schema::FieldType; + use crate::dynamic::type_description_service::{ + WireTypeDescription, schema_to_wire_type_description, + }; + use crate::entity::{EndpointEntity, EndpointKind, Entity, NodeEntity, TypeHash, TypeInfo}; + + fn publisher_entity(node_name: Option<&str>, type_name: Option<&str>) -> Arc { + let node = node_name.map(|name| { + NodeEntity::new( + 1, + "1234567890abcdef1234567890abcdef".parse().unwrap(), + 0, + name.to_string(), + "/".to_string(), + String::new(), + ) + }); + + Arc::new(Entity::Endpoint(EndpointEntity { + id: 1, + node, + kind: EndpointKind::Publisher, + topic: "/chatter".to_string(), + type_info: type_name.map(|name| TypeInfo::new(name, TypeHash::zero())), + qos: Default::default(), + })) + } + + #[test] + fn test_response_to_schema_success() { + let original = MessageSchema::builder("std_msgs/msg/String") + .field("data", FieldType::String) + .build() + .unwrap(); + + let wire_td = schema_to_wire_type_description(&original).unwrap(); + + let response = GetTypeDescriptionResponse { + successful: true, + failure_reason: String::new(), + type_description: wire_td, + type_sources: vec![], + extra_information: vec![], + }; + + let schema = schema_from_type_description_response(&response).unwrap(); + assert_eq!(schema.type_name, "std_msgs/msg/String"); + assert_eq!(schema.fields.len(), 1); + assert_eq!(schema.fields[0].name, "data"); + } + + #[test] + fn test_response_to_schema_failure() { + let response = GetTypeDescriptionResponse { + successful: false, + failure_reason: "Type not found".to_string(), + type_description: WireTypeDescription::default(), + type_sources: vec![], + extra_information: vec![], + }; + + let result = schema_from_type_description_response(&response); + assert!(result.is_err()); + } + + #[test] + fn test_response_to_schema_nested() { + let vector3 = MessageSchema::builder("geometry_msgs/msg/Vector3") + .field("x", FieldType::Float64) + .field("y", FieldType::Float64) + .field("z", FieldType::Float64) + .build() + .unwrap(); + + let twist = MessageSchema::builder("geometry_msgs/msg/Twist") + .field("linear", FieldType::Message(vector3.clone())) + .field("angular", FieldType::Message(vector3)) + .build() + .unwrap(); + + let wire_td = schema_to_wire_type_description(&twist).unwrap(); + + let response = GetTypeDescriptionResponse { + successful: true, + failure_reason: String::new(), + type_description: wire_td, + type_sources: vec![], + extra_information: vec![], + }; + + let schema = schema_from_type_description_response(&response).unwrap(); + assert_eq!(schema.type_name, "geometry_msgs/msg/Twist"); + assert_eq!(schema.fields.len(), 2); + + if let FieldType::Message(nested) = &schema.fields[0].field_type { + assert_eq!(nested.type_name, "geometry_msgs/msg/Vector3"); + assert_eq!(nested.fields.len(), 3); + } else { + panic!("Expected Message type for linear field"); + } + } + + #[test] + fn test_topic_discovery_uses_type_info_from_any_publisher() { + let publishers = vec![ + publisher_entity(None, Some("std_msgs::msg::dds_::String_")), + publisher_entity(Some("talker"), Some("std_msgs::msg::dds_::String_")), + ]; + + let candidates = collect_topic_schema_candidates_from_publishers(&publishers, "/chatter") + .expect("expected type info to be discovered"); + + assert_eq!(candidates.len(), 1); + assert_eq!(candidates[0].node_name, "talker"); + assert_eq!(candidates[0].namespace, "/"); + assert_eq!(candidates[0].type_name, "std_msgs/msg/String"); + assert_eq!(candidates[0].type_hash, TypeHash::zero().to_rihs_string()); + } + + #[test] + fn test_topic_discovery_reports_missing_node_identity_only_when_all_publishers_lack_it() { + let publishers = vec![publisher_entity(None, Some("std_msgs::msg::dds_::String_"))]; + + let err = collect_topic_schema_candidates_from_publishers(&publishers, "/chatter") + .expect_err("expected missing node identity error"); + + assert!(matches!( + err, + DynamicError::MissingNodeIdentity { ref topic } if topic == "/chatter" + )); + } +} diff --git a/crates/ros-z/src/dynamic/type_description_service.rs b/crates/ros-z/src/dynamic/type_description_service.rs index 7ad9dbf5..9e60046c 100644 --- a/crates/ros-z/src/dynamic/type_description_service.rs +++ b/crates/ros-z/src/dynamic/type_description_service.rs @@ -45,6 +45,7 @@ use zenoh::query::Query; use zenoh::{Result as ZResult, Session}; use crate::ServiceTypeInfo; +use crate::attachment::Attachment; use crate::entity::{TypeHash, TypeInfo}; use crate::msg::ZService; use crate::service::{ZServer, ZServerBuilder}; @@ -466,11 +467,11 @@ impl TypeDescriptionService { let entity = crate::entity::EndpointEntity { id: counter.increment(), - node: node_entity, - kind: crate::entity::EntityKind::Service, + node: Some(node_entity), + kind: crate::entity::EndpointKind::Service, topic: service_name.to_string(), type_info: Some(GetTypeDescription::service_type_info()), - ..Default::default() + qos: Default::default(), }; // Build the service server with callback mode to avoid blocking tasks @@ -631,7 +632,13 @@ impl TypeDescriptionService { // Serialize and send the response let bytes = SerdeCdrSerdes::serialize(&response); use zenoh::Wait; - if let Err(e) = query.reply(query.key_expr().clone(), bytes).wait() { + let mut reply = query.reply(query.key_expr().clone(), bytes); + if let Some(att_bytes) = query.attachment() + && let Ok(att) = Attachment::try_from(att_bytes) + { + reply = reply.attachment(att); + } + if let Err(e) = reply.wait() { warn!("[TDS] Failed to send response: {}", e); } } diff --git a/crates/ros-z/src/dynamic/type_info.rs b/crates/ros-z/src/dynamic/type_info.rs new file mode 100644 index 00000000..e601b241 --- /dev/null +++ b/crates/ros-z/src/dynamic/type_info.rs @@ -0,0 +1,55 @@ +use tracing::warn; + +use crate::dynamic::{MessageSchema, MessageSchemaTypeDescription}; +use crate::entity::{TypeHash, TypeInfo}; + +pub(crate) fn dds_type_name_from_schema(schema: &MessageSchema) -> String { + schema + .type_name + .replace("/msg/", "::msg::dds_::") + .replace("/srv/", "::srv::dds_::") + .replace("/action/", "::action::dds_::") + + "_" +} + +pub(crate) fn ros_type_name_from_dds(dds_name: &str) -> String { + dds_name + .replace("::msg::dds_::", "/msg/") + .replace("::srv::dds_::", "/srv/") + .replace("::action::dds_::", "/action/") + .trim_end_matches('_') + .to_string() +} + +pub(crate) fn schema_hash(schema: &MessageSchema) -> TypeHash { + match schema.compute_type_hash() { + Ok(hash) => { + let rihs_string = hash.to_rihs_string(); + TypeHash::from_rihs_string(&rihs_string).unwrap_or_else(TypeHash::zero) + } + Err(error) => { + warn!( + "[NOD] Failed to compute type hash for {}: {}", + schema.type_name, error + ); + TypeHash::zero() + } + } +} + +pub(crate) fn schema_type_info(schema: &MessageSchema) -> TypeInfo { + TypeInfo { + name: dds_type_name_from_schema(schema), + hash: schema_hash(schema), + } +} + +pub(crate) fn schema_type_info_with_hash( + schema: &MessageSchema, + discovered_hash: &str, +) -> TypeInfo { + TypeInfo { + name: dds_type_name_from_schema(schema), + hash: TypeHash::from_rihs_string(discovered_hash).unwrap_or_else(TypeHash::zero), + } +} diff --git a/crates/ros-z/src/dynamic/value.rs b/crates/ros-z/src/dynamic/value.rs index e690b8af..fc25bced 100644 --- a/crates/ros-z/src/dynamic/value.rs +++ b/crates/ros-z/src/dynamic/value.rs @@ -3,8 +3,10 @@ //! This module provides the `DynamicValue` enum for representing any ROS 2 //! value at runtime, along with conversion traits. +use std::sync::Arc; + use super::message::DynamicMessage; -use super::schema::FieldType; +use super::schema::{EnumPayloadSchema, EnumSchema, FieldType}; /// Runtime representation of any ROS 2 value. #[derive(Clone, Debug, PartialEq)] @@ -28,11 +30,54 @@ pub enum DynamicValue { /// Nested message Message(Box), + /// Optional value encoded with a `u32` presence tag. + Optional(Option>), + /// Tagged enum encoded with a `u32` variant index. + Enum(EnumValue), /// Collections (homogeneous) Array(Vec), } +/// Runtime representation of a serde enum value. +#[derive(Clone, Debug, PartialEq)] +pub struct EnumValue { + pub variant_index: u32, + pub variant_name: String, + pub payload: EnumPayloadValue, +} + +impl EnumValue { + /// Create a new enum value. + pub fn new( + variant_index: u32, + variant_name: impl Into, + payload: EnumPayloadValue, + ) -> Self { + Self { + variant_index, + variant_name: variant_name.into(), + payload, + } + } +} + +/// Runtime payload value for a serde enum variant. +#[derive(Clone, Debug, PartialEq)] +pub enum EnumPayloadValue { + Unit, + Newtype(Box), + Tuple(Vec), + Struct(Vec), +} + +/// Named field value used by struct enum variants. +#[derive(Clone, Debug, PartialEq)] +pub struct DynamicNamedValue { + pub name: String, + pub value: DynamicValue, +} + /// Macro to generate accessor methods for primitive types. macro_rules! impl_primitive_accessors { ($($method:ident -> $variant:ident : $ty:ty),* $(,)?) => { @@ -97,6 +142,23 @@ impl DynamicValue { } } + /// Try to extract as an optional reference. + pub fn as_optional(&self) -> Option> { + match self { + DynamicValue::Optional(Some(value)) => Some(Some(value.as_ref())), + DynamicValue::Optional(None) => Some(None), + _ => None, + } + } + + /// Try to extract as an enum reference. + pub fn as_enum(&self) -> Option<&EnumValue> { + match self { + DynamicValue::Enum(value) => Some(value), + _ => None, + } + } + /// Try to extract as an array reference. pub fn as_array(&self) -> Option<&[DynamicValue]> { match self { @@ -210,6 +272,28 @@ impl IntoDynamic for Vec { } } +impl IntoDynamic for Option { + fn into_dynamic(self) -> DynamicValue { + DynamicValue::Optional(self.map(|value| Box::new(value.into_dynamic()))) + } +} + +impl FromDynamic for Option { + fn from_dynamic(value: &DynamicValue) -> Option { + match value { + DynamicValue::Optional(None) => Some(None), + DynamicValue::Optional(Some(inner)) => T::from_dynamic(inner.as_ref()).map(Some), + _ => None, + } + } +} + +impl IntoDynamic for EnumValue { + fn into_dynamic(self) -> DynamicValue { + DynamicValue::Enum(self) + } +} + /// Create the default value for a given field type. pub fn default_for_type(field_type: &FieldType) -> DynamicValue { match field_type { @@ -226,9 +310,45 @@ pub fn default_for_type(field_type: &FieldType) -> DynamicValue { FieldType::Float64 => DynamicValue::Float64(0.0), FieldType::String | FieldType::BoundedString(_) => DynamicValue::String(String::new()), FieldType::Message(schema) => DynamicValue::Message(Box::new(DynamicMessage::new(schema))), + FieldType::Optional(_) => DynamicValue::Optional(None), + FieldType::Enum(schema) => DynamicValue::Enum(default_enum_value(schema)), FieldType::Array(inner, len) => DynamicValue::Array(vec![default_for_type(inner); *len]), FieldType::Sequence(_) | FieldType::BoundedSequence(_, _) => { DynamicValue::Array(Vec::new()) } } } + +fn default_enum_value(schema: &Arc) -> EnumValue { + let variant = schema + .variants + .first() + .expect("enum schemas must have at least one variant"); + + EnumValue { + variant_index: 0, + variant_name: variant.name.clone(), + payload: default_enum_payload(&variant.payload), + } +} + +fn default_enum_payload(payload: &EnumPayloadSchema) -> EnumPayloadValue { + match payload { + EnumPayloadSchema::Unit => EnumPayloadValue::Unit, + EnumPayloadSchema::Newtype(field_type) => { + EnumPayloadValue::Newtype(Box::new(default_for_type(field_type))) + } + EnumPayloadSchema::Tuple(field_types) => { + EnumPayloadValue::Tuple(field_types.iter().map(default_for_type).collect()) + } + EnumPayloadSchema::Struct(fields) => EnumPayloadValue::Struct( + fields + .iter() + .map(|field| DynamicNamedValue { + name: field.name.clone(), + value: default_for_type(&field.field_type), + }) + .collect(), + ), + } +} diff --git a/crates/ros-z/src/encoding.rs b/crates/ros-z/src/encoding.rs index a591e5cd..f76e50c4 100644 --- a/crates/ros-z/src/encoding.rs +++ b/crates/ros-z/src/encoding.rs @@ -29,10 +29,6 @@ use std::fmt; /// // Protobuf with schema /// let proto = Encoding::protobuf() /// .with_schema("geometry_msgs/msg/Vector3"); -/// -/// // FlatBuffers with schema -/// let fb = Encoding::flatbuffers() -/// .with_schema("sensor_msgs/msg/Image"); /// ``` #[derive(Debug, Clone, PartialEq, Eq, Hash, Default)] pub enum Encoding { diff --git a/crates/ros-z/src/entity.rs b/crates/ros-z/src/entity.rs index 37439236..57d5519d 100644 --- a/crates/ros-z/src/entity.rs +++ b/crates/ros-z/src/entity.rs @@ -42,9 +42,13 @@ pub fn node_lv_token_key_expr(entity: &NodeEntity) -> Result> { /// Get the GID (globally unique identifier) for this endpoint pub fn endpoint_gid(entity: &EndpointEntity) -> crate::attachment::GidArray { use sha2::Digest; + let node = entity + .node + .as_ref() + .expect("endpoint_gid requires endpoint node identity"); let mut hasher = sha2::Sha256::new(); // ZenohId has to_le_bytes() method - hasher.update(entity.node.z_id.to_le_bytes()); + hasher.update(node.z_id.to_le_bytes()); hasher.update(entity.id.to_le_bytes()); let hash = hasher.finalize(); let mut gid = [0u8; 16]; @@ -64,7 +68,12 @@ pub fn node_to_liveliness_ke(entity: &NodeEntity) -> Result { /// Convert an EndpointEntity to a LivelinessKE using the default format pub fn endpoint_to_liveliness_ke(entity: &EndpointEntity) -> Result { let format = ros_z_protocol::KeyExprFormat::default(); - format.liveliness_key_expr(entity, &entity.node.z_id) + let Some(node) = entity.node.as_ref() else { + return Err(zenoh::Error::from( + "endpoint liveliness requires node identity", + )); + }; + format.liveliness_key_expr(entity, &node.z_id) } /// Convert an Entity to a LivelinessKE using the default format @@ -79,7 +88,7 @@ pub fn entity_to_liveliness_ke(entity: &Entity) -> Result { pub fn entity_kind(entity: &Entity) -> EntityKind { match entity { Entity::Node(_) => EntityKind::Node, - Entity::Endpoint(e) => e.kind, + Entity::Endpoint(e) => e.entity_kind(), } } diff --git a/crates/ros-z/src/event.rs b/crates/ros-z/src/event.rs index 6729910f..de4307be 100644 --- a/crates/ros-z/src/event.rs +++ b/crates/ros-z/src/event.rs @@ -237,7 +237,7 @@ impl GraphEventManager { appeared: bool, _local_zid: zenoh::session::ZenohId, ) { - use crate::entity::EntityKind; + use crate::entity::EndpointKind; let change = if appeared { 1 } else { -1 }; @@ -255,11 +255,10 @@ impl GraphEventManager { // When a subscription appears/disappears, publishers get PublicationMatched events let event_type = match entity { crate::entity::Entity::Endpoint(endpoint) => match endpoint.kind { - EntityKind::Publisher => ZenohEventType::SubscriptionMatched, - EntityKind::Subscription => ZenohEventType::PublicationMatched, - EntityKind::Service => return, // TODO: Add service matched events - EntityKind::Client => return, // TODO: Add service matched events - EntityKind::Node => unreachable!("EndpointEntity should not have Node kind"), + EndpointKind::Publisher => ZenohEventType::SubscriptionMatched, + EndpointKind::Subscription => ZenohEventType::PublicationMatched, + EndpointKind::Service => return, // TODO: Add service matched events + EndpointKind::Client => return, // TODO: Add service matched events }, crate::entity::Entity::Node(_) => return, // Node changes don't trigger matched events }; diff --git a/crates/ros-z/src/extended_schema.rs b/crates/ros-z/src/extended_schema.rs new file mode 100644 index 00000000..e5dc969d --- /dev/null +++ b/crates/ros-z/src/extended_schema.rs @@ -0,0 +1,335 @@ +use std::sync::Arc; + +use serde::{Deserialize, Serialize}; +use sha2::Digest; + +use crate::dynamic::{DynamicError, FieldType, MessageSchema}; +use crate::node::ZNode; + +pub trait ExtendedMessageTypeInfo: crate::MessageTypeInfo { + fn extended_message_schema() -> Arc; + + fn extended_field_type() -> FieldType { + FieldType::Message(Self::extended_message_schema()) + } +} + +#[derive(Serialize, Deserialize)] +struct ExtendedMessageSchema { + type_name: String, + package: String, + name: String, + fields: Vec, +} + +#[derive(Serialize, Deserialize)] +struct ExtendedFieldSchema { + name: String, + field_type: ExtendedFieldType, +} + +#[derive(Serialize, Deserialize)] +#[serde(tag = "kind", rename_all = "snake_case")] +enum ExtendedFieldType { + Bool, + Int8, + Int16, + Int32, + Int64, + Uint8, + Uint16, + Uint32, + Uint64, + Float32, + Float64, + String, + BoundedString { + capacity: usize, + }, + Message { + schema: Box, + }, + Optional { + inner: Box, + }, + Enum { + schema: Box, + }, + Array { + inner: Box, + len: usize, + }, + Sequence { + inner: Box, + }, + BoundedSequence { + inner: Box, + max: usize, + }, +} + +#[derive(Serialize, Deserialize)] +struct ExtendedEnumSchema { + type_name: String, + variants: Vec, +} + +#[derive(Serialize, Deserialize)] +struct ExtendedEnumVariantSchema { + name: String, + payload: ExtendedEnumPayloadSchema, +} + +#[derive(Serialize, Deserialize)] +#[serde(tag = "kind", rename_all = "snake_case")] +enum ExtendedEnumPayloadSchema { + Unit, + Newtype { field_type: Box }, + Tuple { field_types: Vec }, + Struct { fields: Vec }, +} + +pub fn compute_extended_type_hash( + schema: &MessageSchema, +) -> Result { + let extended = message_schema_to_extended(schema); + let json = ros_z_schema::to_ros2_json(&extended).map_err(|err| { + DynamicError::SerializationError(format!( + "failed to serialize extended schema hash view for {}: {}", + schema.type_name, err + )) + })?; + + let mut hasher = sha2::Sha256::new(); + hasher.update(json.as_bytes()); + Ok(ros_z_schema::TypeHash(hasher.finalize().into())) +} + +pub fn schema_to_extension_json(schema: &MessageSchema) -> Result { + let extended = message_schema_to_extended(schema); + serde_json::to_string(&extended).map_err(|err| { + DynamicError::SerializationError(format!( + "failed to serialize extended schema for {}: {}", + schema.type_name, err + )) + }) +} + +pub fn schema_from_extension_json(json: &str) -> Result, DynamicError> { + let extended: ExtendedMessageSchema = serde_json::from_str(json).map_err(|err| { + DynamicError::DeserializationError(format!("failed to parse extended schema JSON: {}", err)) + })?; + Ok(Arc::new(extended_to_message_schema(extended))) +} + +pub fn register_type(node: &ZNode) -> Result<(), String> { + let schema = T::extended_message_schema(); + if !schema.uses_extended_types() { + return Ok(()); + } + + let service = node.extended_type_description_service().ok_or_else(|| { + "extended type description service is not enabled on this node; call with_extended_type_description_service() to expose schemas for extended-only types".to_string() + })?; + + service + .register_schema(schema) + .map_err(|err| format!("failed to register extended schema: {}", err)) +} + +fn message_schema_to_extended(schema: &MessageSchema) -> ExtendedMessageSchema { + ExtendedMessageSchema { + type_name: schema.type_name.clone(), + package: schema.package.clone(), + name: schema.name.clone(), + fields: schema.fields.iter().map(field_schema_to_extended).collect(), + } +} + +fn field_schema_to_extended(field: &crate::dynamic::FieldSchema) -> ExtendedFieldSchema { + ExtendedFieldSchema { + name: field.name.clone(), + field_type: field_type_to_extended(&field.field_type), + } +} + +fn field_type_to_extended(field_type: &FieldType) -> ExtendedFieldType { + match field_type { + FieldType::Bool => ExtendedFieldType::Bool, + FieldType::Int8 => ExtendedFieldType::Int8, + FieldType::Int16 => ExtendedFieldType::Int16, + FieldType::Int32 => ExtendedFieldType::Int32, + FieldType::Int64 => ExtendedFieldType::Int64, + FieldType::Uint8 => ExtendedFieldType::Uint8, + FieldType::Uint16 => ExtendedFieldType::Uint16, + FieldType::Uint32 => ExtendedFieldType::Uint32, + FieldType::Uint64 => ExtendedFieldType::Uint64, + FieldType::Float32 => ExtendedFieldType::Float32, + FieldType::Float64 => ExtendedFieldType::Float64, + FieldType::String => ExtendedFieldType::String, + FieldType::BoundedString(capacity) => ExtendedFieldType::BoundedString { + capacity: *capacity, + }, + FieldType::Message(schema) => ExtendedFieldType::Message { + schema: Box::new(message_schema_to_extended(schema)), + }, + FieldType::Optional(inner) => ExtendedFieldType::Optional { + inner: Box::new(field_type_to_extended(inner)), + }, + FieldType::Enum(schema) => ExtendedFieldType::Enum { + schema: Box::new(enum_schema_to_extended(schema)), + }, + FieldType::Array(inner, len) => ExtendedFieldType::Array { + inner: Box::new(field_type_to_extended(inner)), + len: *len, + }, + FieldType::Sequence(inner) => ExtendedFieldType::Sequence { + inner: Box::new(field_type_to_extended(inner)), + }, + FieldType::BoundedSequence(inner, max) => ExtendedFieldType::BoundedSequence { + inner: Box::new(field_type_to_extended(inner)), + max: *max, + }, + } +} + +fn enum_schema_to_extended(schema: &crate::dynamic::EnumSchema) -> ExtendedEnumSchema { + ExtendedEnumSchema { + type_name: schema.type_name.clone(), + variants: schema + .variants + .iter() + .map(enum_variant_to_extended) + .collect(), + } +} + +fn enum_variant_to_extended( + variant: &crate::dynamic::EnumVariantSchema, +) -> ExtendedEnumVariantSchema { + ExtendedEnumVariantSchema { + name: variant.name.clone(), + payload: enum_payload_to_extended(&variant.payload), + } +} + +fn enum_payload_to_extended( + payload: &crate::dynamic::EnumPayloadSchema, +) -> ExtendedEnumPayloadSchema { + match payload { + crate::dynamic::EnumPayloadSchema::Unit => ExtendedEnumPayloadSchema::Unit, + crate::dynamic::EnumPayloadSchema::Newtype(field_type) => { + ExtendedEnumPayloadSchema::Newtype { + field_type: Box::new(field_type_to_extended(field_type)), + } + } + crate::dynamic::EnumPayloadSchema::Tuple(field_types) => ExtendedEnumPayloadSchema::Tuple { + field_types: field_types.iter().map(field_type_to_extended).collect(), + }, + crate::dynamic::EnumPayloadSchema::Struct(fields) => ExtendedEnumPayloadSchema::Struct { + fields: fields.iter().map(field_schema_to_extended).collect(), + }, + } +} + +fn extended_to_message_schema(schema: ExtendedMessageSchema) -> MessageSchema { + MessageSchema { + type_name: schema.type_name, + package: schema.package, + name: schema.name, + fields: schema + .fields + .into_iter() + .map(extended_to_field_schema) + .collect(), + type_hash: None, + } +} + +fn extended_to_field_schema(field: ExtendedFieldSchema) -> crate::dynamic::FieldSchema { + crate::dynamic::FieldSchema { + name: field.name, + field_type: extended_to_field_type(field.field_type), + default_value: None, + } +} + +fn extended_to_field_type(field_type: ExtendedFieldType) -> FieldType { + match field_type { + ExtendedFieldType::Bool => FieldType::Bool, + ExtendedFieldType::Int8 => FieldType::Int8, + ExtendedFieldType::Int16 => FieldType::Int16, + ExtendedFieldType::Int32 => FieldType::Int32, + ExtendedFieldType::Int64 => FieldType::Int64, + ExtendedFieldType::Uint8 => FieldType::Uint8, + ExtendedFieldType::Uint16 => FieldType::Uint16, + ExtendedFieldType::Uint32 => FieldType::Uint32, + ExtendedFieldType::Uint64 => FieldType::Uint64, + ExtendedFieldType::Float32 => FieldType::Float32, + ExtendedFieldType::Float64 => FieldType::Float64, + ExtendedFieldType::String => FieldType::String, + ExtendedFieldType::BoundedString { capacity } => FieldType::BoundedString(capacity), + ExtendedFieldType::Message { schema } => { + FieldType::Message(Arc::new(extended_to_message_schema(*schema))) + } + ExtendedFieldType::Optional { inner } => { + FieldType::Optional(Box::new(extended_to_field_type(*inner))) + } + ExtendedFieldType::Enum { schema } => { + FieldType::Enum(Arc::new(extended_to_enum_schema(*schema))) + } + ExtendedFieldType::Array { inner, len } => { + FieldType::Array(Box::new(extended_to_field_type(*inner)), len) + } + ExtendedFieldType::Sequence { inner } => { + FieldType::Sequence(Box::new(extended_to_field_type(*inner))) + } + ExtendedFieldType::BoundedSequence { inner, max } => { + FieldType::BoundedSequence(Box::new(extended_to_field_type(*inner)), max) + } + } +} + +fn extended_to_enum_schema(schema: ExtendedEnumSchema) -> crate::dynamic::EnumSchema { + crate::dynamic::EnumSchema { + type_name: schema.type_name, + variants: schema + .variants + .into_iter() + .map(extended_to_enum_variant) + .collect(), + } +} + +fn extended_to_enum_variant( + variant: ExtendedEnumVariantSchema, +) -> crate::dynamic::EnumVariantSchema { + crate::dynamic::EnumVariantSchema { + name: variant.name, + payload: extended_to_enum_payload(variant.payload), + } +} + +fn extended_to_enum_payload( + payload: ExtendedEnumPayloadSchema, +) -> crate::dynamic::EnumPayloadSchema { + match payload { + ExtendedEnumPayloadSchema::Unit => crate::dynamic::EnumPayloadSchema::Unit, + ExtendedEnumPayloadSchema::Newtype { field_type } => { + crate::dynamic::EnumPayloadSchema::Newtype(Box::new(extended_to_field_type( + *field_type, + ))) + } + ExtendedEnumPayloadSchema::Tuple { field_types } => { + crate::dynamic::EnumPayloadSchema::Tuple( + field_types + .into_iter() + .map(extended_to_field_type) + .collect(), + ) + } + ExtendedEnumPayloadSchema::Struct { fields } => crate::dynamic::EnumPayloadSchema::Struct( + fields.into_iter().map(extended_to_field_schema).collect(), + ), + } +} diff --git a/crates/ros-z/src/extended_type_description_query.rs b/crates/ros-z/src/extended_type_description_query.rs new file mode 100644 index 00000000..279da5c9 --- /dev/null +++ b/crates/ros-z/src/extended_type_description_query.rs @@ -0,0 +1,61 @@ +use std::{sync::Arc, time::Duration}; + +use crate::{Builder, dynamic::DynamicError, node::ZNode}; + +use crate::dynamic::{MessageSchema, discovery::TopicSchemaCandidate}; +use crate::extended_type_description_service::{ + GetExtendedTypeDescription, GetExtendedTypeDescriptionRequest, + GetExtendedTypeDescriptionResponse, +}; + +use crate::extended_schema::schema_from_extension_json; + +/// Query the extended type-description service for a single current topic candidate. +pub(crate) async fn query_extended_type_description( + node: &ZNode, + candidate: &TopicSchemaCandidate, + timeout: Duration, +) -> Result<(Arc, String), DynamicError> { + let service_name = if candidate.namespace.is_empty() || candidate.namespace == "/" { + format!("/{}/get_extended_type_description", candidate.node_name) + } else { + format!( + "{}/{}/get_extended_type_description", + candidate.namespace, candidate.node_name + ) + }; + + let client = node + .create_client::(&service_name) + .build() + .map_err(|e| DynamicError::SerializationError(e.to_string()))?; + let request = GetExtendedTypeDescriptionRequest { + type_name: candidate.type_name.clone(), + type_hash: candidate.type_hash.clone(), + }; + + let response = client + .call_or_timeout(&request, timeout) + .await + .map_err(|_| { + DynamicError::SerializationError( + "extended type description service timed out".to_string(), + ) + })?; + + let schema = schema_from_extended_type_description_response(&response)?; + Ok((schema, response.type_hash)) +} + +pub fn schema_from_extended_type_description_response( + response: &GetExtendedTypeDescriptionResponse, +) -> Result, DynamicError> { + if !response.successful { + return Err(DynamicError::SerializationError(format!( + "Response indicates failure: {}", + response.failure_reason + ))); + } + + schema_from_extension_json(&response.schema_json) +} diff --git a/crates/ros-z/src/extended_type_description_service.rs b/crates/ros-z/src/extended_type_description_service.rs new file mode 100644 index 00000000..2c2015ed --- /dev/null +++ b/crates/ros-z/src/extended_type_description_service.rs @@ -0,0 +1,241 @@ +use std::sync::{Arc, RwLock}; + +use serde::{Deserialize, Serialize}; +use tracing::{debug, info, warn}; +use zenoh::Session; + +use crate::ServiceTypeInfo; +use crate::attachment::Attachment; +use crate::context::GlobalCounter; +use crate::dynamic::{DynamicError, MessageSchema}; +use crate::entity::{EndpointEntity, EndpointKind, NodeEntity, TypeHash, TypeInfo}; +use crate::msg::{SerdeCdrSerdes, ZMessage, ZService}; +use crate::service::{ZServer, ZServerBuilder}; + +use crate::extended_schema::{compute_extended_type_hash, schema_to_extension_json}; + +const EXTENDED_SERVICE_NAME: &str = "~get_extended_type_description"; + +#[derive(Debug, Clone, Default, Serialize, Deserialize)] +pub struct GetExtendedTypeDescriptionRequest { + pub type_name: String, + pub type_hash: String, +} + +impl ZMessage for GetExtendedTypeDescriptionRequest { + type Serdes = SerdeCdrSerdes; +} + +#[derive(Debug, Clone, Default, Serialize, Deserialize)] +pub struct GetExtendedTypeDescriptionResponse { + pub successful: bool, + pub failure_reason: String, + pub type_hash: String, + pub schema_json: String, +} + +impl ZMessage for GetExtendedTypeDescriptionResponse { + type Serdes = SerdeCdrSerdes; +} + +pub struct GetExtendedTypeDescription; + +impl ZService for GetExtendedTypeDescription { + type Request = GetExtendedTypeDescriptionRequest; + type Response = GetExtendedTypeDescriptionResponse; +} + +impl ServiceTypeInfo for GetExtendedTypeDescription { + fn service_type_info() -> TypeInfo { + TypeInfo::new( + "ros_z::srv::dds_::GetExtendedTypeDescription_", + TypeHash::zero(), + ) + } +} + +#[derive(Clone)] +pub struct RegisteredExtendedSchema { + pub schema: Arc, + pub type_hash: String, +} + +impl RegisteredExtendedSchema { + pub fn new(schema: Arc) -> Result { + let type_hash = compute_extended_type_hash(&schema)?.to_rihs_string(); + Ok(Self { schema, type_hash }) + } +} + +#[derive(Clone)] +pub struct ExtendedTypeDescriptionService { + schemas: Arc>>, + _server: Arc>, +} + +impl ExtendedTypeDescriptionService { + pub fn new( + session: Arc, + node_name: &str, + namespace: &str, + node_id: usize, + counter: &GlobalCounter, + clock: &crate::time::ZClock, + ) -> zenoh::Result { + let schemas = Arc::new(RwLock::new(std::collections::HashMap::new())); + let node_entity = NodeEntity::new( + 0, + session.zid(), + node_id, + node_name.to_string(), + namespace.to_string(), + String::new(), + ); + + let entity = EndpointEntity { + id: counter.increment(), + node: Some(node_entity), + kind: EndpointKind::Service, + topic: EXTENDED_SERVICE_NAME.to_string(), + type_info: Some(GetExtendedTypeDescription::service_type_info()), + qos: Default::default(), + }; + + let server_builder: ZServerBuilder = ZServerBuilder { + entity, + session, + clock: clock.clone(), + keyexpr_format: ros_z_protocol::KeyExprFormat::default(), + _phantom_data: Default::default(), + }; + + let schemas_clone = schemas.clone(); + let server = server_builder.build_with_callback(move |query| { + Self::handle_query(&schemas_clone, &query); + })?; + + info!( + "[ETDS] ExtendedTypeDescriptionService created for node: {}/{}", + namespace, node_name + ); + + Ok(Self { + schemas, + _server: Arc::new(server), + }) + } + + pub fn register_schema(&self, schema: Arc) -> Result<(), DynamicError> { + let registered = RegisteredExtendedSchema::new(schema.clone())?; + let type_name = schema.type_name.clone(); + + let mut schemas = self + .schemas + .write() + .map_err(|_| DynamicError::RegistryLockPoisoned)?; + schemas.insert(type_name.clone(), registered); + + debug!("[ETDS] Registered extended schema: {}", type_name); + Ok(()) + } + + pub fn get_schema(&self, type_name: &str) -> Result>, DynamicError> { + let schemas = self + .schemas + .read() + .map_err(|_| DynamicError::RegistryLockPoisoned)?; + Ok(schemas + .get(type_name) + .map(|registered| registered.schema.clone())) + } + + fn handle_query( + schemas: &Arc>>, + query: &zenoh::query::Query, + ) { + let request = match query.payload() { + Some(payload) => match ::deserialize( + payload.to_bytes().as_ref(), + ) { + Ok(request) => request, + Err(err) => { + warn!("[ETDS] Failed to decode request: {}", err); + return; + } + }, + None => { + warn!("[ETDS] Missing request payload"); + return; + } + }; + + let response = Self::build_response(schemas, &request); + let bytes = ZMessage::serialize(&response); + use zenoh::Wait; + let mut reply = query.reply(query.key_expr().clone(), bytes); + if let Some(att_bytes) = query.attachment() + && let Ok(att) = Attachment::try_from(att_bytes) + { + reply = reply.attachment(att); + } + if let Err(err) = reply.wait() { + warn!("[ETDS] Failed to send response: {}", err); + } + } + + fn build_response( + schemas: &Arc>>, + request: &GetExtendedTypeDescriptionRequest, + ) -> GetExtendedTypeDescriptionResponse { + let schemas_guard = match schemas.read() { + Ok(guard) => guard, + Err(_) => { + return GetExtendedTypeDescriptionResponse { + successful: false, + failure_reason: "Internal error: registry lock poisoned".to_string(), + type_hash: String::new(), + schema_json: String::new(), + }; + } + }; + + let registered = match schemas_guard.get(&request.type_name) { + Some(schema) => schema, + None => { + return GetExtendedTypeDescriptionResponse { + successful: false, + failure_reason: format!("Type '{}' not registered", request.type_name), + type_hash: String::new(), + schema_json: String::new(), + }; + } + }; + + if !request.type_hash.is_empty() && request.type_hash != registered.type_hash { + return GetExtendedTypeDescriptionResponse { + successful: false, + failure_reason: format!( + "Type hash mismatch: expected {}, got {}", + registered.type_hash, request.type_hash + ), + type_hash: String::new(), + schema_json: String::new(), + }; + } + + match schema_to_extension_json(®istered.schema) { + Ok(schema_json) => GetExtendedTypeDescriptionResponse { + successful: true, + failure_reason: String::new(), + type_hash: registered.type_hash.clone(), + schema_json, + }, + Err(err) => GetExtendedTypeDescriptionResponse { + successful: false, + failure_reason: format!("Failed to serialize extended schema: {}", err), + type_hash: String::new(), + schema_json: String::new(), + }, + } + } +} diff --git a/crates/ros-z/src/graph.rs b/crates/ros-z/src/graph.rs index 3db2ba8f..100aafc6 100644 --- a/crates/ros-z/src/graph.rs +++ b/crates/ros-z/src/graph.rs @@ -10,12 +10,15 @@ use tokio::sync::Notify; use tracing::debug; use crate::entity::{ - ADMIN_SPACE, EndpointEntity, Entity, EntityKind, LivelinessKE, NodeKey, Topic, + ADMIN_SPACE, EndpointEntity, EndpointKind, Entity, EntityKind, LivelinessKE, NodeKey, Topic, }; use crate::event::GraphEventManager; use tracing; use zenoh::{Result, Session, Wait, pubsub::Subscriber, sample::SampleKind, session::ZenohId}; +#[cfg(test)] +use zenoh::key_expr::KeyExpr; + /// A serializable snapshot of the ROS graph state #[derive(Debug, Clone, Serialize)] pub struct GraphSnapshot { @@ -26,6 +29,52 @@ pub struct GraphSnapshot { pub services: Vec, } +#[cfg(test)] +mod tests { + use super::*; + use crate::entity::{EndpointEntity, EndpointKind}; + + #[test] + fn test_key_expr_origin_zid_supports_ros2dds_tokens() { + let zid: ZenohId = "1234567890abcdef1234567890abcdef".parse().unwrap(); + let key_expr: KeyExpr<'static> = "@/1234567890abcdef1234567890abcdef/@ros2_lv/MP/chatter/std_msgs\u{00A7}msg\u{00A7}String" + .to_string() + .try_into() + .unwrap(); + + assert_eq!(key_expr_origin_zid(&key_expr), Some(zid)); + } + + #[test] + fn test_key_expr_origin_zid_supports_rmw_zenoh_tokens() { + let zid: ZenohId = "1234567890abcdef1234567890abcdef".parse().unwrap(); + let key_expr: KeyExpr<'static> = "@ros2_lv/0/1234567890abcdef1234567890abcdef/1/1/MP/%/%/talker/chatter/std_msgs%msg%String/RIHS01_00000000000000000000000000000000/Q" + .try_into() + .unwrap(); + + assert_eq!(key_expr_origin_zid(&key_expr), Some(zid)); + } + + #[test] + fn test_entity_matches_local_zid_for_ros2dds_endpoint_without_node_identity() { + let zid: ZenohId = "1234567890abcdef1234567890abcdef".parse().unwrap(); + let entity = Entity::Endpoint(EndpointEntity { + id: 1, + node: None, + kind: EndpointKind::Publisher, + topic: "/chatter".to_string(), + type_info: None, + qos: Default::default(), + }); + let key_expr: KeyExpr<'static> = "@/1234567890abcdef1234567890abcdef/@ros2_lv/MP/chatter/std_msgs\u{00A7}msg\u{00A7}String" + .to_string() + .try_into() + .unwrap(); + + assert!(entity_matches_local_zid(&entity, &key_expr, zid)); + } +} + #[derive(Debug, Clone, Serialize)] pub struct TopicSnapshot { pub name: String, @@ -53,6 +102,34 @@ const DEFAULT_SLAB_CAPACITY: usize = 128; /// Type alias for entity parser function type EntityParser = Arc Result + Send + Sync>; +#[cfg(test)] +fn key_expr_origin_zid(key_expr: &KeyExpr) -> Option { + let mut segments = key_expr.split('/'); + + match segments.next()? { + "@" => segments.next()?.parse().ok(), + ADMIN_SPACE => { + segments.next()?; + segments.next()?.parse().ok() + } + _ => None, + } +} + +#[cfg(test)] +fn entity_matches_local_zid(entity: &Entity, key_expr: &KeyExpr, local_zid: ZenohId) -> bool { + let owner_zid = match entity { + Entity::Node(node) => Some(node.z_id), + Entity::Endpoint(endpoint) => endpoint + .node + .as_ref() + .map(|node| node.z_id) + .or_else(|| key_expr_origin_zid(key_expr)), + }; + + owner_zid.is_some_and(|zid| zid == local_zid) +} + pub struct GraphData { cached: HashSet, parsed: HashMap>, @@ -165,28 +242,43 @@ impl GraphData { slab.insert(weak); } Entity::Endpoint(x) => { + let node_desc = x + .node + .as_ref() + .map(|node| format!("{}/{}", node.namespace, node.name)) + .unwrap_or_else(|| "".to_string()); debug!( - "[GRF] Parsed endpoint: kind={:?}, topic={}, node={}/{}", - x.kind, x.topic, x.node.namespace, x.node.name + "[GRF] Parsed endpoint: kind={:?}, topic={}, node={}", + x.kind, x.topic, node_desc ); - let node_key = crate::entity::node_key(&x.node); let type_str = x .type_info .as_ref() .map(|t| t.name.as_str()) .unwrap_or("unknown"); - tracing::debug!( - "parse: Storing Endpoint ({:?}) for node_key=({:?}, {:?}), topic={}, type={}, id={}", - x.kind, - node_key.0, - node_key.1, - x.topic, - type_str, - x.id - ); + if let Some(node) = x.node.as_ref() { + let node_key = crate::entity::node_key(node); + tracing::debug!( + "parse: Storing Endpoint ({:?}) for node_key=({:?}, {:?}), topic={}, type={}, id={}", + x.kind, + node_key.0, + node_key.1, + x.topic, + type_str, + x.id + ); + } else { + tracing::debug!( + "parse: Storing Endpoint ({:?}) without node identity, topic={}, type={}, id={}", + x.kind, + x.topic, + type_str, + x.id + ); + } // Index by topic for Publisher/Subscription entities - if matches!(x.kind, EntityKind::Publisher | EntityKind::Subscription) { + if matches!(x.kind, EndpointKind::Publisher | EndpointKind::Subscription) { // TODO: omit the clone of topic let topic_slab = self .by_topic @@ -202,7 +294,7 @@ impl GraphData { } // Index by service for Service/Client entities - if matches!(x.kind, EntityKind::Service | EntityKind::Client) { + if matches!(x.kind, EndpointKind::Service | EndpointKind::Client) { // TODO: omit the clone of service name (stored in topic field) let service_slab = self .by_service @@ -216,17 +308,19 @@ impl GraphData { service_slab.insert(weak.clone()); } - let node_slab = self - .by_node - .entry(node_key) - .or_insert_with(|| Slab::with_capacity(DEFAULT_SLAB_CAPACITY)); + if let Some(node) = x.node.as_ref() { + let node_slab = self + .by_node + .entry(crate::entity::node_key(node)) + .or_insert_with(|| Slab::with_capacity(DEFAULT_SLAB_CAPACITY)); - // If slab is full, remove failing weak pointers first - if node_slab.len() >= node_slab.capacity() { - node_slab.retain(|_, weak_ptr| weak_ptr.upgrade().is_some()); - } + // If slab is full, remove failing weak pointers first + if node_slab.len() >= node_slab.capacity() { + node_slab.retain(|_, weak_ptr| weak_ptr.upgrade().is_some()); + } - node_slab.insert(weak); + node_slab.insert(weak); + } } } self.parsed.insert(ke, arc); @@ -405,7 +499,6 @@ impl Graph { let c_change_notify = change_notify.clone(); let c_zid = zid; let c_liveliness_pattern = liveliness_pattern.clone(); - let c_parser = parser_arc.clone(); let callback_parser = parser_arc.clone(); tracing::debug!("Creating liveliness subscriber for {}", liveliness_pattern); let sub = session @@ -426,6 +519,18 @@ impl Graph { SampleKind::Put => { debug!("[GRF] Entity appeared: {}", ke.0); tracing::debug!("Graph subscriber: PUT {}", key_expr.as_str()); + let parsed_entity = match callback_parser(&key_expr) { + Ok(entity) => Some(entity), + Err(e) => { + tracing::warn!( + "Failed to parse liveliness token {}: {:?}", + key_expr, + e + ); + None + } + }; + // Only insert if not already parsed (avoid duplicates from liveliness query) let already_parsed = graph_data_guard.parsed.contains_key(&ke); let already_cached = graph_data_guard.cached.contains(&ke); @@ -444,19 +549,9 @@ impl Graph { tracing::debug!(" Adding to cached"); graph_data_guard.insert(ke.clone()); } - // Trigger graph change events using backend-specific parser - match callback_parser(&key_expr) { - Ok(entity) => { - tracing::debug!("Successfully parsed entity: {:?}", entity); - c_event_manager.trigger_graph_change(&entity, true, c_zid); - } - Err(e) => { - tracing::warn!( - "Failed to parse liveliness token {}: {:?}", - key_expr, - e - ); - } + if let Some(entity) = parsed_entity { + tracing::debug!("Successfully parsed entity: {:?}", entity); + c_event_manager.trigger_graph_change(&entity, true, c_zid); } // Wake any tasks waiting in wait_for_subscription / wait_for_publisher. c_change_notify.notify_waiters(); @@ -484,44 +579,21 @@ impl Graph { .timeout(std::time::Duration::from_secs(3)) .wait()?; - // Process all replies and add them to the graph - // IMPORTANT: Filter out entities from the current session to avoid duplicates - // Local entities are already added via add_local_entity() + // Process all replies and add them to the graph. + // At this point plain ros-z still relies on liveliness for local entity + // visibility, so do not filter current-session entities here. let mut reply_count = 0; - let mut filtered_count = 0; while let Ok(reply) = replies.recv() { reply_count += 1; if let Ok(sample) = reply.into_result() { let key_expr = sample.key_expr().to_owned(); let ke = LivelinessKE(key_expr.clone()); - // Parse entity to check if it's from current session using backend-specific parser - if let Ok(entity) = c_parser(&key_expr) { - // Skip entities from current session - let is_local = match &entity { - Entity::Node(node) => node.z_id == zid, - Entity::Endpoint(endpoint) => endpoint.node.z_id == zid, - }; - - if !is_local { - // Only insert entities from other sessions - tracing::debug!( - "Graph: Adding cross-context entity: {}", - key_expr.as_str() - ); - graph_data.lock().insert(ke); - } else { - filtered_count += 1; - tracing::debug!("Graph: Filtered local entity: {}", key_expr.as_str()); - } - } + tracing::debug!("Graph: Caching liveliness entity: {}", key_expr.as_str()); + graph_data.lock().insert(ke); } } - tracing::debug!( - "Graph: Liveliness query received {} replies, filtered {} local entities", - reply_count, - filtered_count - ); + tracing::debug!("Graph: Liveliness query received {} replies", reply_count); Ok(Self { _subscriber: sub, @@ -536,7 +608,10 @@ impl Graph { pub fn is_entity_local(&self, entity: &Entity) -> bool { match entity { Entity::Node(node) => node.z_id == self.zid, - Entity::Endpoint(endpoint) => endpoint.node.z_id == self.zid, + Entity::Endpoint(endpoint) => endpoint + .node + .as_ref() + .is_some_and(|node| node.z_id == self.zid), } } @@ -576,7 +651,7 @@ impl Graph { // Index by topic for Publisher/Subscription if matches!( endpoint.kind, - EntityKind::Publisher | EntityKind::Subscription + EndpointKind::Publisher | EndpointKind::Subscription ) { let topic_slab = data .by_topic @@ -590,7 +665,7 @@ impl Graph { } // Index by service for Service/Client - if matches!(endpoint.kind, EntityKind::Service | EntityKind::Client) { + if matches!(endpoint.kind, EndpointKind::Service | EndpointKind::Client) { let service_slab = data .by_service .entry(endpoint.topic.clone()) @@ -603,15 +678,17 @@ impl Graph { } // Index by node - let node_slab = data - .by_node - .entry(crate::entity::node_key(&endpoint.node)) - .or_insert_with(|| Slab::with_capacity(DEFAULT_SLAB_CAPACITY)); + if let Some(node) = endpoint.node.as_ref() { + let node_slab = data + .by_node + .entry(crate::entity::node_key(node)) + .or_insert_with(|| Slab::with_capacity(DEFAULT_SLAB_CAPACITY)); - if node_slab.len() >= node_slab.capacity() { - node_slab.retain(|_, weak_ptr| weak_ptr.upgrade().is_some()); + if node_slab.len() >= node_slab.capacity() { + node_slab.retain(|_, weak_ptr| weak_ptr.upgrade().is_some()); + } + node_slab.insert(weak); } - node_slab.insert(weak); } } @@ -656,7 +733,7 @@ impl Graph { // Remove from by_topic or by_service depending on kind if matches!( endpoint_entity.kind, - EntityKind::Publisher | EntityKind::Subscription + EndpointKind::Publisher | EndpointKind::Subscription ) && let Some(slab) = data.by_topic.get_mut(&endpoint_entity.topic) { slab.retain(|_, weak| { @@ -667,7 +744,7 @@ impl Graph { } if matches!( endpoint_entity.kind, - EntityKind::Service | EntityKind::Client + EndpointKind::Service | EndpointKind::Client ) && let Some(slab) = data.by_service.get_mut(&endpoint_entity.topic) { slab.retain(|_, weak| { @@ -677,9 +754,8 @@ impl Graph { }); } // Also remove from by_node (endpoints are indexed by their node) - if let Some(slab) = data - .by_node - .get_mut(&crate::entity::node_key(&endpoint_entity.node)) + if let Some(node) = endpoint_entity.node.as_ref() + && let Some(slab) = data.by_node.get_mut(&crate::entity::node_key(node)) { slab.retain(|_, weak| { weak.upgrade().is_some_and(|arc| { @@ -794,7 +870,7 @@ impl Graph { // Skip expensive get_endpoint() if we already found the type if let Some(enp) = crate::entity::entity_get_endpoint(&ent) && found_type.is_none() - && enp.kind == EntityKind::Service + && enp.kind == EndpointKind::Service { found_type = enp.type_info.as_ref().map(|x| x.name.clone()); } @@ -831,8 +907,10 @@ impl Graph { && let Some(enp) = crate::entity::entity_get_endpoint(&ent) { // Include both publishers and subscribers - if matches!(enp.kind, EntityKind::Publisher | EntityKind::Subscription) - && let Some(type_info) = &enp.type_info + if matches!( + enp.kind, + EndpointKind::Publisher | EndpointKind::Subscription + ) && let Some(type_info) = &enp.type_info { found_type = Some(type_info.name.clone()); } @@ -883,7 +961,7 @@ impl Graph { data.visit_by_node(node_key, |ent| { if let Some(enp) = crate::entity::entity_get_endpoint(&ent) - && enp.kind == kind + && enp.entity_kind() == kind && let Some(type_info) = &enp.type_info { // Insert into set for automatic deduplication diff --git a/crates/ros-z/src/lib.rs b/crates/ros-z/src/lib.rs index 9f28e8bd..2f8f426d 100644 --- a/crates/ros-z/src/lib.rs +++ b/crates/ros-z/src/lib.rs @@ -57,6 +57,10 @@ pub mod encoding; pub mod entity; /// Graph events emitted by the Zenoh network graph. pub mod event; +/// ros-z-specific extended schema discovery for enums, options, and other non-ROS shapes. +pub mod extended_schema; +pub(crate) mod extended_type_description_query; +pub(crate) mod extended_type_description_service; #[cfg(feature = "ffi")] pub mod ffi; /// ROS 2 graph introspection (node/topic/service discovery). @@ -102,7 +106,9 @@ pub mod parameter; pub use attachment::GidArray; pub use entity::{TypeHash, TypeInfo}; +pub use extended_schema::ExtendedMessageTypeInfo; pub use ros_msg::{ActionTypeInfo, MessageTypeInfo, ServiceTypeInfo, WithTypeInfo}; +pub use ros_z_derive::{ExtendedMessageTypeInfo, MessageTypeInfo}; pub use zbuf::ZBuf; pub use zenoh::Result; diff --git a/crates/ros-z/src/lifecycle/client.rs b/crates/ros-z/src/lifecycle/client.rs index 96468766..8940f047 100644 --- a/crates/ros-z/src/lifecycle/client.rs +++ b/crates/ros-z/src/lifecycle/client.rs @@ -128,8 +128,7 @@ impl ZLifecycleClient { label: String::new(), }, }; - self.change_state.send_request(&req).await?; - let resp = self.change_state.take_response_timeout(timeout)?; + let resp = self.change_state.call_or_timeout(&req, timeout).await?; Ok(resp.success) } @@ -139,17 +138,19 @@ impl ZLifecycleClient { /// Query the current state of the remote lifecycle node. pub async fn get_state(&self, timeout: Duration) -> Result { - self.get_state.send_request(&GetStateRequest {}).await?; - let resp = self.get_state.take_response_timeout(timeout)?; + let resp = self + .get_state + .call_or_timeout(&GetStateRequest {}, timeout) + .await?; Ok(state_from_lc(&resp.current_state)) } /// List all states in the lifecycle state machine. pub async fn get_available_states(&self, timeout: Duration) -> Result> { - self.get_available_states - .send_request(&GetAvailableStatesRequest {}) + let resp = self + .get_available_states + .call_or_timeout(&GetAvailableStatesRequest {}, timeout) .await?; - let resp = self.get_available_states.take_response_timeout(timeout)?; Ok(resp.available_states) } @@ -158,12 +159,10 @@ impl ZLifecycleClient { &self, timeout: Duration, ) -> Result> { - self.get_available_transitions - .send_request(&GetAvailableTransitionsRequest {}) - .await?; let resp = self .get_available_transitions - .take_response_timeout(timeout)?; + .call_or_timeout(&GetAvailableTransitionsRequest {}, timeout) + .await?; Ok(resp.available_transitions) } } diff --git a/crates/ros-z/src/lifecycle/node.rs b/crates/ros-z/src/lifecycle/node.rs index 4735c3e3..eb528e0a 100644 --- a/crates/ros-z/src/lifecycle/node.rs +++ b/crates/ros-z/src/lifecycle/node.rs @@ -409,7 +409,13 @@ fn encode_reply( resp: &T, ) { let bytes = NativeCdrSerdes::serialize(resp); - if let Err(e) = query.reply(query.key_expr().clone(), bytes).wait() { + let mut reply = query.reply(query.key_expr().clone(), bytes); + if let Some(att_bytes) = query.attachment() + && let Ok(att) = crate::attachment::Attachment::try_from(att_bytes) + { + reply = reply.attachment(att); + } + if let Err(e) = reply.wait() { warn!("failed to send lifecycle reply: {e}"); } } diff --git a/crates/ros-z/src/node.rs b/crates/ros-z/src/node.rs index 955c6101..e2ff794a 100644 --- a/crates/ros-z/src/node.rs +++ b/crates/ros-z/src/node.rs @@ -1,7 +1,7 @@ use std::{sync::Arc, time::Duration}; use tracing::{debug, info, warn}; -use zenoh::{Result, Session, Wait, liveliness::LivelinessToken, sample::Sample}; +use zenoh::{Result, Session, Wait, liveliness::LivelinessToken}; #[cfg(feature = "ffi")] use crate::ffi::publisher::RawPublisher; @@ -11,17 +11,19 @@ use crate::{ cache::ZCacheBuilder, context::{GlobalCounter, RemapRules}, dynamic::{ - DynamicMessage, DynamicSerdeCdrSerdes, MessageSchema, TypeDescriptionClient, - TypeDescriptionService, + DiscoveredTopicSchema, DynPubBuilder, DynSubBuilder, DynamicMessage, DynamicSerdeCdrSerdes, + MessageSchema, SchemaDiscovery, TypeDescriptionService, discovered_schema_type_info, + schema_type_info, }, - entity::{EntityKind, *}, + entity::*, + extended_type_description_service::ExtendedTypeDescriptionService, graph::Graph, msg::{ZMessage, ZService}, parameter::{ Parameter, ParameterDescriptor, ParameterValue, SetParametersResult, service::{ParameterService, ParameterServiceConfig}, }, - pubsub::{ZPub, ZPubBuilder, ZSub, ZSubBuilder}, + pubsub::{ZPubBuilder, ZSubBuilder}, service::{ZClientBuilder, ZServerBuilder}, }; @@ -50,6 +52,9 @@ pub struct ZNode { /// Enabled via `ZNodeBuilder::with_type_description_service()`. /// The service uses callback mode and requires no background task. type_desc_service: Option, + /// Optional ros-z-specific extended type description service. + /// Enabled via `ZNodeBuilder::with_extended_type_description_service()`. + extended_type_desc_service: Option, /// Parameter service providing ROS 2-compatible parameter management. /// Enabled by default; disable via `ZNodeBuilder::without_parameters()`. parameter_service: Option, @@ -77,6 +82,8 @@ pub struct ZNodeBuilder { pub(crate) keyexpr_format: ros_z_protocol::KeyExprFormat, /// Whether to enable the type description service for this node. pub(crate) enable_type_desc_service: bool, + /// Whether to enable the extended type description service for this node. + pub(crate) enable_extended_type_desc_service: bool, /// Whether to enable parameter services for this node (default: true). pub(crate) enable_parameters: bool, /// Initial parameter overrides applied at declaration time. @@ -146,6 +153,15 @@ impl ZNodeBuilder { self } + /// Enable the ros-z-specific extended type description service for this node. + /// + /// When enabled, the node exposes `~get_extended_type_description` for + /// extended-only schemas such as enums and `Option` fields. + pub fn with_extended_type_description_service(mut self) -> Self { + self.enable_extended_type_desc_service = true; + self + } + /// Disable the parameter services for this node. /// /// By default, every node exposes the standard ROS 2 parameter services @@ -248,6 +264,22 @@ impl Builder for ZNodeBuilder { None }; + let extended_type_desc_service = if self.enable_extended_type_desc_service { + debug!("[NOD] Creating extended type description service"); + let service = ExtendedTypeDescriptionService::new( + self.session.clone(), + &self.name, + &self.namespace, + id, + &self.counter, + &self.clock, + )?; + info!("[NOD] ExtendedTypeDescriptionService created"); + Some(service) + } else { + None + }; + // Create parameter service if enabled (default) let parameter_service = if self.enable_parameters { debug!("[NOD] Creating parameter service"); @@ -280,6 +312,7 @@ impl Builder for ZNodeBuilder { shm_config: self.shm_config, keyexpr_format: self.keyexpr_format, type_desc_service, + extended_type_desc_service, parameter_service, }) } @@ -304,32 +337,27 @@ impl ZNode { debug!("[NOD] Creating publisher: topic={}", topic); let mut builder = self.create_pub_impl(topic, Some(T::type_info())); - if let Some(service) = &self.type_desc_service { - match T::message_schema() { - Some(schema) => { - if let Err(e) = service.register_schema(schema.clone()) { - warn!( - "[NOD] Failed to register static schema {} with type description service: {}", - schema.type_name, e - ); - } else { - debug!( - "[NOD] Registered static schema {} with type description service", - schema.type_name - ); - } - - builder = builder.with_dyn_schema(schema); - } - None => { - debug!( - "[NOD] No static schema provided for {}, skipping type description registration", - std::any::type_name::() - ); - } + match T::message_schema() { + Some(schema) => { + self.register_schema_with_type_description_service(&schema); + builder = builder.with_dyn_schema(schema); + } + None => { + debug!( + "[NOD] No static schema provided for {}, skipping type description registration", + std::any::type_name::() + ); } } + if let Err(e) = T::register_type_extensions(self) { + warn!( + "[NOD] Failed to register non-standard schema extensions for {}: {}", + std::any::type_name::(), + e + ); + } + builder } @@ -346,11 +374,11 @@ impl ZNode { // to allow error handling in the Result type let entity = EndpointEntity { id: self.counter.increment(), - node: self.entity.clone(), + node: Some(self.entity.clone()), + kind: EndpointKind::Publisher, topic: topic.to_string(), - kind: EntityKind::Publisher, type_info, - ..Default::default() + qos: Default::default(), }; ZPubBuilder { entity, @@ -394,11 +422,11 @@ impl ZNode { // to allow error handling in the Result type let entity = EndpointEntity { id: self.counter.increment(), - node: self.entity.clone(), + node: Some(self.entity.clone()), + kind: EndpointKind::Subscription, topic: topic.to_string(), - kind: EntityKind::Subscription, type_info, - ..Default::default() + qos: Default::default(), }; ZSubBuilder { entity, @@ -475,11 +503,11 @@ impl ZNode { // to allow error handling in the Result type let entity = EndpointEntity { id: self.counter.increment(), - node: self.entity.clone(), + node: Some(self.entity.clone()), + kind: EndpointKind::Service, topic: name.to_string(), - kind: EntityKind::Service, type_info, - ..Default::default() + qos: Default::default(), }; ZServerBuilder { entity, @@ -515,11 +543,11 @@ impl ZNode { // to allow error handling in the Result type let entity = EndpointEntity { id: self.counter.increment(), - node: self.entity.clone(), + node: Some(self.entity.clone()), + kind: EndpointKind::Client, topic: name.to_string(), - kind: EntityKind::Client, type_info, - ..Default::default() + qos: Default::default(), }; ZClientBuilder { entity, @@ -553,7 +581,7 @@ impl ZNode { use zenoh::qos::CongestionControl; use crate::{ - entity::{EndpointEntity, EntityKind}, + entity::{EndpointEntity, EndpointKind}, topic_name, }; @@ -565,9 +593,9 @@ impl ZNode { let entity = EndpointEntity { id: self.counter.increment(), - node: self.entity.clone(), + node: Some(self.entity.clone()), + kind: EndpointKind::Publisher, topic: qualified_topic.clone(), - kind: EntityKind::Publisher, type_info: Some(TypeInfo { name: type_name.to_string(), hash: TypeHash::from_rihs_string(type_hash).unwrap_or(TypeHash::zero()), @@ -626,7 +654,7 @@ impl ZNode { F: Fn(&[u8]) + Send + Sync + 'static, { use crate::{ - entity::{EndpointEntity, EntityKind}, + entity::{EndpointEntity, EndpointKind}, topic_name, }; @@ -638,9 +666,9 @@ impl ZNode { let entity = EndpointEntity { id: self.counter.increment(), - node: self.entity.clone(), + node: Some(self.entity.clone()), + kind: EndpointKind::Subscription, topic: qualified_topic.clone(), - kind: EntityKind::Subscription, type_info: Some(TypeInfo { name: type_name.to_string(), hash: TypeHash::from_rihs_string(type_hash).unwrap_or(TypeHash::zero()), @@ -696,6 +724,23 @@ impl ZNode { self.type_desc_service.is_some() } + /// Get a reference to this node's extended type description service, if enabled. + pub fn extended_type_description_service(&self) -> Option<&ExtendedTypeDescriptionService> { + self.extended_type_desc_service.as_ref() + } + + /// Get a mutable reference to this node's extended type description service, if enabled. + pub fn extended_type_description_service_mut( + &mut self, + ) -> Option<&mut ExtendedTypeDescriptionService> { + self.extended_type_desc_service.as_mut() + } + + /// Check if this node has an extended type description service. + pub fn has_extended_type_description_service(&self) -> bool { + self.extended_type_desc_service.is_some() + } + /// Get access to the global counter for entity ID generation. pub fn counter(&self) -> &Arc { &self.counter @@ -834,76 +879,42 @@ impl ZNode { /// .field("data", FieldType::String) /// .build()?; /// - /// let publisher = node.create_dyn_pub("chatter", schema)?; + /// let publisher = node.create_dyn_pub("chatter", schema).build()?; /// /// let mut msg = DynamicMessage::new(publisher.schema()); /// msg.set("data", "Hello, world!")?; /// publisher.publish(&msg)?; /// ``` - pub fn create_dyn_pub( + pub fn create_dyn_pub(&self, topic: &str, schema: Arc) -> DynPubBuilder { + self.register_schema_with_type_description_service(&schema); + self.create_dyn_pub_impl(topic, Some(schema_type_info(&schema)), schema) + } + + /// Discover the schema that publishers currently expose on a topic. + /// + /// The topic name is qualified according to the same ROS 2 rules as the + /// regular publisher and subscriber builder APIs. + pub async fn discover_topic_schema( &self, topic: &str, - schema: Arc, - ) -> Result> { - // Register schema with type description service if enabled - if let Some(service) = &self.type_desc_service { - if let Err(e) = service.register_schema(schema.clone()) { - warn!( - "[NOD] Failed to register schema {} with type description service: {}", - schema.type_name, e - ); - } else { - debug!( - "[NOD] Registered schema {} with type description service", - schema.type_name - ); - } - } - - // Create TypeInfo from schema for proper key expression matching - // Convert ROS 2 canonical name to DDS name - // "std_msgs/msg/String" → "std_msgs::msg::dds_::String_" - let dds_name = schema - .type_name - .replace("/msg/", "::msg::dds_::") - .replace("/srv/", "::srv::dds_::") - .replace("/action/", "::action::dds_::") - + "_"; - - // Compute type hash and convert to entity::TypeHash format - use crate::dynamic::MessageSchemaTypeDescription; - let type_hash = match schema.compute_type_hash() { - Ok(hash) => { - let rihs_string = hash.to_rihs_string(); - crate::entity::TypeHash::from_rihs_string(&rihs_string) - .unwrap_or_else(crate::entity::TypeHash::zero) - } - Err(e) => { - warn!( - "[NOD] Failed to compute type hash for {}: {}", - schema.type_name, e - ); - crate::entity::TypeHash::zero() - } - }; - - let type_info = Some(crate::entity::TypeInfo { - name: dds_name, - hash: type_hash, - }); - - // Build the publisher - self.create_pub_impl::(topic, type_info) - .with_serdes::() - .with_dyn_schema(schema) - .build() + discovery_timeout: Duration, + ) -> std::result::Result { + SchemaDiscovery::new(self, discovery_timeout) + .discover(topic) + .await } /// Create a dynamic subscriber with automatic schema discovery. /// /// This method queries publishers on the topic for their type description - /// and creates a subscriber using the discovered schema. This is useful - /// when you don't know the message type at compile time. + /// and returns a preconfigured subscriber builder using the discovered + /// schema. This is useful when you don't know the message type at compile + /// time. + /// + /// The topic name will be qualified according to ROS 2 rules: + /// - Absolute topics (starting with '/') are used as-is + /// - Private topics (starting with '~') are expanded to /// + /// - Relative topics are expanded to // /// /// # Arguments /// @@ -912,19 +923,19 @@ impl ZNode { /// /// # Returns /// - /// A tuple of (subscriber, schema) on success. The schema is returned - /// so you can use it to create messages or inspect the type. + /// A preconfigured dynamic subscriber builder on success. /// /// # Example /// /// ```ignore /// // Discover schema from publishers and create subscriber - /// let (subscriber, schema) = node.create_dyn_sub_auto( + /// let subscriber = node.create_dyn_sub_auto( /// "chatter", /// Duration::from_secs(5), - /// ).await?; + /// ).await? + /// .build()?; /// - /// println!("Discovered type: {}", schema.type_name); + /// println!("Discovered type: {}", subscriber.schema().unwrap().type_name); /// /// // Receive messages /// let msg = subscriber.recv()?; @@ -934,61 +945,27 @@ impl ZNode { &self, topic: &str, discovery_timeout: Duration, - ) -> Result<( - ZSub, - Arc, - )> { + ) -> Result { debug!( "[NOD] Creating dynamic subscriber with auto-discovery for topic: {}", topic ); - // Create a TypeDescriptionClient to discover the schema. - // Use a short per-attempt timeout (3 s) so that transient Zenoh routing - // delays can be recovered by the retry loop inside get_type_description_for_topic - // rather than burning the entire discovery_timeout on a single query. - let client = TypeDescriptionClient::with_graph( - self.session.clone(), - self.counter.clone(), - self.graph.clone(), - ) - .with_timeout(Duration::from_secs(3)); - - // Discover schema from topic publishers - let (schema, type_hash) = client - .get_type_description_for_topic(topic, discovery_timeout) + let discovered = self + .discover_topic_schema(topic, discovery_timeout) .await - .map_err(|e| zenoh::Error::from(format!("Schema discovery failed: {}", e)))?; + .map_err(|error| zenoh::Error::from(error.to_string()))?; info!( "[NOD] Discovered schema for topic {}: {} (hash: {})", - topic, schema.type_name, type_hash + discovered.qualified_topic, discovered.schema.type_name, discovered.type_hash ); - // Create TypeInfo from discovered schema for proper key expression matching - // Convert ROS 2 canonical name to DDS name - // "std_msgs/msg/String" → "std_msgs::msg::dds_::String_" - let dds_name = schema - .type_name - .replace("/msg/", "::msg::dds_::") - .replace("/srv/", "::srv::dds_::") - .replace("/action/", "::action::dds_::") - + "_"; - - let type_info = Some(crate::entity::TypeInfo { - name: dds_name, - hash: crate::entity::TypeHash::from_rihs_string(&type_hash) - .unwrap_or_else(crate::entity::TypeHash::zero), - }); - - // Build the subscriber with the discovered schema - let subscriber = self - .create_sub_impl::(topic, type_info) - .with_serdes::() - .with_dyn_schema(schema.clone()) - .build()?; - - Ok((subscriber, schema)) + Ok(self.create_dyn_sub_impl( + topic, + Some(discovered_schema_type_info(&discovered)), + discovered.schema, + )) } /// Create a dynamic subscriber with a known schema. @@ -1001,6 +978,11 @@ impl ZNode { /// * `topic` - The topic name to subscribe to /// * `schema` - The message schema for deserialization /// + /// The topic name will be qualified according to ROS 2 rules: + /// - Absolute topics (starting with '/') are used as-is + /// - Private topics (starting with '~') are expanded to /// + /// - Relative topics are expanded to // + /// /// # Example /// /// ```ignore @@ -1008,51 +990,49 @@ impl ZNode { /// .field("data", FieldType::String) /// .build()?; /// - /// let subscriber = node.create_dyn_sub("chatter", schema)?; + /// let subscriber = node.create_dyn_sub("chatter", schema).build()?; /// let msg = subscriber.recv()?; /// ``` - pub fn create_dyn_sub( + pub fn create_dyn_sub(&self, topic: &str, schema: Arc) -> DynSubBuilder { + self.create_dyn_sub_impl(topic, Some(schema_type_info(&schema)), schema) + } + + fn create_dyn_pub_impl( &self, topic: &str, + type_info: Option, schema: Arc, - ) -> Result> { - // Create TypeInfo from schema for proper key expression matching - // Convert ROS 2 canonical name to DDS name - // "std_msgs/msg/String" → "std_msgs::msg::dds_::String_" - let dds_name = schema - .type_name - .replace("/msg/", "::msg::dds_::") - .replace("/srv/", "::srv::dds_::") - .replace("/action/", "::action::dds_::") - + "_"; - - // Compute type hash and convert to entity::TypeHash format - use crate::dynamic::MessageSchemaTypeDescription; - let type_hash = match schema.compute_type_hash() { - Ok(hash) => { - let rihs_string = hash.to_rihs_string(); - crate::entity::TypeHash::from_rihs_string(&rihs_string) - .unwrap_or_else(crate::entity::TypeHash::zero) - } - Err(e) => { - warn!( - "[NOD] Failed to compute type hash for {}: {}", - schema.type_name, e - ); - crate::entity::TypeHash::zero() - } - }; - - let type_info = Some(crate::entity::TypeInfo { - name: dds_name, - hash: type_hash, - }); + ) -> DynPubBuilder { + self.create_pub_impl::(topic, type_info) + .with_serdes::() + .with_dyn_schema(schema) + } - // Build the subscriber with proper type info + fn create_dyn_sub_impl( + &self, + topic: &str, + type_info: Option, + schema: Arc, + ) -> DynSubBuilder { self.create_sub_impl::(topic, type_info) .with_serdes::() .with_dyn_schema(schema) - .build() + } + + fn register_schema_with_type_description_service(&self, schema: &Arc) { + if let Some(service) = &self.type_desc_service { + if let Err(error) = service.register_schema(Arc::clone(schema)) { + warn!( + "[NOD] Failed to register schema {} with type description service: {}", + schema.type_name, error + ); + } else { + debug!( + "[NOD] Registered schema {} with type description service", + schema.type_name + ); + } + } } } @@ -1062,11 +1042,14 @@ mod tests { #[test] fn test_node_entity_name_namespace() { - let entity = NodeEntity { - name: "my_node".to_string(), - namespace: "/my_ns".to_string(), - ..Default::default() - }; + let entity = NodeEntity::new( + 0, + "1234567890abcdef1234567890abcdef".parse().unwrap(), + 0, + "my_node".to_string(), + "/my_ns".to_string(), + String::new(), + ); assert_eq!(entity.name, "my_node"); assert_eq!(entity.namespace, "/my_ns"); } diff --git a/crates/ros-z/src/parameter/client.rs b/crates/ros-z/src/parameter/client.rs index e4bc7992..174e04cf 100644 --- a/crates/ros-z/src/parameter/client.rs +++ b/crates/ros-z/src/parameter/client.rs @@ -135,12 +135,12 @@ impl ParameterClient { /// Describe remote parameters by name. pub async fn describe(&self, names: &[impl AsRef]) -> Result> { - self.describe_client - .send_request(&DescribeParametersRequest { + let response = self + .describe_client + .call(&DescribeParametersRequest { names: names.iter().map(|name| name.as_ref().to_string()).collect(), }) .await?; - let response = self.describe_client.async_take_response().await?; Ok(response .descriptors .iter() @@ -150,12 +150,12 @@ impl ParameterClient { /// Fetch remote parameter values by name. pub async fn get(&self, names: &[impl AsRef]) -> Result> { - self.get_client - .send_request(&GetParametersRequest { + let response = self + .get_client + .call(&GetParametersRequest { names: names.iter().map(|name| name.as_ref().to_string()).collect(), }) .await?; - let response = self.get_client.async_take_response().await?; Ok(response .values .iter() @@ -165,12 +165,12 @@ impl ParameterClient { /// Fetch remote parameter types by name. pub async fn get_types(&self, names: &[impl AsRef]) -> Result> { - self.get_types_client - .send_request(&GetParameterTypesRequest { + let response = self + .get_types_client + .call(&GetParameterTypesRequest { names: names.iter().map(|name| name.as_ref().to_string()).collect(), }) .await?; - let response = self.get_types_client.async_take_response().await?; Ok(response .types .into_iter() @@ -184,8 +184,9 @@ impl ParameterClient { prefixes: &[impl AsRef], depth: Option, ) -> Result { - self.list_client - .send_request(&ListParametersRequest { + let response = self + .list_client + .call(&ListParametersRequest { prefixes: prefixes .iter() .map(|prefix| prefix.as_ref().to_string()) @@ -193,7 +194,6 @@ impl ParameterClient { depth: depth.unwrap_or(DEPTH_RECURSIVE), }) .await?; - let response = self.list_client.async_take_response().await?; Ok(ParameterList { names: response.result.names, prefixes: response.result.prefixes, @@ -202,12 +202,12 @@ impl ParameterClient { /// Set one or more remote parameters non-atomically. pub async fn set(&self, parameters: &[Parameter]) -> Result> { - self.set_client - .send_request(&SetParametersRequest { + let response = self + .set_client + .call(&SetParametersRequest { parameters: parameters.iter().map(Parameter::to_wire).collect(), }) .await?; - let response = self.set_client.async_take_response().await?; Ok(response .results .into_iter() @@ -220,12 +220,12 @@ impl ParameterClient { /// Set remote parameters atomically. pub async fn set_atomically(&self, parameters: &[Parameter]) -> Result { - self.set_atomically_client - .send_request(&SetParametersAtomicallyRequest { + let response = self + .set_atomically_client + .call(&SetParametersAtomicallyRequest { parameters: parameters.iter().map(Parameter::to_wire).collect(), }) .await?; - let response = self.set_atomically_client.async_take_response().await?; Ok(SetParametersResult { successful: response.result.successful, reason: response.result.reason, diff --git a/crates/ros-z/src/parameter/service.rs b/crates/ros-z/src/parameter/service.rs index 334eb3a6..fd2b1d9f 100644 --- a/crates/ros-z/src/parameter/service.rs +++ b/crates/ros-z/src/parameter/service.rs @@ -24,7 +24,7 @@ use crate::{ Builder, ServiceTypeInfo, attachment::Attachment, context::GlobalCounter, - entity::{EndpointEntity, EntityKind, NodeEntity, TypeInfo}, + entity::{EndpointEntity, EndpointKind, NodeEntity, TypeInfo}, msg::{SerdeCdrSerdes, ZDeserializer, ZSerializer}, pubsub::{ZPub, ZPubBuilder}, qos::{QosDurability, QosHistory, QosProfile, QosReliability}, @@ -284,11 +284,11 @@ impl ParameterService { let make_entity = |counter: &GlobalCounter, service_name: &str, type_info: TypeInfo| EndpointEntity { id: counter.increment(), - node: node_entity.clone(), - kind: EntityKind::Service, + node: Some(node_entity.clone()), + kind: EndpointKind::Service, topic: service_name.to_string(), type_info: Some(type_info), - ..Default::default() + qos: Default::default(), }; let ke_format = ros_z_protocol::KeyExprFormat::default(); @@ -296,8 +296,8 @@ impl ParameterService { // ── /parameter_events publisher ─────────────────────────────────────── let pub_entity = EndpointEntity { id: counter.increment(), - node: node_entity.clone(), - kind: EntityKind::Publisher, + node: Some(node_entity.clone()), + kind: EndpointKind::Publisher, topic: "/parameter_events".to_string(), type_info: Some(parameter_event_type_info()), qos: { diff --git a/crates/ros-z/src/prelude.rs b/crates/ros-z/src/prelude.rs index e90413cb..f0b76bec 100644 --- a/crates/ros-z/src/prelude.rs +++ b/crates/ros-z/src/prelude.rs @@ -24,7 +24,7 @@ pub use crate::Builder; pub use crate::context::{ZContext, ZContextBuilder}; pub use crate::node::ZNode; pub use crate::pubsub::{ZPub, ZSub}; -pub use crate::service::{QueryKey, ZClient, ZServer}; +pub use crate::service::{RequestId, ServiceReply, ServiceRequest, ZClient, ZServer}; pub use crate::action::server::{Accepted, Executing, Requested}; /// Action types. @@ -39,7 +39,10 @@ pub use crate::qos::{ pub use crate::action::ZAction; /// Trait bounds for custom messages and services. -pub use crate::ros_msg::{ActionTypeInfo, MessageTypeInfo, ServiceTypeInfo, WithTypeInfo}; +pub use crate::{ + ExtendedMessageTypeInfo, + ros_msg::{ActionTypeInfo, MessageTypeInfo, ServiceTypeInfo, WithTypeInfo}, +}; /// Type identity helpers for custom message definitions. pub use crate::entity::{TypeHash, TypeInfo}; diff --git a/crates/ros-z/src/pubsub.rs b/crates/ros-z/src/pubsub.rs index 4b109e5c..f6e75c15 100644 --- a/crates/ros-z/src/pubsub.rs +++ b/crates/ros-z/src/pubsub.rs @@ -102,7 +102,7 @@ impl ZPubBuilder { /// let provider = Arc::new(ShmProviderBuilder::new(20 * 1024 * 1024).build()?); /// let config = ShmConfig::new(provider).with_threshold(5_000); /// - /// let pub = node.create_pub::("topic") + /// let publisher = node.create_pub::("topic") /// .with_shm_config(config) /// .build()?; /// # Ok(()) @@ -126,7 +126,7 @@ impl ZPubBuilder { /// # let ctx = ros_z::context::ZContextBuilder::default().with_shm_enabled()?.build()?; /// # let node = ctx.create_node("test").build()?; /// // Context has SHM enabled, but disable for this publisher - /// let pub = node.create_pub::("small_messages") + /// let publisher = node.create_pub::("small_messages") /// .without_shm() /// .build()?; /// # Ok(()) @@ -172,7 +172,7 @@ impl ZPubBuilder { /// # let ctx = ros_z::context::ZContextBuilder::default().build()?; /// # let node = ctx.create_node("test").build()?; /// // Publish with Protobuf encoding - /// let pub = node.create_pub::("/topic") + /// let publisher = node.create_pub::("/topic") /// .with_encoding(Encoding::protobuf().with_schema("geometry_msgs/msg/Point")) /// .build()?; /// # Ok(()) @@ -199,35 +199,10 @@ impl ZPubBuilder { /// .build()?; /// ``` pub fn with_dyn_schema(mut self, schema: Arc) -> Self { - use crate::dynamic::MessageSchemaTypeDescription; - // Only compute and set type_info if it hasn't been set already // (e.g., from create_dyn_sub_auto which provides the publisher's hash) if self.entity.type_info.is_none() { - // Compute TypeInfo from schema for proper key expression matching with ROS 2 - // Convert ROS 2 canonical name to DDS name - // "std_msgs/msg/String" → "std_msgs::msg::dds_::String_" - let dds_name = schema - .type_name - .replace("/msg/", "::msg::dds_::") - .replace("/srv/", "::srv::dds_::") - .replace("/action/", "::action::dds_::") - + "_"; - - // Convert schema TypeHash to entity TypeHash via RIHS string - let type_hash = match schema.compute_type_hash() { - Ok(hash) => { - let rihs_string = hash.to_rihs_string(); - crate::entity::TypeHash::from_rihs_string(&rihs_string) - .unwrap_or_else(crate::entity::TypeHash::zero) - } - Err(_) => crate::entity::TypeHash::zero(), - }; - - self.entity.type_info = Some(crate::entity::TypeInfo { - name: dds_name, - hash: type_hash, - }); + self.entity.type_info = Some(crate::dynamic::schema_type_info(&schema)); } self.dyn_schema = Some(schema); @@ -248,13 +223,13 @@ where qos_durability = ?self.entity.qos.durability ))] fn build(mut self) -> Result { + let Some(node) = self.entity.node.as_ref() else { + return Err(zenoh::Error::from("publisher build requires node identity")); + }; // Qualify the topic name according to ROS 2 rules - let qualified_topic = topic_name::qualify_topic_name( - &self.entity.topic, - &self.entity.node.namespace, - &self.entity.node.name, - ) - .map_err(|e| zenoh::Error::from(format!("Failed to qualify topic: {}", e)))?; + let qualified_topic = + topic_name::qualify_topic_name(&self.entity.topic, &node.namespace, &node.name) + .map_err(|e| zenoh::Error::from(format!("Failed to qualify topic: {}", e)))?; self.entity.topic = qualified_topic.clone(); debug!("[PUB] Qualified topic: {}", qualified_topic); @@ -660,35 +635,10 @@ where /// .build()?; /// ``` pub fn with_dyn_schema(mut self, schema: Arc) -> Self { - use crate::dynamic::MessageSchemaTypeDescription; - // Only compute and set type_info if it hasn't been set already // (e.g., from create_dyn_sub_auto which provides the publisher's hash) if self.entity.type_info.is_none() { - // Compute TypeInfo from schema for proper key expression matching with ROS 2 - // Convert ROS 2 canonical name to DDS name - // "std_msgs/msg/String" → "std_msgs::msg::dds_::String_" - let dds_name = schema - .type_name - .replace("/msg/", "::msg::dds_::") - .replace("/srv/", "::srv::dds_::") - .replace("/action/", "::action::dds_::") - + "_"; - - // Convert schema TypeHash to entity TypeHash via RIHS string - let type_hash = match schema.compute_type_hash() { - Ok(hash) => { - let rihs_string = hash.to_rihs_string(); - crate::entity::TypeHash::from_rihs_string(&rihs_string) - .unwrap_or_else(crate::entity::TypeHash::zero) - } - Err(_) => crate::entity::TypeHash::zero(), - }; - - self.entity.type_info = Some(crate::entity::TypeInfo { - name: dds_name, - hash: type_hash, - }); + self.entity.type_info = Some(crate::dynamic::schema_type_info(&schema)); } self.dyn_schema = Some(schema); @@ -713,12 +663,14 @@ where where F: Fn(Sample) + Send + Sync + 'static, { - let qualified_topic = crate::topic_name::qualify_topic_name( - &self.entity.topic, - &self.entity.node.namespace, - &self.entity.node.name, - ) - .map_err(|e| zenoh::Error::from(format!("Failed to qualify topic: {}", e)))?; + let Some(node) = self.entity.node.as_ref() else { + return Err(zenoh::Error::from( + "subscriber build requires node identity", + )); + }; + let qualified_topic = + crate::topic_name::qualify_topic_name(&self.entity.topic, &node.namespace, &node.name) + .map_err(|e| zenoh::Error::from(format!("Failed to qualify topic: {}", e)))?; self.entity.topic = qualified_topic.clone(); debug!("[CACHE] Qualified topic: {}", qualified_topic); @@ -754,12 +706,14 @@ where where S: ZDeserializer, { - let qualified_topic = topic_name::qualify_topic_name( - &self.entity.topic, - &self.entity.node.namespace, - &self.entity.node.name, - ) - .map_err(|e| zenoh::Error::from(format!("Failed to qualify topic: {}", e)))?; + let Some(node) = self.entity.node.as_ref() else { + return Err(zenoh::Error::from( + "subscriber build requires node identity", + )); + }; + let qualified_topic = + topic_name::qualify_topic_name(&self.entity.topic, &node.namespace, &node.name) + .map_err(|e| zenoh::Error::from(format!("Failed to qualify topic: {}", e)))?; self.entity.topic = qualified_topic.clone(); debug!("[SUB] Qualified topic: {}", qualified_topic); @@ -1270,8 +1224,12 @@ mod tests { #[test] fn test_endpoint_entity_topic_field() { let entity = ros_z_protocol::entity::EndpointEntity { + id: 0, + node: None, + kind: ros_z_protocol::entity::EndpointKind::Publisher, topic: "/my_topic".to_string(), - ..Default::default() + type_info: None, + qos: Default::default(), }; assert_eq!(entity.topic, "/my_topic"); } diff --git a/crates/ros-z/src/ros_msg.rs b/crates/ros-z/src/ros_msg.rs index 0c6caa6a..720cec5b 100644 --- a/crates/ros-z/src/ros_msg.rs +++ b/crates/ros-z/src/ros_msg.rs @@ -47,6 +47,15 @@ pub trait MessageTypeInfo { None } + /// Register any non-standard schema discovery hooks for this type on the node. + /// + /// Core ros-z keeps the standard type-description path separate, so the + /// default implementation is a no-op. Extended schema derives override this + /// to register with ros-z's parallel extended type description service. + fn register_type_extensions(_node: &crate::node::ZNode) -> std::result::Result<(), String> { + Ok(()) + } + // === Dynamic Methods (Runtime) === /// Returns the ROS message type name at runtime diff --git a/crates/ros-z/src/service.rs b/crates/ros-z/src/service.rs index 39a15a25..7d6777fc 100644 --- a/crates/ros-z/src/service.rs +++ b/crates/ros-z/src/service.rs @@ -1,19 +1,15 @@ #![allow(unused)] use std::{ - collections::HashMap, marker::PhantomData, - sync::{Arc, atomic::AtomicUsize}, + sync::{Arc, Mutex, atomic::AtomicUsize}, time::Duration, }; use serde::Deserialize; use tracing::{debug, error, info, trace, warn}; use zenoh::{ - Result, Session, Wait, bytes, - key_expr::KeyExpr, - liveliness::LivelinessToken, - query::{Query, Reply}, + Result, Session, Wait, bytes, key_expr::KeyExpr, liveliness::LivelinessToken, query::Query, sample::Sample, }; @@ -45,13 +41,10 @@ pub struct ZClientBuilder { impl_with_type_info!(ZClientBuilder); impl_with_type_info!(ZServerBuilder); -/// A ROS 2-style service client that sends typed requests and receives typed responses. +/// A ROS 2-style reusable service handle for typed request/response calls. /// /// Create a client via [`ZNode::create_client`](crate::node::ZNode::create_client). -/// Send a request with [`send_request`](ZClient::send_request) (async), then retrieve -/// the response with [`take_response`](ZClient::take_response) (non-blocking), -/// [`take_response_timeout`](ZClient::take_response_timeout) (waits up to a deadline), -/// or [`async_take_response`](ZClient::async_take_response) (async wait). +/// Invoke the service with [`call`](ZClient::call) or [`call_or_timeout`](ZClient::call_or_timeout). /// /// # Example /// @@ -60,8 +53,7 @@ impl_with_type_info!(ZServerBuilder); /// use std::time::Duration; /// /// // client: ZClient -/// client.send_request(&request).await?; -/// let response = client.take_response_timeout(Duration::from_secs(5))?; +/// let response = client.call_or_timeout(&request, Duration::from_secs(5)).await?; /// ``` pub struct ZClient { // TODO: replace this with the sample sn @@ -70,10 +62,12 @@ pub struct ZClient { gid: GidArray, inner: zenoh::query::Querier<'static>, lv_token: LivelinessToken, - tx: flume::Sender, - pub(crate) rx: flume::Receiver, topic: String, clock: crate::time::ZClock, + #[cfg(feature = "rmw")] + completed_tx: flume::Sender, + #[cfg(feature = "rmw")] + completed_rx: flume::Receiver, _phantom_data: PhantomData, } @@ -95,13 +89,13 @@ where service = %self.entity.topic ))] fn build(mut self) -> Result { + let Some(node) = self.entity.node.as_ref() else { + return Err(zenoh::Error::from("client build requires node identity")); + }; // Qualify the service name according to ROS 2 rules - let qualified_service = topic_name::qualify_service_name( - &self.entity.topic, - &self.entity.node.namespace, - &self.entity.node.name, - ) - .map_err(|e| zenoh::Error::from(format!("Failed to qualify service: {}", e)))?; + let qualified_service = + topic_name::qualify_service_name(&self.entity.topic, &node.namespace, &node.name) + .map_err(|e| zenoh::Error::from(format!("Failed to qualify service: {}", e)))?; self.entity.topic = qualified_service.clone(); debug!("[CLN] Qualified service: {}", qualified_service); @@ -125,12 +119,14 @@ where .liveliness() .declare_token((*lv_ke).clone()) .wait()?; - // Use bounded channel based on QoS depth - let depth = match self.entity.qos.history { - ros_z_protocol::qos::QosHistory::KeepLast(n) => n, - ros_z_protocol::qos::QosHistory::KeepAll => 1000, // Default reasonable limit for KeepAll + #[cfg(feature = "rmw")] + let (completed_tx, completed_rx) = { + let depth = match self.entity.qos.history { + ros_z_protocol::qos::QosHistory::KeepLast(n) => n, + ros_z_protocol::qos::QosHistory::KeepAll => 1000, + }; + flume::bounded(depth) }; - let (tx, rx) = flume::bounded(depth); debug!("[CLN] Client ready: service={}", self.entity.topic); Ok(ZClient { @@ -138,10 +134,12 @@ where inner, lv_token, gid: crate::entity::endpoint_gid(&self.entity), - tx, - rx, topic: self.entity.topic.clone(), clock: self.clock, + #[cfg(feature = "rmw")] + completed_tx, + #[cfg(feature = "rmw")] + completed_rx, _phantom_data: Default::default(), }) } @@ -159,69 +157,73 @@ where ) } - /// Access the raw response receiver channel. - pub fn rx(&self) -> &flume::Receiver { - &self.rx - } + async fn call_sample(&self, payload: impl Into) -> Result { + let attachment = self.new_attachment(); + let (response_tx, response_rx) = tokio::sync::oneshot::channel(); + let response_tx = Arc::new(Mutex::new(Some(response_tx))); - pub fn take_sample(&self) -> Result { - match self.rx.try_recv() { - Ok(sample) => Ok(sample), - Err(flume::TryRecvError::Empty) => Err("No sample available".into()), - Err(flume::TryRecvError::Disconnected) => Err("Channel disconnected".into()), - } - } + self.inner + .get() + .payload(payload) + .attachment(attachment) + .callback(move |reply| match reply.into_result() { + Ok(sample) => { + let sender = response_tx + .lock() + .expect("service reply sender mutex poisoned") + .take(); + match sender { + Some(sender) => { + if sender.send(sample).is_err() { + tracing::warn!( + "Service call receiver dropped before reply delivery" + ); + } + } + None => { + tracing::warn!("Service call received extra reply after completion"); + } + } + } + Err(error) => { + tracing::debug!("Service reply error: {error:?}"); + } + }) + .await?; - pub fn take_sample_timeout(&self, timeout: Duration) -> Result { - Ok(self.rx.recv_timeout(timeout)?) - } + let sample = response_rx.await.map_err(|_| { + zenoh::Error::from("Service call ended before any response was received") + })?; - /// Retrieve the next response without blocking. - /// - /// Returns `Err` immediately if no response has arrived yet. Use - /// [`take_response_timeout`](ZClient::take_response_timeout) to wait up to a - /// deadline, or [`async_take_response`](ZClient::async_take_response) to await - /// indefinitely in an async context. - // For ROS-Z - pub fn take_response(&self) -> Result - where - T::Response: ZMessage, - for<'a> ::Serdes: - ZDeserializer = &'a [u8]>, - { - let sample = self.take_sample()?; - let msg = ::deserialize(&sample.payload().to_bytes()) - .map_err(|e| zenoh::Error::from(e.to_string()))?; - Ok(msg) + Ok(sample) } - /// Wait for the next response, up to `timeout`. Returns `Err` if no response - /// arrives within the deadline. - pub fn take_response_timeout(&self, timeout: Duration) -> Result + /// Call the service and wait indefinitely for the first reply. + pub async fn call(&self, msg: &T::Request) -> Result where + T::Request: ZMessage, T::Response: ZMessage, for<'a> ::Serdes: ZDeserializer = &'a [u8]>, { - let sample = self.take_sample_timeout(timeout)?; + let sample = self.call_sample(msg.serialize()).await?; let payload_bytes = sample.payload().to_bytes(); let msg = ::deserialize(&payload_bytes[..]) .map_err(|e| zenoh::Error::from(e.to_string()))?; Ok(msg) } - /// Asynchronously wait for the next response. Awaits indefinitely until a - /// response arrives or the channel is disconnected. - pub async fn async_take_response(&self) -> Result + + /// Call the service and fail if no reply arrives before `timeout` elapses. + pub async fn call_or_timeout(&self, msg: &T::Request, timeout: Duration) -> Result where + T::Request: ZMessage, T::Response: ZMessage, for<'a> ::Serdes: ZDeserializer = &'a [u8]>, { - let sample = self.rx.recv_async().await?; - let payload_bytes = sample.payload().to_bytes(); - let msg = ::deserialize(&payload_bytes[..]) - .map_err(|e| zenoh::Error::from(e.to_string()))?; - Ok(msg) + tokio::time::timeout(timeout, self.call(msg)) + .await + .map_err(|_| zenoh::Error::from(format!("Service call timed out after {timeout:?}")))? } } @@ -229,66 +231,17 @@ impl ZClient where T: ZService, { - /// Send a typed request to the service server. - /// - /// This is an `async fn` — it must be `.await`ed. The call resolves once the - /// Zenoh query is dispatched; it does **not** wait for a response. Retrieve the - /// response separately with [`take_response`](ZClient::take_response), - /// [`take_response_timeout`](ZClient::take_response_timeout), or - /// [`async_take_response`](ZClient::async_take_response). - /// - /// Succeeds even when no server is running (fire-and-forget dispatch). - #[tracing::instrument(name = "send_request", skip(self, msg), fields( + #[cfg(feature = "rmw")] + #[tracing::instrument(name = "rmw_send_request", skip(self, msg, notify), fields( service = %self.topic, sn = self.sn.load(Ordering::Acquire), payload_len = tracing::field::Empty ))] - pub async fn send_request(&self, msg: &T::Request) -> Result<()> { - let payload = msg.serialize(); - tracing::Span::current().record("payload_len", payload.len()); - - // Log the key expression being queried - let query_ke = self.inner.key_expr(); - info!("[CLN] Sending request to key expression: {}", query_ke); - debug!("[CLN] Sending request"); - - let tx = self.tx.clone(); - self.inner - .get() - .payload(payload) - .attachment(self.new_attachment()) - .callback(move |reply| { - match reply.into_result() { - Ok(sample) => { - info!( - "[CLN] Reply received: len={}, kind={:?}", - sample.payload().len(), - sample.kind() - ); - debug!("[CLN] Reply received: len={}", sample.payload().len()); - // Use try_send for bounded channel - if full, drop the response (QoS depth enforcement) - if tx.try_send(sample).is_err() { - tracing::warn!( - "Client response queue full, dropping response (QoS depth enforced)" - ); - } - } - Err(e) => { - warn!("[CLN] Reply error: {:?}", e); - } - } - }) - .await?; - - Ok(()) - } - - #[cfg(feature = "rmw")] pub fn rmw_send_request(&self, msg: &T::Request, notify: F) -> Result where F: Fn() + Send + Sync + 'static, { - let tx = self.tx.clone(); + let completed_tx = self.completed_tx.clone(); let attachment = self.new_attachment(); let sn = attachment.sequence_number; self.inner @@ -298,8 +251,7 @@ where .callback(move |reply| { match reply.into_result() { Ok(sample) => { - // Use try_send for bounded channel - if full, drop the response (QoS depth enforcement) - if tx.try_send(sample).is_err() { + if completed_tx.try_send(sample).is_err() { tracing::warn!( "Client response queue full, dropping response (QoS depth enforced)" ); @@ -316,6 +268,22 @@ where .wait()?; Ok(sn) } + + #[cfg(feature = "rmw")] + pub fn rmw_try_take_response_sample(&self) -> Result> { + match self.completed_rx.try_recv() { + Ok(sample) => Ok(Some(sample)), + Err(flume::TryRecvError::Empty) => Ok(None), + Err(flume::TryRecvError::Disconnected) => { + Err(zenoh::Error::from("Client response channel disconnected")) + } + } + } + + #[cfg(feature = "rmw")] + pub fn rmw_has_responses(&self) -> bool { + !self.completed_rx.is_empty() + } } #[derive(Debug)] @@ -354,17 +322,11 @@ impl ZServerBuilder { } pub struct ZServer { - // NOTE: This is biased toward RMW key_expr: KeyExpr<'static>, - // TODO: replace this with the sample sn - sn: AtomicUsize, - // TODO: replace this with zenoh's global entity id - gid: GidArray, inner: zenoh::query::Queryable<()>, lv_token: LivelinessToken, clock: crate::time::ZClock, pub(crate) queue: Option>>, - pub(crate) map: HashMap, _phantom_data: PhantomData, } @@ -396,16 +358,6 @@ where pub fn try_queue(&self) -> Option<&Arc>> { self.queue.as_ref() } - - /// Number of pending queries stored in the response map. - pub fn map_len(&self) -> usize { - self.map.len() - } - - /// Insert a query into the response map, returning any previously stored query for that key. - pub fn map_insert(&mut self, key: QueryKey, query: Query) -> Option { - self.map.insert(key, query) - } } impl ZServerBuilder @@ -418,12 +370,12 @@ where handler: DataHandler, queue: Option>>, ) -> Result> { - let qualified_service = topic_name::qualify_service_name( - &self.entity.topic, - &self.entity.node.namespace, - &self.entity.node.name, - ) - .map_err(|e| zenoh::Error::from(format!("Failed to qualify service: {}", e)))?; + let Some(node) = self.entity.node.as_ref() else { + return Err(zenoh::Error::from("service build requires node identity")); + }; + let qualified_service = + topic_name::qualify_service_name(&self.entity.topic, &node.namespace, &node.name) + .map_err(|e| zenoh::Error::from(format!("Failed to qualify service: {}", e)))?; self.entity.topic = qualified_service; @@ -472,13 +424,10 @@ where Ok(ZServer { key_expr, - sn: AtomicUsize::new(1), // Start at 1 for ROS compatibility inner, lv_token, clock: self.clock, - gid: crate::entity::endpoint_gid(&self.entity), queue, - map: HashMap::new(), _phantom_data: Default::default(), }) } @@ -527,36 +476,94 @@ where } #[derive(Debug, PartialEq, Eq, Hash, Clone)] -pub struct QueryKey { - pub sn: i64, - pub gid: GidArray, +pub struct RequestId { + pub sequence_number: i64, + pub writer_guid: GidArray, } -impl From for QueryKey { +impl From for RequestId { fn from(value: Attachment) -> Self { Self { - sn: value.sequence_number, - gid: value.source_gid, + sequence_number: value.sequence_number, + writer_guid: value.source_gid, } } } +pub struct ServiceReply { + request_id: RequestId, + key_expr: KeyExpr<'static>, + query: Query, + clock: crate::time::ZClock, + _phantom_data: PhantomData, +} + +impl ServiceReply { + pub fn id(&self) -> &RequestId { + &self.request_id + } + + pub fn reply_blocking(self, msg: &T::Response) -> Result<()> { + let attachment = Attachment::with_clock( + self.request_id.sequence_number, + self.request_id.writer_guid, + &self.clock, + ); + self.query + .reply(&self.key_expr, msg.serialize()) + .attachment(attachment) + .wait() + } + + pub async fn reply(self, msg: &T::Response) -> Result<()> { + let attachment = Attachment::with_clock( + self.request_id.sequence_number, + self.request_id.writer_guid, + &self.clock, + ); + self.query + .reply(&self.key_expr, msg.serialize()) + .attachment(attachment) + .await + } +} + +pub struct ServiceRequest { + message: T::Request, + reply: ServiceReply, +} + +impl ServiceRequest { + pub fn id(&self) -> &RequestId { + self.reply.id() + } + + pub fn message(&self) -> &T::Request { + &self.message + } + + pub fn into_message(self) -> T::Request { + self.message + } + + pub fn into_parts(self) -> (T::Request, ServiceReply) { + (self.message, self.reply) + } + + pub fn reply_blocking(self, response: &T::Response) -> Result<()> { + self.reply.reply_blocking(response) + } + + pub async fn reply(self, response: &T::Response) -> Result<()> { + self.reply.reply(response).await + } +} + impl ZServer where T: ZService, { - fn new_attachment(&self) -> Attachment { - Attachment::with_clock( - self.sn.fetch_add(1, Ordering::AcqRel) as _, - self.gid, - &self.clock, - ) - } - - /// Retrieve the next query on the service without deserializing the payload. - /// - /// This method is useful when custom deserialization logic is needed. - pub fn take_query(&self) -> Result { + fn take_query(&self) -> Result { let queue = self.queue.as_ref().ok_or_else(|| { zenoh::Error::from("Server was built with callback, no queue available") })?; @@ -565,6 +572,52 @@ where .ok_or_else(|| zenoh::Error::from("No query available")) } + fn decode_request(&self, query: Query) -> Result> + where + T::Request: ZMessage + Send + Sync + 'static, + for<'a> ::Serdes: + ZDeserializer = &'a [u8]>, + { + let attachment_bytes = query + .attachment() + .ok_or_else(|| zenoh::Error::from("Service request missing attachment"))?; + let attachment: Attachment = attachment_bytes.try_into()?; + let request_id: RequestId = attachment.into(); + + let payload_bytes = query + .payload() + .map(|payload| payload.to_bytes()) + .unwrap_or_default(); + let message = ::deserialize(&payload_bytes[..]) + .map_err(|e| zenoh::Error::from(e.to_string()))?; + + Ok(ServiceRequest { + message, + reply: ServiceReply { + request_id, + key_expr: self.key_expr.clone(), + query, + clock: self.clock.clone(), + _phantom_data: PhantomData, + }, + }) + } + + pub fn try_take_request(&mut self) -> Result>> + where + T::Request: ZMessage + Send + Sync + 'static, + for<'a> ::Serdes: + ZDeserializer = &'a [u8]>, + { + let queue = self.queue.as_ref().ok_or_else(|| { + zenoh::Error::from("Server was built with callback, no queue available") + })?; + match queue.try_recv() { + Some(query) => self.decode_request(query).map(Some), + None => Ok(None), + } + } + /// Blocks waiting to receive the next request on the service and then deserializes the payload. /// /// This method may fail if the message does not deserialize as the requested type. @@ -573,7 +626,7 @@ where sn = tracing::field::Empty, payload_len = tracing::field::Empty ))] - pub fn take_request(&mut self) -> Result<(QueryKey, T::Request)> + pub fn take_request(&mut self) -> Result> where T::Request: ZMessage + Send + Sync + 'static, for<'a> ::Serdes: @@ -585,32 +638,13 @@ where zenoh::Error::from("Server was built with callback, no queue available") })?; let query = queue.recv(); - let attachment: Attachment = query.attachment().unwrap().try_into()?; - let key: QueryKey = attachment.into(); - - tracing::Span::current().record("sn", key.sn); - - let payload_bytes = query.payload().unwrap().to_bytes(); - tracing::Span::current().record("payload_len", payload_bytes.len()); - - if self.map.contains_key(&key) { - warn!("[SRV] Duplicate request: sn={}", key.sn); - return Err("Existing query detected".into()); - } - - debug!("[SRV] Processing request"); - - let msg = ::deserialize(&payload_bytes[..]) - .map_err(|e| zenoh::Error::from(e.to_string()))?; - self.map.insert(key.clone(), query); - - Ok((key, msg)) + self.decode_request(query) } /// Awaits the next request on the service and then deserializes the payload. /// /// This method may fail if the message does not deserialize as the requested type. - pub async fn async_take_request(&mut self) -> Result<(QueryKey, T::Request)> + pub async fn async_take_request(&mut self) -> Result> where T::Request: ZMessage + Send + Sync + 'static, for<'a> ::Serdes: @@ -620,70 +654,7 @@ where zenoh::Error::from("Server was built with callback, no queue available") })?; let query = queue.recv_async().await; - let attachment: Attachment = query.attachment().unwrap().try_into()?; - let key: QueryKey = attachment.into(); - if self.map.contains_key(&key) { - return Err("Existing query detected".into()); - } - let payload_bytes = query.payload().unwrap().to_bytes(); - let msg = ::deserialize(&payload_bytes[..]) - .map_err(|e| zenoh::Error::from(e.to_string()))?; - self.map.insert(key.clone(), query); - - Ok((key, msg)) - } - - /// Blocks sending the response to a service request. - /// - /// - `msg` is the response message to send. - /// - `key` is the query key of the request to reply to and is obtained from [take_request](Self::take_request) or [async_take_request](Self::async_take_request) - #[tracing::instrument(name = "send_response", skip(self, msg), fields( - service = %self.key_expr, - sn = %key.sn, - payload_len = tracing::field::Empty - ))] - pub fn send_response(&mut self, msg: &T::Response, key: &QueryKey) -> Result<()> { - debug!( - "[SRV] Looking for query with key sn:{}, gid:{:?}", - key.sn, key.gid - ); - match self.map.remove(key) { - Some(query) => { - let payload = msg.serialize(); - tracing::Span::current().record("payload_len", payload.len()); - - debug!("[SRV] Sending response"); - - // Use the sequence number and GID from the request - let attachment = Attachment::with_clock(key.sn, key.gid, &self.clock); - query - .reply(&self.key_expr, payload) - .attachment(attachment) - .wait() - } - None => { - error!("[SRV] No query found for sn={}", key.sn); - Err("Query map doesn't contain key".into()) - } - } - } - - /// Awaits sending the response to a service request. - /// - /// - `msg` is the response message to send. - /// - `key` is the query key of the request to reply to and is obtained from [take_request](Self::take_request) or [async_take_request](Self::async_take_request) - pub async fn async_send_response(&mut self, msg: &T::Response, key: &QueryKey) -> Result<()> { - match self.map.remove(key) { - Some(query) => { - // Use the sequence number and GID from the request - let attachment = Attachment::with_clock(key.sn, key.gid, &self.clock); - query - .reply(&self.key_expr, msg.serialize()) - .attachment(attachment) - .await - } - None => Err("Quey map doesn't contains {key}".into()), - } + self.decode_request(query) } } @@ -752,14 +723,14 @@ mod tests { } #[test] - fn test_query_key_clone_and_eq() { - let key = crate::service::QueryKey { - gid: [1u8; 16], - sn: 42, + fn test_request_id_clone_and_eq() { + let key = crate::service::RequestId { + writer_guid: [1u8; 16], + sequence_number: 42, }; let key2 = key.clone(); - assert_eq!(key.sn, key2.sn); - assert_eq!(key.gid, key2.gid); + assert_eq!(key.sequence_number, key2.sequence_number); + assert_eq!(key.writer_guid, key2.writer_guid); } #[test] diff --git a/crates/ros-z/src/shm.rs b/crates/ros-z/src/shm.rs index ee3645de..84ab38f9 100644 --- a/crates/ros-z/src/shm.rs +++ b/crates/ros-z/src/shm.rs @@ -49,7 +49,7 @@ //! let provider = Arc::new(ShmProviderBuilder::new(20_000_000).build()?); //! let config = ShmConfig::new(provider).with_threshold(10_000); //! -//! let pub = node.create_pub::("topic") +//! let publisher = node.create_pub::("topic") //! .with_shm_config(config) //! .build()?; //! # Ok(()) @@ -128,6 +128,7 @@ impl ShmConfig { /// /// ```rust,no_run /// use ros_z::shm::{ShmConfig, ShmProviderBuilder}; + /// use ros_z::Builder; /// use std::sync::Arc; /// /// # fn main() -> ros_z::Result<()> { diff --git a/crates/ros-z/tests/extended_message_type_info.rs b/crates/ros-z/tests/extended_message_type_info.rs new file mode 100644 index 00000000..65a5f802 --- /dev/null +++ b/crates/ros-z/tests/extended_message_type_info.rs @@ -0,0 +1,383 @@ +use std::time::Duration; + +use ros_z::{ + Builder, ExtendedMessageTypeInfo, MessageTypeInfo, + context::ZContextBuilder, + dynamic::{DynamicValue, EnumPayloadValue}, +}; +use serde::{Deserialize, Serialize}; +use zenoh::Wait; +use zenoh::config::WhatAmI; + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, ros_z::ExtendedMessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/TelemetryLite")] +struct TelemetryLite { + label: String, + temperatures: Vec, +} + +impl ros_z::msg::ZMessage for TelemetryLite { + type Serdes = ros_z::msg::SerdeCdrSerdes; +} + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, ros_z::ExtendedMessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/RobotState")] +enum RobotState { + Idle, + Error(String), + Charging { minutes_remaining: u32 }, +} + +impl ros_z::msg::ZMessage for RobotState { + type Serdes = ros_z::msg::SerdeCdrSerdes; +} + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, ros_z::ExtendedMessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/RobotEnvelope")] +struct RobotEnvelope { + label: String, + mission_id: Option, + state: RobotState, +} + +impl ros_z::msg::ZMessage for RobotEnvelope { + type Serdes = ros_z::msg::SerdeCdrSerdes; +} + +struct TestRouter { + endpoint: String, + _session: zenoh::Session, +} + +impl TestRouter { + fn new() -> Self { + let port = { + let listener = + std::net::TcpListener::bind("127.0.0.1:0").expect("failed to bind port 0"); + listener.local_addr().unwrap().port() + }; + + let endpoint = format!("tcp/127.0.0.1:{port}"); + let mut config = zenoh::Config::default(); + config.set_mode(Some(WhatAmI::Router)).unwrap(); + config + .insert_json5("listen/endpoints", &format!("[\"{endpoint}\"]")) + .unwrap(); + config + .insert_json5("scouting/multicast/enabled", "false") + .unwrap(); + + let session = zenoh::open(config) + .wait() + .expect("failed to open test router"); + std::thread::sleep(Duration::from_millis(300)); + + Self { + endpoint, + _session: session, + } + } + + fn endpoint(&self) -> &str { + &self.endpoint + } +} + +fn create_context_with_router(router: &TestRouter) -> ros_z::Result { + ZContextBuilder::default() + .disable_multicast_scouting() + .with_connect_endpoints([router.endpoint()]) + .build() +} + +#[test] +fn extended_derive_keeps_standard_schema_for_compatible_structs() { + let schema = TelemetryLite::message_schema().expect("standard-compatible schema"); + assert!(!schema.uses_extended_types()); + assert_eq!(schema.type_name, "custom_msgs/msg/TelemetryLite"); + + let extended = TelemetryLite::extended_message_schema(); + assert_eq!(extended.type_name, schema.type_name); + assert!(!extended.uses_extended_types()); + + assert!( + RobotEnvelope::message_schema().is_none(), + "extended-only structs should not expose a standard schema" + ); + assert!( + RobotState::message_schema().is_none(), + "extended enums should not expose a standard schema" + ); +} + +#[tokio::test(flavor = "multi_thread", worker_threads = 2)] +async fn fallback_discovery_uses_standard_service_for_compatible_extended_types() { + let router = TestRouter::new(); + + let pub_ctx = create_context_with_router(&router).expect("publisher context"); + let pub_node = pub_ctx + .create_node("telemetry_talker") + .with_type_description_service() + .build() + .expect("publisher node"); + + let publisher = pub_node + .create_pub::("/extended_standard_topic") + .build() + .expect("publisher"); + + let registered = pub_node + .type_description_service() + .expect("standard type description service") + .get_schema("custom_msgs/msg/TelemetryLite") + .expect("schema lookup"); + assert!( + registered.is_some(), + "standard-compatible schema should register" + ); + + let sub_ctx = create_context_with_router(&router).expect("subscriber context"); + let sub_node = sub_ctx + .create_node("telemetry_listener") + .build() + .expect("subscriber node"); + + let publish_task = tokio::spawn(async move { + for _ in 0..20 { + let msg = TelemetryLite { + label: "robot-7".to_string(), + temperatures: vec![20.0, 20.5], + }; + publisher.publish(&msg).expect("publish"); + tokio::time::sleep(Duration::from_millis(100)).await; + } + }); + + tokio::time::sleep(Duration::from_millis(400)).await; + + let subscriber = sub_node + .create_dyn_sub_auto("/extended_standard_topic", Duration::from_secs(10)) + .await + .expect("fallback dynamic subscriber") + .build() + .expect("subscriber build"); + let schema = subscriber.schema().expect("discovered schema"); + + assert_eq!(schema.type_name, "custom_msgs/msg/TelemetryLite"); + assert!(!schema.uses_extended_types()); + + let message = subscriber + .recv_timeout(Duration::from_secs(3)) + .expect("dynamic message"); + assert_eq!( + message.get::("label").unwrap(), + "robot-7".to_string() + ); + + publish_task.await.expect("publisher task"); +} + +#[tokio::test(flavor = "multi_thread", worker_threads = 2)] +async fn extended_only_types_require_explicit_extended_service_enablement() { + let router = TestRouter::new(); + + let pub_ctx = create_context_with_router(&router).expect("publisher context"); + let pub_node = pub_ctx + .create_node("extended_talker") + .build() + .expect("publisher node"); + + let publisher = pub_node + .create_pub::("/extended_robot_topic") + .build() + .expect("publisher"); + + let sub_ctx = create_context_with_router(&router).expect("subscriber context"); + let sub_node = sub_ctx + .create_node("extended_listener") + .build() + .expect("subscriber node"); + + let publish_task = tokio::spawn(async move { + for _ in 0..20 { + let msg = RobotEnvelope { + label: "robot-9".to_string(), + mission_id: Some(42), + state: RobotState::Charging { + minutes_remaining: 12, + }, + }; + publisher.publish(&msg).expect("publish"); + tokio::time::sleep(Duration::from_millis(100)).await; + } + }); + + tokio::time::sleep(Duration::from_millis(400)).await; + + let result = sub_node + .create_dyn_sub_auto("/extended_robot_topic", Duration::from_secs(3)) + .await; + assert!( + result.is_err(), + "extended discovery should fail when the publisher did not enable the extended service" + ); + + publish_task.await.expect("publisher task"); +} + +#[tokio::test(flavor = "multi_thread", worker_threads = 2)] +async fn extended_only_types_use_extended_service_when_enabled() { + let router = TestRouter::new(); + + let pub_ctx = create_context_with_router(&router).expect("publisher context"); + let pub_node = pub_ctx + .create_node("extended_talker") + .with_extended_type_description_service() + .build() + .expect("publisher node"); + + let publisher = pub_node + .create_pub::("/extended_robot_topic") + .build() + .expect("publisher"); + + assert!( + pub_node.type_description_service().is_none(), + "extended-only discovery should not require the standard type description service" + ); + let extended_schema = pub_node + .extended_type_description_service() + .expect("extended type description service") + .get_schema("custom_msgs/msg/RobotEnvelope") + .expect("schema lookup"); + assert!(extended_schema.is_some(), "extended schema should register"); + + let sub_ctx = create_context_with_router(&router).expect("subscriber context"); + let sub_node = sub_ctx + .create_node("extended_listener") + .build() + .expect("subscriber node"); + + let publish_task = tokio::spawn(async move { + for _ in 0..20 { + let msg = RobotEnvelope { + label: "robot-9".to_string(), + mission_id: Some(42), + state: RobotState::Charging { + minutes_remaining: 12, + }, + }; + publisher.publish(&msg).expect("publish"); + tokio::time::sleep(Duration::from_millis(100)).await; + } + }); + + tokio::time::sleep(Duration::from_millis(400)).await; + + let subscriber = sub_node + .create_dyn_sub_auto("/extended_robot_topic", Duration::from_secs(10)) + .await + .expect("extended fallback subscriber") + .build() + .expect("subscriber build"); + let schema = subscriber.schema().expect("discovered schema"); + + assert!(schema.uses_extended_types()); + assert_eq!(schema.type_name, "custom_msgs/msg/RobotEnvelope"); + + let message = subscriber + .recv_timeout(Duration::from_secs(3)) + .expect("dynamic message"); + assert_eq!( + message.get::("label").unwrap(), + "robot-9".to_string() + ); + + match message.get_dynamic("mission_id").expect("mission_id") { + DynamicValue::Optional(Some(value)) => { + assert_eq!(value.as_ref().as_u32(), Some(42)); + } + other => panic!("expected Some mission_id, got {other:?}"), + } + + match message.get_dynamic("state").expect("state") { + DynamicValue::Enum(value) => { + assert_eq!(value.variant_name, "Charging"); + match value.payload { + EnumPayloadValue::Struct(fields) => { + assert_eq!(fields.len(), 1); + assert_eq!(fields[0].name, "minutes_remaining"); + assert_eq!(fields[0].value.as_u32(), Some(12)); + } + other => panic!("expected struct payload, got {other:?}"), + } + } + other => panic!("expected enum value, got {other:?}"), + } + + publish_task.await.expect("publisher task"); +} + +#[tokio::test(flavor = "multi_thread", worker_threads = 2)] +async fn top_level_enums_are_discoverable_through_the_extended_service() { + let router = TestRouter::new(); + + let pub_ctx = create_context_with_router(&router).expect("publisher context"); + let pub_node = pub_ctx + .create_node("state_talker") + .with_extended_type_description_service() + .build() + .expect("publisher node"); + + let publisher = pub_node + .create_pub::("/robot_state_topic") + .build() + .expect("publisher"); + + let sub_ctx = create_context_with_router(&router).expect("subscriber context"); + let sub_node = sub_ctx + .create_node("state_listener") + .build() + .expect("subscriber node"); + + let publish_task = tokio::spawn(async move { + for _ in 0..20 { + publisher + .publish(&RobotState::Error("battery low".to_string())) + .expect("publish"); + tokio::time::sleep(Duration::from_millis(100)).await; + } + }); + + tokio::time::sleep(Duration::from_millis(400)).await; + + let subscriber = sub_node + .create_dyn_sub_auto("/robot_state_topic", Duration::from_secs(10)) + .await + .expect("extended enum discovery") + .build() + .expect("subscriber build"); + let schema = subscriber.schema().expect("discovered schema"); + + assert_eq!(schema.field_count(), 1); + assert_eq!(schema.field("value").unwrap().name, "value"); + + let message = subscriber + .recv_timeout(Duration::from_secs(3)) + .expect("dynamic message"); + + match message.get_dynamic("value").expect("enum value") { + DynamicValue::Enum(value) => { + assert_eq!(value.variant_name, "Error"); + match value.payload { + EnumPayloadValue::Newtype(value) => { + assert_eq!(value.as_ref().as_str(), Some("battery low")); + } + other => panic!("expected newtype payload, got {other:?}"), + } + } + other => panic!("expected enum field, got {other:?}"), + } + + publish_task.await.expect("publisher task"); +} diff --git a/crates/ros-z/tests/graph.rs b/crates/ros-z/tests/graph.rs index 49658fe2..7905275b 100644 --- a/crates/ros-z/tests/graph.rs +++ b/crates/ros-z/tests/graph.rs @@ -15,7 +15,6 @@ use ros_z::{ entity::{EntityKind, NodeKey}, }; use ros_z_msgs::{example_interfaces::srv::AddTwoInts, std_msgs::String as RosString}; - /// Helper to create a test context and node async fn setup_test_node( node_name: &str, @@ -74,7 +73,6 @@ async fn wait_for_subscribers( #[cfg(test)] mod tests { use super::*; - /// Tests getting topic names and types from the graph #[tokio::test(flavor = "multi_thread")] async fn test_get_topic_names_and_types() -> Result<()> { diff --git a/crates/ros-z/tests/message_type_info_derive.rs b/crates/ros-z/tests/message_type_info_derive.rs new file mode 100644 index 00000000..160b0177 --- /dev/null +++ b/crates/ros-z/tests/message_type_info_derive.rs @@ -0,0 +1,213 @@ +use std::time::Duration; + +use ros_z::{ + Builder, MessageTypeInfo, TypeHash, + context::ZContextBuilder, + dynamic::{FieldType, MessageSchemaTypeDescription}, +}; +use serde::{Deserialize, Serialize}; +use zenoh::Wait; +use zenoh::config::WhatAmI; + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, ros_z::MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/Position2D")] +struct Position2D { + x: f64, + y: f64, +} + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, ros_z::MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/RobotTelemetry")] +struct RobotTelemetry { + label: String, + pose: Position2D, + temperatures: Vec, + flags: [bool; 2], + payload: Vec, +} + +impl ros_z::msg::ZMessage for RobotTelemetry { + type Serdes = ros_z::msg::SerdeCdrSerdes; +} + +struct TestRouter { + endpoint: String, + _session: zenoh::Session, +} + +impl TestRouter { + fn new() -> Self { + let port = { + let listener = + std::net::TcpListener::bind("127.0.0.1:0").expect("failed to bind port 0"); + listener.local_addr().unwrap().port() + }; + + let endpoint = format!("tcp/127.0.0.1:{port}"); + let mut config = zenoh::Config::default(); + config.set_mode(Some(WhatAmI::Router)).unwrap(); + config + .insert_json5("listen/endpoints", &format!("[\"{endpoint}\"]")) + .unwrap(); + config + .insert_json5("scouting/multicast/enabled", "false") + .unwrap(); + + let session = zenoh::open(config) + .wait() + .expect("failed to open test router"); + std::thread::sleep(Duration::from_millis(300)); + + Self { + endpoint, + _session: session, + } + } + + fn endpoint(&self) -> &str { + &self.endpoint + } +} + +fn create_context_with_router(router: &TestRouter) -> ros_z::Result { + ZContextBuilder::default() + .disable_multicast_scouting() + .with_connect_endpoints([router.endpoint()]) + .build() +} + +#[test] +fn derive_generates_type_info_and_schema() { + let schema = RobotTelemetry::message_schema().expect("schema should be generated"); + + assert_eq!( + RobotTelemetry::type_name(), + "custom_msgs::msg::dds_::RobotTelemetry_" + ); + assert_eq!(schema.type_name, "custom_msgs/msg/RobotTelemetry"); + assert_eq!(schema.field_count(), 5); + + let label = schema.field("label").expect("label field"); + assert!(matches!(label.field_type, FieldType::String)); + + let pose = schema.field("pose").expect("pose field"); + match &pose.field_type { + FieldType::Message(nested) => { + assert_eq!(nested.type_name, "custom_msgs/msg/Position2D"); + assert_eq!(nested.field_count(), 2); + } + other => panic!("expected nested message field, got {:?}", other), + } + + let temperatures = schema.field("temperatures").expect("temperatures field"); + match &temperatures.field_type { + FieldType::Sequence(inner) => { + assert!(matches!(inner.as_ref(), FieldType::Float32)); + } + other => panic!("expected sequence field, got {:?}", other), + } + + let flags = schema.field("flags").expect("flags field"); + match &flags.field_type { + FieldType::Array(inner, len) => { + assert_eq!(*len, 2); + assert!(matches!(inner.as_ref(), FieldType::Bool)); + } + other => panic!("expected fixed array field, got {:?}", other), + } + + let payload = schema.field("payload").expect("payload field"); + match &payload.field_type { + FieldType::Sequence(inner) => { + assert!(matches!(inner.as_ref(), FieldType::Uint8)); + } + other => panic!("expected byte sequence field, got {:?}", other), + } + + let expected_hash = TypeHash::from_rihs_string( + &schema + .compute_type_hash() + .expect("schema hash") + .to_rihs_string(), + ) + .expect("valid entity type hash"); + + let reported_hash = RobotTelemetry::type_hash(); + if TypeHash::zero().to_rihs_string() == "TypeHashNotSupported" { + assert_eq!(reported_hash, TypeHash::zero()); + } else { + assert_eq!(reported_hash, expected_hash); + } +} + +#[tokio::test(flavor = "multi_thread", worker_threads = 2)] +async fn derived_message_schema_is_auto_registered_and_discoverable() { + let router = TestRouter::new(); + + let pub_ctx = create_context_with_router(&router).expect("publisher context"); + let pub_node = pub_ctx + .create_node("derived_talker") + .with_type_description_service() + .build() + .expect("publisher node"); + + let publisher = pub_node + .create_pub::("/derived_topic") + .build() + .expect("publisher"); + + let registered = pub_node + .type_description_service() + .expect("type description service") + .get_schema("custom_msgs/msg/RobotTelemetry") + .expect("query registered schema"); + assert!(registered.is_some(), "schema should be auto-registered"); + + let sub_ctx = create_context_with_router(&router).expect("subscriber context"); + let sub_node = sub_ctx + .create_node("derived_listener") + .build() + .expect("subscriber node"); + + let publish_task = tokio::spawn(async move { + for _ in 0..25 { + let msg = RobotTelemetry { + label: "robot-1".to_string(), + pose: Position2D { x: 1.25, y: -2.5 }, + temperatures: vec![20.5, 21.0, 21.5], + flags: [true, false], + payload: vec![1, 2, 3, 4], + }; + publisher.publish(&msg).expect("publish"); + tokio::time::sleep(Duration::from_millis(100)).await; + } + }); + + tokio::time::sleep(Duration::from_millis(400)).await; + + let subscriber = sub_node + .create_dyn_sub_auto("/derived_topic", Duration::from_secs(10)) + .await + .expect("dynamic subscriber with auto-discovery") + .build() + .expect("subscriber build"); + let discovered_schema = subscriber.schema().expect("discovered schema"); + + assert_eq!( + discovered_schema.type_name, + "custom_msgs/msg/RobotTelemetry" + ); + assert_eq!(discovered_schema.field_count(), 5); + + let msg = subscriber + .recv_timeout(Duration::from_secs(3)) + .expect("received dynamic message"); + assert_eq!( + msg.get::("label").expect("label field"), + "robot-1".to_string() + ); + assert_eq!(msg.get::("pose.x").expect("nested pose.x"), 1.25); + assert_eq!(msg.get::("pose.y").expect("nested pose.y"), -2.5); + + publish_task.await.expect("publisher task"); +} diff --git a/crates/ros-z/tests/message_type_info_derive_ui.rs b/crates/ros-z/tests/message_type_info_derive_ui.rs new file mode 100644 index 00000000..15ade070 --- /dev/null +++ b/crates/ros-z/tests/message_type_info_derive_ui.rs @@ -0,0 +1,5 @@ +#[test] +fn message_type_info_rejects_v2_only_shapes() { + let cases = trybuild::TestCases::new(); + cases.compile_fail("tests/ui/message_type_info/*.rs"); +} diff --git a/crates/ros-z/tests/service.rs b/crates/ros-z/tests/service.rs index a3d177f8..6888de43 100644 --- a/crates/ros-z/tests/service.rs +++ b/crates/ros-z/tests/service.rs @@ -88,15 +88,15 @@ fn test_basic_service_request_response() { .expect("Failed to create server"); // Wait for request - let (key, request) = server.take_request().expect("Failed to take request"); - assert_eq!(request.a, 10); - assert_eq!(request.b, 32); + let request = server.take_request().expect("Failed to take request"); + assert_eq!(request.message().a, 10); + assert_eq!(request.message().b, 32); let response = AddTwoIntsResponse { - sum: request.a + request.b, + sum: request.message().a + request.message().b, }; - server - .send_response(&response, &key) + request + .reply_blocking(&response) .expect("Failed to send response"); } }); @@ -119,13 +119,13 @@ fn test_basic_service_request_response() { let request = AddTwoIntsRequest { a: 10, b: 32 }; - tokio::runtime::Runtime::new() + let response = tokio::runtime::Runtime::new() .unwrap() - .block_on(async { client.send_request(&request).await }) - .expect("Failed to send request"); - - let response = client - .take_response_timeout(Duration::from_secs(2)) + .block_on(async { + client + .call_or_timeout(&request, Duration::from_secs(2)) + .await + }) .expect("Failed to receive response"); assert_eq!(response.sum, 42); @@ -157,17 +157,16 @@ async fn test_async_service_request_response() { .expect("Failed to create server"); // Wait for request asynchronously - let (key, request) = server + let request = server .async_take_request() .await .expect("Failed to take request"); - let response = AddTwoIntsResponse { - sum: request.a + request.b, + sum: request.message().a + request.message().b, }; - server - .async_send_response(&response, &key) + request + .reply(&response) .await .expect("Failed to send response"); }); @@ -188,13 +187,8 @@ async fn test_async_service_request_response() { tokio::time::sleep(Duration::from_millis(100)).await; let request = AddTwoIntsRequest { a: 100, b: 23 }; - client - .send_request(&request) - .await - .expect("Failed to send request"); - let response = client - .async_take_response() + .call(&request) .await .expect("Failed to receive response"); @@ -229,15 +223,15 @@ fn test_multiple_service_requests() { // Handle 3 requests for expected_a in [1, 2, 3] { - let (key, request) = server.take_request().expect("Failed to take request"); - assert_eq!(request.a, expected_a); - assert_eq!(request.b, 10); + let request = server.take_request().expect("Failed to take request"); + assert_eq!(request.message().a, expected_a); + assert_eq!(request.message().b, 10); let response = AddTwoIntsResponse { - sum: request.a + request.b, + sum: request.message().a + request.message().b, }; - server - .send_response(&response, &key) + request + .reply_blocking(&response) .expect("Failed to send response"); } } @@ -265,11 +259,12 @@ fn test_multiple_service_requests() { for a in [1, 2, 3] { let request = AddTwoIntsRequest { a, b: 10 }; - rt.block_on(async { client.send_request(&request).await }) - .expect("Failed to send request"); - - let response = client - .take_response_timeout(Duration::from_secs(2)) + let response = rt + .block_on(async { + client + .call_or_timeout(&request, Duration::from_secs(2)) + .await + }) .expect("Failed to receive response"); assert_eq!(response.sum, a + 10); diff --git a/crates/ros-z/tests/ui/message_type_info/enum.rs b/crates/ros-z/tests/ui/message_type_info/enum.rs new file mode 100644 index 00000000..ab597f7b --- /dev/null +++ b/crates/ros-z/tests/ui/message_type_info/enum.rs @@ -0,0 +1,11 @@ +use ros_z::MessageTypeInfo; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Clone, Serialize, Deserialize, MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/RobotState")] +enum RobotState { + Idle, + Error(String), +} + +fn main() {} diff --git a/crates/ros-z/tests/ui/message_type_info/enum.stderr b/crates/ros-z/tests/ui/message_type_info/enum.stderr new file mode 100644 index 00000000..ddd5e768 --- /dev/null +++ b/crates/ros-z/tests/ui/message_type_info/enum.stderr @@ -0,0 +1,9 @@ +error: MessageTypeInfo derive only supports named structs in v1 + --> tests/ui/message_type_info/enum.rs:5:1 + | +5 | / #[ros_msg(type_name = "custom_msgs/msg/RobotState")] +6 | | enum RobotState { +7 | | Idle, +8 | | Error(String), +9 | | } + | |_^ diff --git a/crates/ros-z/tests/ui/message_type_info/option_field.rs b/crates/ros-z/tests/ui/message_type_info/option_field.rs new file mode 100644 index 00000000..ecf6f2d3 --- /dev/null +++ b/crates/ros-z/tests/ui/message_type_info/option_field.rs @@ -0,0 +1,10 @@ +use ros_z::MessageTypeInfo; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Clone, Serialize, Deserialize, MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/OptionField")] +struct OptionField { + value: Option, +} + +fn main() {} diff --git a/crates/ros-z/tests/ui/message_type_info/option_field.stderr b/crates/ros-z/tests/ui/message_type_info/option_field.stderr new file mode 100644 index 00000000..7da97f16 --- /dev/null +++ b/crates/ros-z/tests/ui/message_type_info/option_field.stderr @@ -0,0 +1,5 @@ +error: Option fields are not supported by MessageTypeInfo derive in v1 + --> tests/ui/message_type_info/option_field.rs:7:12 + | +7 | value: Option, + | ^^^^^^^^^^^ diff --git a/crates/ros-z/tests/ui/message_type_info/tuple_struct.rs b/crates/ros-z/tests/ui/message_type_info/tuple_struct.rs new file mode 100644 index 00000000..91346430 --- /dev/null +++ b/crates/ros-z/tests/ui/message_type_info/tuple_struct.rs @@ -0,0 +1,8 @@ +use ros_z::MessageTypeInfo; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Clone, Serialize, Deserialize, MessageTypeInfo)] +#[ros_msg(type_name = "custom_msgs/msg/TupleStatus")] +struct TupleStatus(f32, f32); + +fn main() {} diff --git a/crates/ros-z/tests/ui/message_type_info/tuple_struct.stderr b/crates/ros-z/tests/ui/message_type_info/tuple_struct.stderr new file mode 100644 index 00000000..791e13ad --- /dev/null +++ b/crates/ros-z/tests/ui/message_type_info/tuple_struct.stderr @@ -0,0 +1,6 @@ +error: MessageTypeInfo derive does not support tuple structs in v1 + --> tests/ui/message_type_info/tuple_struct.rs:5:1 + | +5 | / #[ros_msg(type_name = "custom_msgs/msg/TupleStatus")] +6 | | struct TupleStatus(f32, f32); + | |_____________________________^