diff --git a/examples/dynamic_pub_sub/Cargo.toml b/examples/dynamic_pub_sub/Cargo.toml new file mode 100644 index 000000000..d47f25426 --- /dev/null +++ b/examples/dynamic_pub_sub/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "dynamic_pub_sub" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +# TODO(luca) change this to a version once dynamic message support is released on crates.io +rclrs = { path = "../../rclrs", features = ["dyn_msg"] } +anyhow = {version = "1", features = ["backtrace"]} diff --git a/examples/dynamic_pub_sub/src/main.rs b/examples/dynamic_pub_sub/src/main.rs new file mode 100644 index 000000000..939e12a96 --- /dev/null +++ b/examples/dynamic_pub_sub/src/main.rs @@ -0,0 +1,23 @@ +use anyhow::{Error, Result}; +use rclrs::*; + +fn main() -> Result<(), Error> { + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + + let node = executor.create_node("dynamic_subscriber")?; + + let worker = node.create_worker::(0); + let _subscription = worker.create_dynamic_subscription( + "rclrs_example_msgs/msg/VariousTypes".try_into()?, + "topic", + move |num_messages: &mut usize, msg, _msg_info| { + *num_messages += 1; + println!("#{} | I heard: '{:#?}'", *num_messages, msg.structure()); + }, + )?; + + println!("Waiting for messages..."); + executor.spin(SpinOptions::default()).first_error()?; + Ok(()) +} diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 0663a87f1..1648fecd2 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -7,16 +7,27 @@ use std::{ fmt::{self, Display}, + ops::Deref, path::PathBuf, sync::Arc, }; +use rosidl_runtime_rs::RmwMessage; + use crate::rcl_bindings::{ rosidl_typesupport_introspection_c__MessageMembers_s as rosidl_message_members_t, *, }; +mod dynamic_publisher; +mod dynamic_subscription; mod error; +mod field_access; +mod message_structure; +pub use dynamic_publisher::*; +pub use dynamic_subscription::*; pub use error::*; +pub use field_access::*; +pub use message_structure::*; /// Factory for constructing messages in a certain package dynamically. /// @@ -39,7 +50,7 @@ struct DynamicMessagePackage { /// A parsed/validated message type name of the form `/msg/`. #[derive(Clone, Debug, PartialEq, Eq)] -struct MessageTypeName { +pub struct MessageTypeName { /// The package name, which acts as a namespace. pub package_name: String, /// The name of the message type in the package. @@ -52,17 +63,33 @@ struct MessageTypeName { /// can be used as a factory to create message instances. #[derive(Clone)] pub struct DynamicMessageMetadata { - #[allow(dead_code)] message_type: MessageTypeName, // The library needs to be kept loaded in order to keep the type_support_ptr valid. + // This is the introspection type support library, not the regular one. #[allow(dead_code)] introspection_type_support_library: Arc, - #[allow(dead_code)] type_support_ptr: *const rosidl_message_type_support_t, - #[allow(dead_code)] + structure: MessageStructure, fini_function: unsafe extern "C" fn(*mut std::os::raw::c_void), } +/// A message whose type is not known at compile-time. +/// +/// This type allows inspecting the structure of the message as well as the +/// values contained in it. +/// It also allows _modifying_ the values, but not the structure, because +/// even a dynamic message must always correspond to a specific message type. +// There is no clone function yet, we need to add that in rosidl. +pub struct DynamicMessage { + metadata: DynamicMessageMetadata, + // This is aligned to the maximum possible alignment of a message (8) + // by the use of a special allocation function. + storage: Box<[u8]>, + // This type allows moving the message contents out into another message, + // in which case the drop impl is not responsible for calling fini anymore + needs_fini: bool, +} + // ========================= impl for DynamicMessagePackage ========================= /// This is an analogue of rclcpp::get_typesupport_library. @@ -172,6 +199,8 @@ impl DynamicMessagePackage { let message_members: &rosidl_message_members_t = // SAFETY: The data pointer is supposed to be always valid. unsafe { &*(type_support.data as *const rosidl_message_members_t) }; + // SAFETY: The message members coming from a type support library will always be valid. + let structure = unsafe { MessageStructure::from_rosidl_message_members(message_members) }; // The fini function will always exist. let fini_function = message_members.fini_function.unwrap(); let metadata = DynamicMessageMetadata { @@ -180,6 +209,7 @@ impl DynamicMessagePackage { &self.introspection_type_support_library, ), type_support_ptr, + structure, fini_function, }; Ok(metadata) @@ -230,6 +260,13 @@ impl Display for MessageTypeName { // ========================= impl for DynamicMessageMetadata ========================= +impl Deref for DynamicMessageMetadata { + type Target = MessageStructure; + fn deref(&self) -> &Self::Target { + &self.structure + } +} + // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. unsafe impl Send for DynamicMessageMetadata {} @@ -242,13 +279,211 @@ impl DynamicMessageMetadata { /// Loads the metadata for the given message type. /// /// See [`DynamicMessage::new()`] for the expected format of the `full_message_type`. - pub fn new(full_message_type: &str) -> Result { - let MessageTypeName { - package_name, - type_name, - } = full_message_type.try_into()?; - let pkg = DynamicMessagePackage::new(package_name)?; - pkg.message_metadata(type_name) + pub fn new(message_type: MessageTypeName) -> Result { + let pkg = DynamicMessagePackage::new(&message_type.package_name)?; + pkg.message_metadata(&message_type.type_name) + } + + /// Instantiates a new message. + pub fn create(&self) -> Result { + // Get an aligned boxed slice. This is inspired by the maligned crate. + use std::alloc::Layout; + // As mentioned in the struct definition, the maximum alignment required is 8. + let layout = Layout::from_size_align(self.structure.size, 8).unwrap(); + let mut storage = unsafe { + assert_ne!(self.structure.size, 0); + // SAFETY: The layout has non-zero size. + let ptr = std::alloc::alloc_zeroed(layout); + // SAFETY: This is valid, memory in ptr has appropriate size and is initialized + let slice = std::slice::from_raw_parts_mut(ptr, self.structure.size); + // The mutable reference decays into a (fat) *mut [u8] + Box::from_raw(slice) + }; + // SAFETY: The pointer returned by get_type_support_handle() is always valid. + let type_support = unsafe { &*self.type_support_ptr }; + let message_members: &rosidl_message_members_t = + // SAFETY: The data pointer is supposed to be always valid. + unsafe { &*(type_support.data as *const rosidl_message_members_t) }; + // SAFETY: The init function is passed zeroed memory of the correct alignment. + unsafe { + (message_members.init_function.unwrap())( + storage.as_mut_ptr() as _, + rosidl_runtime_c__message_initialization::ROSIDL_RUNTIME_C_MSG_INIT_ALL, + ); + }; + let dyn_msg = DynamicMessage { + metadata: self.clone(), + storage, + needs_fini: true, + }; + Ok(dyn_msg) + } + + /// Returns a description of the message structure. + pub fn structure(&self) -> &MessageStructure { + &self.structure + } +} + +// ========================= impl for DynamicMessage ========================= + +impl Deref for DynamicMessage { + type Target = MessageStructure; + fn deref(&self) -> &Self::Target { + &self.metadata.structure + } +} + +impl Drop for DynamicMessage { + fn drop(&mut self) { + if self.needs_fini { + // SAFETY: The fini_function expects to be passed a pointer to the message + unsafe { (self.metadata.fini_function)(self.storage.as_mut_ptr() as _) } + } + } +} + +impl PartialEq for DynamicMessage { + fn eq(&self, other: &Self) -> bool { + self.metadata.type_support_ptr == other.metadata.type_support_ptr + && self.storage == other.storage + } +} + +impl Eq for DynamicMessage {} + +impl DynamicMessage { + /// Dynamically loads a type support library for the specified type and creates a message instance. + /// + /// The full message type is of the form `/msg/`, e.g. + /// `std_msgs/msg/String`. + /// + /// The message instance will contain the default values of the message type. + /// + /// This method might not be the most efficient because of the library loading. If you need to + /// instantiate multiple messages of the same type consider using + /// [`crate::dynamic_message::DynamicMessageMetadata::create`] + pub fn new(message_type: MessageTypeName) -> Result { + DynamicMessageMetadata::new(message_type)?.create() + } + + /// See [`DynamicMessageView::get()`][1]. + /// + /// [1]: crate::dynamic_message::DynamicMessageView::get + pub fn get(&self, field_name: &str) -> Option> { + let field_info = self.metadata.structure.get_field_info(field_name)?; + // For the unwrap_or, see DynamicMessageViewMut::get_mut + let size = field_info.size().unwrap_or(1); + let bytes = &self.storage[field_info.offset..field_info.offset + size]; + // SAFETY: The bytes contain a valid field of the type recorded in field_info. + unsafe { Value::new(bytes, field_info) } + } + + /// See [`DynamicMessageViewMut::get_mut()`][1]. + /// + /// [1]: crate::dynamic_message::DynamicMessageViewMut::get_mut + pub fn get_mut(&mut self, field_name: &str) -> Option> { + let field_info = self.metadata.structure.get_field_info(field_name)?; + // For the unwrap_or, see DynamicMessageViewMut::get_mut + let size = field_info.size().unwrap_or(1); + let bytes = &mut self.storage[field_info.offset..field_info.offset + size]; + // SAFETY: The bytes contain a valid field of the type recorded in field_info. + Some(unsafe { ValueMut::new(bytes, field_info) }) + } + + /// Returns a description of the message structure. + pub fn structure(&self) -> &MessageStructure { + &self.metadata.structure + } + + /// Iterate over all fields in declaration order. + pub fn iter(&self) -> impl Iterator)> + '_ { + self.metadata.structure.fields.iter().map(|field_info| { + let value = self.get(&field_info.name).unwrap(); + (field_info.name.as_str(), value) + }) + } + + /// Iterate over all fields in declaration order (mutable version). + pub fn iter_mut(&mut self) -> impl Iterator)> + '_ { + self.view_mut().iter_mut() + } + + /// Returns a view object of this message. + /// + /// The purpose for this conversion is to allow uniform handling of this top-level message + /// and nested messages contained in it through a [`DynamicMessageView`]. + pub fn view(&self) -> DynamicMessageView<'_> { + DynamicMessageView { + structure: &self.metadata.structure, + storage: &self.storage, + } + } + + /// Returns a mutable view object of this message. + /// + /// The purpose for this conversion is to allow uniform handling of this top-level message + /// and nested messages contained in it through a [`DynamicMessageViewMut`]. + pub fn view_mut(&mut self) -> DynamicMessageViewMut<'_> { + DynamicMessageViewMut { + structure: &self.metadata.structure, + storage: &mut self.storage, + } + } + + /// Converts a statically typed RMW-native message into a `DynamicMessage`. + pub fn convert_from_rmw_message(mut msg: T) -> Result + where + T: RmwMessage, + { + let message_type = MessageTypeName::try_from(::TYPE_NAME)?; + let mut dyn_msg = Self::new(message_type)?; + let align = std::mem::align_of::(); + assert_eq!(dyn_msg.storage.as_ptr().align_offset(align), 0); + { + // SAFETY: This transmutes the slice of bytes into a &mut T. This is fine, since + // under the hood it *is* a T. + // However, the resulting value is not seen as borrowing from dyn_msg by the borrow checker, + // so we are careful to not create a second mutable reference before dropping this one, + // since that would be UB. + let dyn_msg_transmuted = unsafe { &mut *(dyn_msg.storage.as_mut_ptr() as *mut T) }; + // We cannot simply overwrite one message with the other, or we will get a memory leak/double-free. + // Swapping is the solution. + std::mem::swap(&mut msg, dyn_msg_transmuted); + } + Ok(dyn_msg) + } + + /// Converts a `DynamicMessage` into a statically typed RMW-native message. + /// + /// If the RMW-native message type does not match the underlying message type of this `DynamicMessage`, + /// it is not converted but instead returned unchanged. + pub fn convert_into_rmw_message(mut self) -> Result + where + T: RmwMessage, + { + if ::TYPE_NAME == self.metadata.message_type.to_string() { + // SAFETY: Even though a zero-initialized message might not match RMW expectations for + // what a message should look like, it is safe to temporarily have a zero-initialized + // value, i.e. it is not undefined behavior to do this since it's a C struct, and an + // all-zeroes bit pattern is always a valid instance of any C struct. + let mut dest = unsafe { std::mem::zeroed::() }; + let dest_ptr = &mut dest as *mut T as *mut u8; + // This reinterprets the struct as a slice of bytes. + // The bytes copied into the dest slice are a valid value of T, as ensured by comparison + // of the type support pointers. + let dest_slice = + unsafe { std::slice::from_raw_parts_mut(dest_ptr, std::mem::size_of::()) }; + // This creates a shallow copy, with ownership of the "deep" (or inner) parts moving + // into the destination. + dest_slice.copy_from_slice(&self.storage); + // Don't run the fini function on the src data anymore, because the inner parts would be + // double-freed by dst and src. + self.needs_fini = false; + Ok(dest) + } else { + Err(self) + } } } @@ -262,29 +497,72 @@ mod tests { assert_send::(); assert_sync::(); + assert_send::(); + assert_sync::(); } #[test] fn invalid_message_type_name() { assert!(matches!( - DynamicMessageMetadata::new("x"), + MessageTypeName::try_from("x"), Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); assert!(matches!( - DynamicMessageMetadata::new("x/y"), + MessageTypeName::try_from("x/y"), Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); assert!(matches!( - DynamicMessageMetadata::new("x//y"), + MessageTypeName::try_from("x//y"), Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); assert!(matches!( - DynamicMessageMetadata::new("x/msg/y"), - Err(DynamicMessageError::RequiredPrefixNotSourced { .. }) + MessageTypeName::try_from("x/msg/y/z"), + Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); + // This is valid, but not found in the prefix + let message_type = MessageTypeName::try_from("x/msg/y").unwrap(); assert!(matches!( - DynamicMessageMetadata::new("x/msg/y/z"), - Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) + DynamicMessage::new(message_type), + Err(DynamicMessageError::RequiredPrefixNotSourced { .. }) )); } + + #[test] + fn message_getters_setters() { + let message_type = MessageTypeName { + package_name: "test_msgs".to_owned(), + type_name: "BasicTypes".to_owned(), + }; + let mut message = DynamicMessage::new(message_type).unwrap(); + + { + // Access non existing values + assert!(message.get("invalid_value").is_none()); + } + + { + // Get then set a sample value + let value = message.get_mut("int32_value").unwrap(); + let ValueMut::Simple(value) = value else { + panic!("Unexpected value type, expected Simple value"); + }; + let SimpleValueMut::Int32(value) = value else { + panic!("Unexpected value type, expected Int32"); + }; + assert_eq!(*value, 0); + *value = 42; + } + + { + // Read previously set value + let value = message.get("int32_value").unwrap(); + let Value::Simple(value) = value else { + panic!("Unexpected value type, expected Simple value"); + }; + let SimpleValue::Int32(value) = value else { + panic!("Unexpected value type, expected Int32"); + }; + assert_eq!(*value, 42); + } + } } diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs new file mode 100644 index 000000000..657b26d4b --- /dev/null +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -0,0 +1,253 @@ +use std::ffi::CString; +use std::sync::{Arc, Mutex}; + +use super::{ + get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageError, + DynamicMessageMetadata, MessageTypeName, +}; +use crate::error::{RclrsError, ToResult}; +use crate::rcl_bindings::*; +use crate::{NodeHandle, PublisherHandle, PublisherOptions, ENTITY_LIFECYCLE_MUTEX}; + +/// Struct for sending dynamic messages. +/// +/// Create a dynamic publisher using [`Node::create_dynamic_publisher`][1]. +/// Refer to [`crate::Publisher`] for details of the behavior. +/// +/// [1]: crate::NodeState::create_dynamic_publisher +pub type DynamicPublisher = Arc; + +/// The inner state of a dynamic publisher. +/// +/// Refer to [`crate::PublisherState`] for details of the behavior. +pub struct DynamicPublisherState { + handle: PublisherHandle, + metadata: DynamicMessageMetadata, + // This is the regular type support library, not the introspection one. + #[allow(dead_code)] + type_support_library: Arc, +} + +impl DynamicPublisherState { + /// Creates a new `DynamicPublisherState`. + /// + /// Node and namespace changes are always applied _before_ topic remapping. + pub(crate) fn create<'a>( + topic_type: MessageTypeName, + options: impl Into>, + node_handle: Arc, + ) -> Result, RclrsError> { + // This loads the introspection type support library. + let metadata = DynamicMessageMetadata::new(topic_type)?; + let PublisherOptions { topic, qos } = options.into(); + // However, we also need the regular type support library – + // the rosidl_typesupport_c one. + let message_type = &metadata.message_type; + let type_support_library = + get_type_support_library(&message_type.package_name, "rosidl_typesupport_c")?; + // SAFETY: The symbol type of the type support getter function can be trusted + // assuming the install dir hasn't been tampered with. + // The pointer returned by this function is kept valid by keeping the library loaded. + let type_support_ptr = unsafe { + get_type_support_handle( + type_support_library.as_ref(), + "rosidl_typesupport_c", + message_type, + )? + }; + + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_publisher = unsafe { rcl_get_zero_initialized_publisher() }; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.into(), + })?; + + // SAFETY: No preconditions for this function. + let mut publisher_options = unsafe { rcl_publisher_get_default_options() }; + publisher_options.qos = qos.into(); + + { + let rcl_node = node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: The rcl_publisher is zero-initialized as expected by this function. + // The rcl_node is kept alive because it is co-owned by the subscription. + // The topic name and the options are copied by this function, so they can be dropped + // afterwards. + // TODO: type support? + rcl_publisher_init( + &mut rcl_publisher, + &*rcl_node, + type_support_ptr, + topic_c_string.as_ptr(), + &publisher_options, + ) + .ok()?; + } + } + + Ok(Arc::new(Self { + handle: PublisherHandle { + rcl_publisher: Mutex::new(rcl_publisher), + node_handle, + }, + metadata, + type_support_library, + })) + } + + /// Returns the topic name of the publisher. + /// + /// This returns the topic name after remapping, so it is not necessarily the + /// topic name which was used when creating the publisher. + pub fn topic_name(&self) -> String { + self.handle.topic_name() + } + + /// Returns the number of subscriptions of the publisher. + pub fn get_subscription_count(&self) -> Result { + self.handle.get_subscription_count() + } + + /// Publishes a message. + /// + /// Calling `publish()` is a potentially blocking call, see [this issue][1] for details. + /// + /// [1]: https://github.com/ros2/ros2/issues/255 + pub fn publish(&self, mut message: DynamicMessage) -> Result<(), RclrsError> { + if message.metadata.message_type != self.metadata.message_type { + return Err(DynamicMessageError::MessageTypeMismatch.into()); + } + let rcl_publisher = &mut *self.handle.rcl_publisher.lock().unwrap(); + unsafe { + // SAFETY: The message type is guaranteed to match the publisher type by the type system. + // The message does not need to be valid beyond the duration of this function call. + // The third argument is explictly allowed to be NULL. + rcl_publish( + rcl_publisher, + message.storage.as_mut_ptr() as *mut _, + std::ptr::null_mut(), + ) + .ok() + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::test_helpers::*; + + fn assert_send() {} + fn assert_sync() {} + + #[test] + fn dynamic_publisher_is_sync_and_send() { + assert_send::(); + assert_sync::(); + } + + #[test] + fn test_dynamic_publishers() -> Result<(), RclrsError> { + use crate::TopicEndpointInfo; + use test_msgs::msg; + + let namespace = "/test_dynamic_publishers_graph"; + let graph = construct_test_graph(namespace)?; + + let node_1_empty_publisher = graph + .node1 + .create_dynamic_publisher("test_msgs/msg/Empty".try_into()?, "graph_test_topic_1")?; + let topic1 = node_1_empty_publisher.topic_name(); + let node_1_basic_types_publisher = graph.node1.create_dynamic_publisher( + "test_msgs/msg/BasicTypes".try_into()?, + "graph_test_topic_2", + )?; + let topic2 = node_1_basic_types_publisher.topic_name(); + let node_2_default_publisher = graph + .node2 + .create_dynamic_publisher("test_msgs/msg/Defaults".try_into()?, "graph_test_topic_3")?; + let topic3 = node_2_default_publisher.topic_name(); + + std::thread::sleep(std::time::Duration::from_millis(100)); + + // Test count_publishers() + assert_eq!(graph.node1.count_publishers(&topic1)?, 1); + assert_eq!(graph.node1.count_publishers(&topic2)?, 1); + assert_eq!(graph.node1.count_publishers(&topic3)?, 1); + + // Test get_publisher_names_and_types_by_node() + let node_1_publisher_names_and_types = graph + .node1 + .get_publisher_names_and_types_by_node(&graph.node1.name(), namespace)?; + + let types = node_1_publisher_names_and_types.get(&topic1).unwrap(); + assert!(types.contains(&"test_msgs/msg/Empty".to_string())); + + let types = node_1_publisher_names_and_types.get(&topic2).unwrap(); + assert!(types.contains(&"test_msgs/msg/BasicTypes".to_string())); + + let node_2_publisher_names_and_types = graph + .node1 + .get_publisher_names_and_types_by_node(&graph.node2.name(), namespace)?; + + let types = node_2_publisher_names_and_types.get(&topic3).unwrap(); + assert!(types.contains(&"test_msgs/msg/Defaults".to_string())); + + // Test get_publishers_info_by_topic() + let expected_publishers_info = vec![TopicEndpointInfo { + node_name: String::from("graph_test_node_1"), + node_namespace: String::from(namespace), + topic_type: String::from("test_msgs/msg/Empty"), + }]; + assert_eq!( + graph.node1.get_publishers_info_by_topic(&topic1)?, + expected_publishers_info + ); + assert_eq!( + graph.node2.get_publishers_info_by_topic(&topic1)?, + expected_publishers_info + ); + + // Test get_subscription_count() + assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(0)); + assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(0)); + assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(0)); + // Test subscription with static types + let _node_1_empty_subscriber = graph + .node1 + .create_subscription("graph_test_topic_1", |_msg: msg::Empty| {}); + let _node_1_basic_types_subscriber = graph + .node1 + .create_subscription("graph_test_topic_2", |_msg: msg::BasicTypes| {}); + let _node_2_default_subscriber = graph + .node2 + .create_subscription("graph_test_topic_3", |_msg: msg::Defaults| {}); + assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(1)); + assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(1)); + assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(1)); + + // Test subscription with dynamic types + let _node_1_empty_subscriber = graph.node1.create_dynamic_subscription( + "test_msgs/msg/Empty".try_into().unwrap(), + "graph_test_topic_1", + |_, _| {}, + ); + let _node_1_basic_types_subscriber = graph.node1.create_dynamic_subscription( + "test_msgs/msg/BasicTypes".try_into().unwrap(), + "graph_test_topic_2", + |_, _| {}, + ); + let _node_2_default_subscriber = graph.node2.create_dynamic_subscription( + "test_msgs/msg/Defaults".try_into().unwrap(), + "graph_test_topic_3", + |_, _| {}, + ); + assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(2)); + assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(2)); + assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(2)); + + Ok(()) + } +} diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs new file mode 100644 index 000000000..be3ec3c1d --- /dev/null +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -0,0 +1,445 @@ +use std::any::Any; +use std::boxed::Box; +use std::ffi::CString; +use std::ops::{Deref, DerefMut}; +use std::sync::{Arc, Mutex}; + +use futures::future::BoxFuture; + +use super::{ + get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageMetadata, + MessageStructure, MessageTypeName, +}; +use crate::rcl_bindings::*; +use crate::{ + MessageInfo, Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, + RclrsErrorFilter, SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, + WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, +}; + +/// Struct for receiving messages whose type is not know at compile time. +/// +/// Create a dynamic subscription using [`NodeState::create_dynamic_subscription()`][1] +/// or [`NodeState::create_async_dynamic_subscription`][2]. +/// +/// There can be multiple subscriptions for the same topic, in different nodes or the same node. +/// A clone of a `Subscription` will refer to the same subscription instance as the original. +/// The underlying instance is tied to [`DynamicSubscriptionState`] which implements the [`DynamicSubscription`] API. +/// +/// Receiving messages requires the node's executor to [spin][3]. +/// +/// When a subscription is created, it may take some time to get "matched" with a corresponding +/// publisher. +/// +/// [1]: crate::NodeState::create_subscription +/// [2]: crate::NodeState::create_async_subscription +/// [3]: crate::Executor::spin +pub type DynamicSubscription = Arc>; + +/// A [`DynamicSubscription`] that runs on a [`Worker`]. +/// +/// Create a worker dynamic subscription using [`WorkerState::create_dynamic_subscription`][1]. +/// +/// [1]: crate::WorkerState::create_dynamic_subscription +pub type WorkerDynamicSubscription = Arc>>; + +struct DynamicSubscriptionExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, + metadata: Arc, +} + +pub(crate) struct NodeDynamicSubscriptionCallback( + Box, +); + +impl NodeDynamicSubscriptionCallback { + pub(crate) fn new(f: impl Fn(DynamicMessage, MessageInfo) + Send + Sync + 'static) -> Self { + NodeDynamicSubscriptionCallback(Box::new(f)) + } +} + +pub(crate) struct NodeAsyncDynamicSubscriptionCallback( + Box BoxFuture<'static, ()> + Send + Sync>, +); + +impl NodeAsyncDynamicSubscriptionCallback { + pub(crate) fn new( + f: impl FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + Sync + 'static, + ) -> Self { + NodeAsyncDynamicSubscriptionCallback(Box::new(f)) + } +} + +pub(crate) struct WorkerDynamicSubscriptionCallback( + Box, +); + +impl WorkerDynamicSubscriptionCallback { + pub(crate) fn new( + f: impl FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send + Sync + 'static, + ) -> Self { + WorkerDynamicSubscriptionCallback(Box::new(f)) + } +} + +impl Deref for NodeDynamicSubscriptionCallback { + type Target = Box; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl Deref for NodeAsyncDynamicSubscriptionCallback { + type Target = + Box BoxFuture<'static, ()> + Send + Sync>; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl DerefMut for NodeDynamicSubscriptionCallback { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +impl DerefMut for NodeAsyncDynamicSubscriptionCallback { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +impl Deref for WorkerDynamicSubscriptionCallback { + type Target = Box; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl DerefMut for WorkerDynamicSubscriptionCallback { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +pub(crate) enum DynamicSubscriptionCallback { + /// A callback with the message and the message info as arguments. + Node(NodeAsyncDynamicSubscriptionCallback), + /// A callback with the payload, message, and the message info as arguments. + Worker(WorkerDynamicSubscriptionCallback), +} + +impl From for DynamicSubscriptionCallback<()> { + fn from(value: NodeDynamicSubscriptionCallback) -> Self { + let func = Arc::new(value); + DynamicSubscriptionCallback::Node(NodeAsyncDynamicSubscriptionCallback(Box::new( + move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + }, + ))) + } +} + +impl From for DynamicSubscriptionCallback<()> { + fn from(value: NodeAsyncDynamicSubscriptionCallback) -> Self { + DynamicSubscriptionCallback::Node(value) + } +} + +impl From> + for DynamicSubscriptionCallback +{ + fn from(value: WorkerDynamicSubscriptionCallback) -> Self { + DynamicSubscriptionCallback::Worker(value) + } +} + +impl DynamicSubscriptionCallback { + fn execute( + &mut self, + executable: &DynamicSubscriptionExecutable, + any_payload: &mut dyn Any, + commands: &WorkerCommands, + ) -> Result<(), RclrsError> { + let Some(payload) = any_payload.downcast_mut::() else { + return Err(RclrsError::InvalidPayload { + expected: std::any::TypeId::of::(), + received: (*any_payload).type_id(), + }); + }; + let mut evaluate = || { + match self { + Self::Node(cb) => { + let (msg, msg_info) = executable.take()?; + commands.run_async(cb(msg, msg_info)); + } + Self::Worker(cb) => { + let (msg, msg_info) = executable.take()?; + cb(payload, msg, msg_info); + } + } + Ok(()) + }; + evaluate().take_failed_ok() + } +} + +impl DynamicSubscriptionExecutable { + fn take(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> { + let mut dynamic_message = self.metadata.create()?; + let rmw_message = dynamic_message.storage.as_mut_ptr(); + let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; + let rcl_subscription = &mut *self.handle.lock(); + unsafe { + // SAFETY: The first two pointers are valid/initialized, and do not need to be valid + // beyond the function call. + // The latter two pointers are explicitly allowed to be NULL. + rcl_take( + rcl_subscription, + rmw_message as *mut _, + &mut message_info, + std::ptr::null_mut(), + ) + .ok()? + }; + Ok(( + dynamic_message, + MessageInfo::from_rmw_message_info(&message_info), + )) + } +} + +impl RclPrimitive for DynamicSubscriptionExecutable { + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + self.callback + .lock() + .unwrap() + .execute(&self, payload, &self.commands) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Subscription + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Subscription(self.handle.lock()) + } +} + +/// Struct for receiving messages whose type is only known at runtime. +pub struct DynamicSubscriptionState +where + Scope: WorkScope, +{ + /// This handle is used to access the data that rcl holds for this subscription. + handle: Arc, + /// This allows us to replace the callback in the subscription task. + /// + /// Holding onto this sender will keep the subscription task alive. Once + /// this sender is dropped, the subscription task will end itself. + // TODO(luca) Expose an API to update the callback at runtime + #[allow(unused)] + callback: Arc>>, + /// Holding onto this keeps the waiter for this subscription alive in the + /// wait set of the executor. + #[allow(unused)] + lifecycle: WaitableLifecycle, + metadata: Arc, + // This is the regular type support library, not the introspection one. + #[allow(dead_code)] + type_support_library: Arc, +} + +impl DynamicSubscriptionState +where + Scope: WorkScope, +{ + /// Creates a new dynamic subscription. + /// + /// This is not a public function, by the same rationale as `Subscription::new()`. + pub(crate) fn create<'a>( + topic_type: MessageTypeName, + options: impl Into>, + callback: impl Into>, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + // This loads the introspection type support library. + let metadata = DynamicMessageMetadata::new(topic_type)?; + let SubscriptionOptions { topic, qos } = options.into(); + // However, we also need the regular type support library – + // the rosidl_typesupport_c one. + let message_type = &metadata.message_type; + let type_support_library = + get_type_support_library(&message_type.package_name, "rosidl_typesupport_c")?; + // SAFETY: The symbol type of the type support getter function can be trusted + // assuming the install dir hasn't been tampered with. + // The pointer returned by this function is kept valid by keeping the library loaded. + let type_support_ptr = unsafe { + get_type_support_handle( + type_support_library.as_ref(), + "rosidl_typesupport_c", + message_type, + )? + }; + + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.into(), + })?; + + // SAFETY: No preconditions for this function. + let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() }; + rcl_subscription_options.qos = qos.into(); + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; + { + let rcl_node = node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: + // * The rcl_subscription is zero-initialized as mandated by this function. + // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the subscription. + // * The topic name and the options are copied by this function, so they can be dropped afterwards. + // * The entity lifecycle mutex is locked to protect against the risk of global + // variables in the rmw implementation being unsafely modified during cleanup. + rcl_subscription_init( + &mut rcl_subscription, + &*rcl_node, + type_support_ptr, + topic_c_string.as_ptr(), + &rcl_subscription_options, + ) + .ok()?; + } + } + + let handle = Arc::new(SubscriptionHandle { + rcl_subscription: Mutex::new(rcl_subscription), + node_handle: Arc::clone(node_handle), + }); + + let callback = Arc::new(Mutex::new(callback.into())); + let metadata = Arc::new(metadata); + + let (waitable, lifecycle) = Waitable::new( + Box::new(DynamicSubscriptionExecutable { + handle: Arc::clone(&handle), + callback: Arc::clone(&callback), + commands: Arc::clone(commands), + metadata: Arc::clone(&metadata), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + commands.add_to_wait_set(waitable); + + Ok(Arc::new(Self { + handle, + callback, + lifecycle, + metadata, + type_support_library, + })) + } + + /// Returns the topic name of the subscription. + /// + /// This returns the topic name after remapping, so it is not necessarily the + /// topic name which was used when creating the subscription. + pub fn topic_name(&self) -> String { + self.handle.topic_name() + } + + /// Returns a description of the message structure. + pub fn structure(&self) -> &MessageStructure { + &self.metadata.structure + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::test_helpers::*; + + fn assert_send() {} + fn assert_sync() {} + + #[test] + fn dynamic_subscription_is_sync_and_send() { + assert_send::(); + assert_sync::(); + } + + #[test] + fn test_dynamic_subscriptions() -> Result<(), RclrsError> { + use crate::TopicEndpointInfo; + + let namespace = "/test_dynamic_subscriptions_graph"; + let graph = construct_test_graph(namespace)?; + + let node_2_empty_subscription = graph.node2.create_dynamic_subscription::<_>( + "test_msgs/msg/Empty".try_into()?, + "graph_test_topic_1", + |_, _| {}, + )?; + let topic1 = node_2_empty_subscription.topic_name(); + let node_2_basic_types_subscription = graph.node2.create_dynamic_subscription::<_>( + "test_msgs/msg/BasicTypes".try_into()?, + "graph_test_topic_2", + |_, _| {}, + )?; + let topic2 = node_2_basic_types_subscription.topic_name(); + + let node_1_defaults_subscription = graph.node1.create_dynamic_subscription::<_>( + "test_msgs/msg/Defaults".try_into()?, + "graph_test_topic_3", + |_, _| {}, + )?; + let topic3 = node_1_defaults_subscription.topic_name(); + + std::thread::sleep(std::time::Duration::from_millis(100)); + + // Test count_subscriptions() + assert_eq!(graph.node2.count_subscriptions(&topic1)?, 1); + assert_eq!(graph.node2.count_subscriptions(&topic2)?, 1); + + // Test get_subscription_names_and_types_by_node() + let node_1_subscription_names_and_types = graph + .node1 + .get_subscription_names_and_types_by_node(&graph.node1.name(), namespace)?; + + let types = node_1_subscription_names_and_types.get(&topic3).unwrap(); + assert!(types.contains(&"test_msgs/msg/Defaults".to_string())); + + let node_2_subscription_names_and_types = graph + .node2 + .get_subscription_names_and_types_by_node(&graph.node2.name(), namespace)?; + + let types = node_2_subscription_names_and_types.get(&topic1).unwrap(); + assert!(types.contains(&"test_msgs/msg/Empty".to_string())); + + let types = node_2_subscription_names_and_types.get(&topic2).unwrap(); + assert!(types.contains(&"test_msgs/msg/BasicTypes".to_string())); + + // Test get_subscriptions_info_by_topic() + let expected_subscriptions_info = vec![TopicEndpointInfo { + node_name: String::from("graph_test_node_2"), + node_namespace: String::from(namespace), + topic_type: String::from("test_msgs/msg/Empty"), + }]; + assert_eq!( + graph.node1.get_subscriptions_info_by_topic(&topic1)?, + expected_subscriptions_info + ); + assert_eq!( + graph.node2.get_subscriptions_info_by_topic(&topic1)?, + expected_subscriptions_info + ); + Ok(()) + } +} diff --git a/rclrs/src/dynamic_message/error.rs b/rclrs/src/dynamic_message/error.rs index 1303e9e30..777305b4c 100644 --- a/rclrs/src/dynamic_message/error.rs +++ b/rclrs/src/dynamic_message/error.rs @@ -1,6 +1,7 @@ use std::{error::Error, fmt}; /// An error related to creating a dynamic message based on the name of the message's type. +// TODO(luca) we need PartialEq (and maybe Eq?) for testing #[derive(Debug)] pub enum DynamicMessageError { /// The type support library was not found because no matching prefix was sourced. @@ -50,3 +51,15 @@ impl Error for DynamicMessageError { } } } + +impl PartialEq for DynamicMessageError { + fn eq(&self, other: &Self) -> bool { + if std::mem::discriminant(self) != std::mem::discriminant(other) { + return false; + } + // TODO(luca) this is not very efficient, revisit + return self.to_string() == other.to_string(); + } +} + +impl Eq for DynamicMessageError {} diff --git a/rclrs/src/dynamic_message/field_access.rs b/rclrs/src/dynamic_message/field_access.rs new file mode 100644 index 000000000..4ae2a5a17 --- /dev/null +++ b/rclrs/src/dynamic_message/field_access.rs @@ -0,0 +1,988 @@ +use rosidl_runtime_rs::Sequence; + +use super::{BaseType, MessageFieldInfo, ValueKind}; + +mod dynamic_bounded_string; +mod dynamic_message_view; +mod dynamic_sequence; +pub use dynamic_bounded_string::*; +pub use dynamic_message_view::*; +pub use dynamic_sequence::*; + +// Note: +// This module defines a bunch of structs in two flavors: immutable and mutable. +// It's annoying, but imo still better than doing something hard-to-read like +// https://lab.whitequark.org/notes/2016-12-13/abstracting-over-mutability-in-rust/ + +/// Helper function for use with reinterpret()/reinterpret_array() +fn check(bytes: &[u8]) { + assert!(bytes.len() >= std::mem::size_of::()); + let align = std::mem::align_of::(); + assert_eq!(bytes.as_ptr().align_offset(align), 0); +} + +// The purpose of this macro is to reduce duplication between the value types for the +// mutable and immutable cases. +macro_rules! define_value_types { + ($select:ident) => { + + // Creates either an immutable or a mutable reference to the type. + macro_rules! make_ref { ($lt:lifetime, $type:ty) => { + $select!( + immutable => &$lt $type, + mutable => &$lt mut $type + ) + }} + + /// A single value. + // The field variants are for the most part self-explaining. + #[allow(missing_docs)] + #[derive(Debug, PartialEq)] + pub enum SimpleValue<'msg> { + Float(make_ref!('msg, f32)), + Double(make_ref!('msg, f64)), + /// It's platform-dependent what the size of long double is. + /// Here's a pointer, you figure it out. + LongDouble($select!( + immutable => *const u8, + mutable => *mut u8 + )), + Char(make_ref!('msg, u8)), + WChar(make_ref!('msg, u16)), + Boolean(make_ref!('msg, bool)), + Octet(make_ref!('msg, u8)), + Uint8(make_ref!('msg, u8)), + Int8(make_ref!('msg, i8)), + Uint16(make_ref!('msg, u16)), + Int16(make_ref!('msg, i16)), + Uint32(make_ref!('msg, u32)), + Int32(make_ref!('msg, i32)), + Uint64(make_ref!('msg, u64)), + Int64(make_ref!('msg, i64)), + String(make_ref!('msg, rosidl_runtime_rs::String)), + BoundedString($select!( + immutable => DynamicBoundedString<'msg>, + mutable => DynamicBoundedStringMut<'msg> + )), + WString(make_ref!('msg, rosidl_runtime_rs::WString)), + BoundedWString($select!( + immutable => DynamicBoundedWString<'msg>, + mutable => DynamicBoundedWStringMut<'msg> + )), + Message($select!( + immutable => DynamicMessageView<'msg>, + mutable => DynamicMessageViewMut<'msg> + )), + } + + /// An array of values. + // The field variants are for the most part self-explaining. + #[allow(missing_docs)] + #[derive(Debug, PartialEq)] + pub enum ArrayValue<'msg> { + FloatArray(make_ref!('msg, [f32])), + DoubleArray(make_ref!('msg, [f64])), + /// It's platform-dependent what the size of long double is. + /// Here's a pointer and an array size, you figure it out. + LongDoubleArray($select!( + immutable => *const u8, + mutable => *mut u8 + ), usize), + CharArray(make_ref!('msg, [u8])), + WCharArray(make_ref!('msg, [u16])), + BooleanArray(make_ref!('msg, [bool])), + OctetArray(make_ref!('msg, [u8])), + Uint8Array(make_ref!('msg, [u8])), + Int8Array(make_ref!('msg, [i8])), + Uint16Array(make_ref!('msg, [u16])), + Int16Array(make_ref!('msg, [i16])), + Uint32Array(make_ref!('msg, [u32])), + Int32Array(make_ref!('msg, [i32])), + Uint64Array(make_ref!('msg, [u64])), + Int64Array(make_ref!('msg, [i64])), + StringArray(make_ref!('msg, [rosidl_runtime_rs::String])), + BoundedStringArray($select!( + immutable => Box<[DynamicBoundedString<'msg>]>, + mutable => Box<[DynamicBoundedStringMut<'msg>]> + )), + WStringArray(make_ref!('msg, [rosidl_runtime_rs::WString])), + BoundedWStringArray($select!( + immutable => Box<[DynamicBoundedWString<'msg>]>, + mutable => Box<[DynamicBoundedWStringMut<'msg>]> + )), + MessageArray($select!( + immutable => Box<[DynamicMessageView<'msg>]>, + mutable => Box<[DynamicMessageViewMut<'msg>]> + )), + } + + /// A sequence of unbounded length. + // The field variants are for the most part self-explaining. + // Developers: Please also see the explanation in dynamic_sequence.rs. + #[allow(missing_docs)] + #[derive(Debug, PartialEq)] + pub enum SequenceValue<'msg> { + FloatSequence(make_ref!('msg, Sequence)), + DoubleSequence(make_ref!('msg, Sequence)), + /// It's platform-dependent what the size of long double is. + /// Here's a pointer to the [`Sequence`][1] struct. + /// + /// [1]: rosidl_runtime_rs::Sequence + LongDoubleSequence($select!( + immutable => *const u8, + mutable => *mut u8 + )), + CharSequence(make_ref!('msg, Sequence)), + WCharSequence(make_ref!('msg, Sequence)), + BooleanSequence(make_ref!('msg, Sequence)), + OctetSequence(make_ref!('msg, Sequence)), + Uint8Sequence(make_ref!('msg, Sequence)), + Int8Sequence(make_ref!('msg, Sequence)), + Uint16Sequence(make_ref!('msg, Sequence)), + Int16Sequence(make_ref!('msg, Sequence)), + Uint32Sequence(make_ref!('msg, Sequence)), + Int32Sequence(make_ref!('msg, Sequence)), + Uint64Sequence(make_ref!('msg, Sequence)), + Int64Sequence(make_ref!('msg, Sequence)), + StringSequence(make_ref!('msg, Sequence)), + /// This variant is not a [`Sequence`][1], since there is no suitable element type + /// that both matches the underlying struct layout and includes information about + /// the string length bound. + /// + /// [1]: rosidl_runtime_rs::Sequence + BoundedStringSequence($select!( + immutable => DynamicSequence<'msg, DynamicBoundedString<'msg>>, + mutable => DynamicSequenceMut<'msg, DynamicBoundedStringMut<'msg>> + )), + WStringSequence(make_ref!('msg, Sequence)), + /// This variant is not a [`Sequence`][1], since there is no suitable element type + /// that both matches the underlying struct layout and includes information about + /// the string length bound. + /// + /// [1]: rosidl_runtime_rs::Sequence + BoundedWStringSequence($select!( + immutable => DynamicSequence<'msg, DynamicBoundedWString<'msg>>, + mutable => DynamicSequenceMut<'msg, DynamicBoundedWStringMut<'msg>> + )), + /// This variant is not a [`Sequence`][1], since the actual + /// element type has a different size and layout from [`DynamicMessageView`][2]. + /// + /// [1]: rosidl_runtime_rs::Sequence + /// [2]: DynamicMessageView + MessageSequence($select!( + immutable => DynamicSequence<'msg, DynamicMessageView<'msg>>, + mutable => DynamicSequenceMut<'msg, DynamicMessageViewMut<'msg>>) + ), + } + + // Internal type alias to avoid repeating this a hundred times + type BoundedSequence<'msg, T> = $select!( + immutable => DynamicBoundedSequence<'msg, T>, + mutable => DynamicBoundedSequenceMut<'msg, T> + ); + + /// A sequence of bounded length. + // The field variants are for the most part self-explaining. + // Developers: Please also see the explanation in dynamic_sequence.rs. + #[allow(missing_docs)] + #[derive(Debug, PartialEq)] + pub enum BoundedSequenceValue<'msg> { + FloatBoundedSequence(BoundedSequence<'msg, f32>), + DoubleBoundedSequence(BoundedSequence<'msg, f64>), + /// It's platform-dependent what the size of long double is. + /// Here's a pointer to the [`BoundedSequence`][1] struct and the upper bound. + /// + /// [1]: rosidl_runtime_rs::BoundedSequence + LongDoubleBoundedSequence($select!( + immutable => *const u8, + mutable => *mut u8 + ), usize), + CharBoundedSequence(BoundedSequence<'msg, u8>), + WCharBoundedSequence(BoundedSequence<'msg, u16>), + BooleanBoundedSequence(BoundedSequence<'msg, bool>), + OctetBoundedSequence(BoundedSequence<'msg, u8>), + Uint8BoundedSequence(BoundedSequence<'msg, u8>), + Int8BoundedSequence(BoundedSequence<'msg, i8>), + Uint16BoundedSequence(BoundedSequence<'msg, u16>), + Int16BoundedSequence(BoundedSequence<'msg, i16>), + Uint32BoundedSequence(BoundedSequence<'msg, u32>), + Int32BoundedSequence(BoundedSequence<'msg, i32>), + Uint64BoundedSequence(BoundedSequence<'msg, u64>), + Int64BoundedSequence(BoundedSequence<'msg, i64>), + StringBoundedSequence(BoundedSequence<'msg, rosidl_runtime_rs::String>), + BoundedStringBoundedSequence($select!( + immutable => DynamicBoundedSequence<'msg, DynamicBoundedString<'msg>>, + mutable => DynamicBoundedSequenceMut<'msg, DynamicBoundedStringMut<'msg>>) + ), + WStringBoundedSequence(BoundedSequence<'msg, rosidl_runtime_rs::WString>), + BoundedWStringBoundedSequence($select!( + immutable => DynamicBoundedSequence<'msg, DynamicBoundedWString<'msg>>, + mutable => DynamicBoundedSequenceMut<'msg, DynamicBoundedWStringMut<'msg>>) + ), + MessageBoundedSequence($select!( + immutable => DynamicBoundedSequence<'msg, DynamicMessageView<'msg>>, + mutable => DynamicBoundedSequenceMut<'msg, DynamicMessageViewMut<'msg>>) + ), + } + + // SAFETY: This is in effect a transmutation. + // + // Here is why this is safe when used as intended, i.e. to reinterpret bytes inside + // a dynamic message to their true types, assuming the introspection library is not lying: + // * There is no (safe) way for users of rcrls to cause an invalid bit pattern for T to + // be written to the storage of the dynamic message. All accesses are through references to T. + // * Correct alignment is ensured during the allocation of the storage for the dynamic message + // * The lifetime of the input slice is preserved in the produced reference, so + // this works exactly like borrowing a regular field from a message – the message + // can never be dropped while a borrow exists etc. + // * This function does not transmute & to &mut + // * This is only used for primitive values and rosidl_runtime_rs types marked as repr(C), + // so there is no risk of reinterpreting as a type with undefined layout. + unsafe fn reinterpret<'a, T>(bytes: make_ref!('a, [u8])) -> make_ref!('a, T) { + check::(bytes); + $select!( + immutable => { &*(bytes.as_ptr() as *const T) }, + mutable => { &mut *(bytes.as_mut_ptr() as *mut T) } + ) + } + + // SAFETY: This is in effect a transmutation. The caller of this function must ensure + // that the bytes correspond to a valid [T]. + // + // See also the reinterpret() function. + // + // std::slice::from_raw_parts is the correct way to transmute a slice. + // We can't rely on the internal representation of slices (or other stdlib types). + unsafe fn reinterpret_array<'a, T>(bytes: make_ref!('a, [u8]), array_size: usize) -> make_ref!('a, [T]) { + check::(bytes); + $select!( + immutable => { std::slice::from_raw_parts(bytes.as_ptr() as *const T, array_size) }, + mutable => { std::slice::from_raw_parts_mut(bytes.as_mut_ptr() as *mut T, array_size) } + ) + } + + impl<'msg> SimpleValue<'msg> { + pub(super) unsafe fn new( + bytes: make_ref!('msg, [u8]), + field_info: &'msg MessageFieldInfo, + ) -> Self { + match &field_info.base_type { + BaseType::Float => SimpleValue::Float(reinterpret::(bytes)), + BaseType::Double => SimpleValue::Double(reinterpret::(bytes)), + BaseType::LongDouble => SimpleValue::LongDouble($select!( + immutable => bytes.as_ptr(), + mutable => bytes.as_mut_ptr() + )), + BaseType::Char => SimpleValue::Char(reinterpret::(bytes)), + BaseType::WChar => SimpleValue::WChar(reinterpret::(bytes)), + BaseType::Boolean => { + assert!(bytes[0] <= 1); + SimpleValue::Boolean(reinterpret::(bytes)) + } + BaseType::Octet => SimpleValue::Octet(reinterpret::(bytes)), + BaseType::Uint8 => SimpleValue::Uint8(reinterpret::(bytes)), + BaseType::Int8 => SimpleValue::Int8(reinterpret::(bytes)), + BaseType::Uint16 => SimpleValue::Uint16(reinterpret::(bytes)), + BaseType::Int16 => SimpleValue::Int16(reinterpret::(bytes)), + BaseType::Uint32 => SimpleValue::Uint32(reinterpret::(bytes)), + BaseType::Int32 => SimpleValue::Int32(reinterpret::(bytes)), + BaseType::Uint64 => SimpleValue::Uint64(reinterpret::(bytes)), + BaseType::Int64 => SimpleValue::Int64(reinterpret::(bytes)), + BaseType::String => SimpleValue::String(reinterpret::(bytes)), + BaseType::BoundedString { upper_bound } => { + SimpleValue::BoundedString($select!( + immutable => DynamicBoundedString { + inner: reinterpret::(bytes), + upper_bound: *upper_bound, + }, + mutable => DynamicBoundedStringMut { + inner: reinterpret::(bytes), + upper_bound: *upper_bound, + } + )) + } + BaseType::WString => SimpleValue::WString(reinterpret::(bytes)), + BaseType::BoundedWString { upper_bound } => { + SimpleValue::BoundedWString($select!( + immutable => DynamicBoundedWString { + inner: reinterpret::(bytes), + upper_bound: *upper_bound, + }, + mutable => DynamicBoundedWStringMut { + inner: reinterpret::(bytes), + upper_bound: *upper_bound, + } + )) + } + BaseType::Message(structure) => SimpleValue::Message($select!( + immutable => DynamicMessageView { + storage: &bytes[..structure.size], + structure: &*structure, + }, + mutable => DynamicMessageViewMut { + storage: &mut bytes[..structure.size], + structure: &*structure, + } + )), + } + } + } + + impl<'msg> ArrayValue<'msg> { + pub(super) unsafe fn new( + bytes: make_ref!('msg, [u8]), + field_info: &'msg MessageFieldInfo, + array_length: usize, + ) -> Self { + match &field_info.base_type { + BaseType::Float => { + ArrayValue::FloatArray(reinterpret_array::(bytes, array_length)) + } + BaseType::Double => { + ArrayValue::DoubleArray(reinterpret_array::(bytes, array_length)) + } + BaseType::LongDouble => { + ArrayValue::LongDoubleArray($select!( + immutable => bytes.as_ptr(), + mutable => bytes.as_mut_ptr() + ), array_length) + } + BaseType::Char => { + ArrayValue::CharArray(reinterpret_array::(bytes, array_length)) + } + BaseType::WChar => { + ArrayValue::WCharArray(reinterpret_array::(bytes, array_length)) + } + BaseType::Boolean => { + assert!(bytes[0] <= 1); + ArrayValue::BooleanArray(reinterpret_array::( + bytes, + array_length, + )) + } + BaseType::Octet => { + ArrayValue::OctetArray(reinterpret_array::(bytes, array_length)) + } + BaseType::Uint8 => { + ArrayValue::Uint8Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Int8 => { + ArrayValue::Int8Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Uint16 => { + ArrayValue::Uint16Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Int16 => { + ArrayValue::Int16Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Uint32 => { + ArrayValue::Uint32Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Int32 => { + ArrayValue::Int32Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Uint64 => { + ArrayValue::Uint64Array(reinterpret_array::(bytes, array_length)) + } + BaseType::Int64 => { + ArrayValue::Int64Array(reinterpret_array::(bytes, array_length)) + } + BaseType::String => { + ArrayValue::StringArray(reinterpret_array::( + bytes, + array_length, + )) + } + BaseType::BoundedString { upper_bound } => { + let slice = reinterpret_array::( + bytes, + array_length, + ); + let dynamic_bounded_strings: Vec<_> = slice + .into_iter() + .map(|inner| $select!( + immutable => DynamicBoundedString { + inner, + upper_bound: *upper_bound, + }, + mutable => DynamicBoundedStringMut { + inner, + upper_bound: *upper_bound, + } + )) + .collect(); + ArrayValue::BoundedStringArray( + dynamic_bounded_strings.into_boxed_slice(), + ) + } + BaseType::WString => { + ArrayValue::WStringArray(reinterpret_array::( + bytes, + array_length, + )) + } + BaseType::BoundedWString { upper_bound } => { + let slice = reinterpret_array::( + bytes, + array_length, + ); + let dynamic_bounded_wstrings: Vec<_> = slice + .into_iter() + .map(|inner| $select!( + immutable => DynamicBoundedWString { + inner, + upper_bound: *upper_bound, + }, + mutable => DynamicBoundedWStringMut { + inner, + upper_bound: *upper_bound, + } + )) + .collect(); + ArrayValue::BoundedWStringArray( + dynamic_bounded_wstrings.into_boxed_slice(), + ) + } + BaseType::Message(structure) => { + let messages: Vec<_> = $select!( + immutable => bytes.chunks(structure.size) + .take(array_length) + .map(|chunk| DynamicMessageView { + storage: chunk, + structure: &*structure, + }) + .collect(), + mutable => bytes.chunks_mut(structure.size) + .take(array_length) + .map(|chunk| DynamicMessageViewMut { + storage: chunk, + structure: &*structure, + }) + .collect() + ); + ArrayValue::MessageArray(messages.into_boxed_slice()) + } + } + } + } + + impl<'msg> SequenceValue<'msg> { + pub(super) unsafe fn new( + bytes: make_ref!('msg, [u8]), + field_info: &'msg MessageFieldInfo, + ) -> Self { + match &field_info.base_type { + BaseType::Float => { + SequenceValue::FloatSequence(reinterpret::>(bytes)) + } + BaseType::Double => { + SequenceValue::DoubleSequence(reinterpret::>(bytes)) + } + BaseType::LongDouble => SequenceValue::LongDoubleSequence($select!( + immutable => bytes.as_ptr(), + mutable => bytes.as_mut_ptr() + )), + BaseType::Char => { + SequenceValue::CharSequence(reinterpret::>(bytes)) + } + BaseType::WChar => { + SequenceValue::WCharSequence(reinterpret::>(bytes)) + } + BaseType::Boolean => { + SequenceValue::BooleanSequence(reinterpret::>(bytes)) + } + BaseType::Octet => { + SequenceValue::OctetSequence(reinterpret::>(bytes)) + } + BaseType::Uint8 => { + SequenceValue::Uint8Sequence(reinterpret::>(bytes)) + } + BaseType::Int8 => { + SequenceValue::Int8Sequence(reinterpret::>(bytes)) + } + BaseType::Uint16 => { + SequenceValue::Uint16Sequence(reinterpret::>(bytes)) + } + BaseType::Int16 => { + SequenceValue::Int16Sequence(reinterpret::>(bytes)) + } + BaseType::Uint32 => { + SequenceValue::Uint32Sequence(reinterpret::>(bytes)) + } + BaseType::Int32 => { + SequenceValue::Int32Sequence(reinterpret::>(bytes)) + } + BaseType::Uint64 => { + SequenceValue::Uint64Sequence(reinterpret::>(bytes)) + } + BaseType::Int64 => { + SequenceValue::Int64Sequence(reinterpret::>(bytes)) + } + BaseType::String => { + SequenceValue::StringSequence(reinterpret::< + Sequence, + >(bytes)) + } + BaseType::BoundedString { upper_bound } => { + SequenceValue::BoundedStringSequence( + $select!( + immutable => { + DynamicSequence::new_proxy( + bytes, + *upper_bound + ) + }, + mutable => DynamicSequenceMut::new_proxy( + bytes, + *upper_bound, + field_info.resize_function.unwrap(), + ) + ) + ) + } + BaseType::WString => { + SequenceValue::WStringSequence(reinterpret::< + Sequence, + >(bytes)) + } + BaseType::BoundedWString { upper_bound } => { + SequenceValue::BoundedWStringSequence( + $select!( + immutable => { + DynamicSequence::new_proxy( + bytes, + *upper_bound + ) + }, + mutable => DynamicSequenceMut::new_proxy( + bytes, + *upper_bound, + field_info.resize_function.unwrap(), + ) + ) + ) + } + BaseType::Message(structure) => { + SequenceValue::MessageSequence($select!( + immutable => { + DynamicSequence::new_proxy( + bytes, + &**structure, + ) + }, + mutable => DynamicSequenceMut::new_proxy( + bytes, + &**structure, + field_info.resize_function.unwrap() + ) + )) + } + } + } + } + + impl<'msg> BoundedSequenceValue<'msg> { + pub(super) unsafe fn new( + bytes: make_ref!('msg, [u8]), + field_info: &'msg MessageFieldInfo, + sequence_upper_bound: usize, + ) -> Self { + match &field_info.base_type { + BaseType::Float => { + BoundedSequenceValue::FloatBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Double => { + BoundedSequenceValue::DoubleBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::LongDouble => BoundedSequenceValue::LongDoubleBoundedSequence( + $select!( + immutable => bytes.as_ptr(), + mutable => bytes.as_mut_ptr() + ), + sequence_upper_bound, + ), + BaseType::Char => { + BoundedSequenceValue::CharBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::WChar => { + BoundedSequenceValue::WCharBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Boolean => { + BoundedSequenceValue::BooleanBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Octet => { + BoundedSequenceValue::OctetBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Uint8 => { + BoundedSequenceValue::Uint8BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Int8 => { + BoundedSequenceValue::Int8BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Uint16 => { + BoundedSequenceValue::Uint16BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Int16 => { + BoundedSequenceValue::Int16BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Uint32 => { + BoundedSequenceValue::Uint32BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Int32 => { + BoundedSequenceValue::Int32BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Uint64 => { + BoundedSequenceValue::Uint64BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::Int64 => { + BoundedSequenceValue::Int64BoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )) + } + BaseType::String => { + BoundedSequenceValue::StringBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap() + ) + )) + } + BaseType::BoundedString { upper_bound } => { + BoundedSequenceValue::BoundedStringBoundedSequence($select!( + immutable => { DynamicBoundedSequence::new_proxy(bytes, sequence_upper_bound, *upper_bound) }, + mutable => DynamicBoundedSequenceMut::new_proxy( + bytes, + *upper_bound, + sequence_upper_bound, + field_info.resize_function.unwrap() + ) + )) + } + BaseType::WString => { + BoundedSequenceValue::WStringBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_primitive( + bytes, + sequence_upper_bound + ) + }, + mutable => DynamicBoundedSequenceMut::new_primitive( + bytes, + sequence_upper_bound, + field_info.resize_function.unwrap() + ) + )) + } + BaseType::BoundedWString { upper_bound } => { + BoundedSequenceValue::BoundedWStringBoundedSequence($select!( + immutable => { DynamicBoundedSequence::new_proxy(bytes, sequence_upper_bound, *upper_bound) }, + mutable => DynamicBoundedSequenceMut::new_proxy( + bytes, + *upper_bound, + sequence_upper_bound, + field_info.resize_function.unwrap() + ) + )) + } + BaseType::Message(structure) => BoundedSequenceValue::MessageBoundedSequence($select!( + immutable => { + DynamicBoundedSequence::new_proxy(bytes, sequence_upper_bound, &**structure) + }, + mutable => DynamicBoundedSequenceMut::new_proxy( + bytes, + &**structure, + sequence_upper_bound, + field_info.resize_function.unwrap(), + ) + )), + } + } + } + }; +} + +mod immutable { + use super::*; + + macro_rules! select_immutable { + (immutable => $a:ty, mutable => $b:ty) => { + $a + }; + (immutable => $a:expr, mutable => $b:expr) => { + $a + }; + } + + define_value_types!(select_immutable); +} + +pub use immutable::ArrayValue; +pub use immutable::BoundedSequenceValue; +pub use immutable::SequenceValue; +pub use immutable::SimpleValue; + +/// The value of a field in a [`DynamicMessage`][1]. +/// +/// This type, and all the types inside it, are reference types – they contain +/// only a reference to the underlying data. +/// +/// [1]: crate::dynamic_message::DynamicMessage +#[derive(Debug, PartialEq)] +pub enum Value<'msg> { + /// A single value. + Simple(SimpleValue<'msg>), + /// An array of values. + Array(ArrayValue<'msg>), + /// A sequence of unbounded length. + Sequence(SequenceValue<'msg>), + /// A sequence of bounded length. + BoundedSequence(BoundedSequenceValue<'msg>), +} +impl<'msg> Value<'msg> { + pub(crate) unsafe fn new( + value_bytes: &'msg [u8], + field_info: &'msg MessageFieldInfo, + ) -> Option> { + Some(match field_info.value_kind { + ValueKind::Simple => Value::Simple(SimpleValue::new(value_bytes, field_info)), + ValueKind::Array { length } => { + Value::Array(ArrayValue::new(value_bytes, field_info, length)) + } + ValueKind::Sequence => Value::Sequence(SequenceValue::new(value_bytes, field_info)), + ValueKind::BoundedSequence { upper_bound } => Value::BoundedSequence( + BoundedSequenceValue::new(value_bytes, field_info, upper_bound), + ), + }) + } +} + +mod mutable { + use super::*; + + macro_rules! select_mutable { + (immutable => $a:ty, mutable => $b:ty) => { + $b + }; + (immutable => $a:expr, mutable => $b:expr) => { + $b + }; + } + + define_value_types!(select_mutable); +} + +pub use mutable::ArrayValue as ArrayValueMut; +pub use mutable::BoundedSequenceValue as BoundedSequenceValueMut; +pub use mutable::SequenceValue as SequenceValueMut; +pub use mutable::SimpleValue as SimpleValueMut; + +/// The value of a field in a [`DynamicMessage`][1]. +/// +/// This type, and all the types inside it, are reference types – they contain +/// only a reference to the underlying data. +/// +/// [1]: crate::dynamic_message::DynamicMessage +#[derive(Debug, PartialEq)] +pub enum ValueMut<'msg> { + /// A single value. + Simple(SimpleValueMut<'msg>), + /// An array of values. + Array(ArrayValueMut<'msg>), + /// A sequence of unbounded length. + Sequence(SequenceValueMut<'msg>), + /// A sequence of bounded length. + BoundedSequence(BoundedSequenceValueMut<'msg>), +} + +impl<'msg> ValueMut<'msg> { + pub(crate) unsafe fn new( + value_bytes: &'msg mut [u8], + field_info: &'msg MessageFieldInfo, + ) -> ValueMut<'msg> { + match field_info.value_kind { + ValueKind::Simple => ValueMut::Simple(SimpleValueMut::new(value_bytes, field_info)), + ValueKind::Array { length } => { + ValueMut::Array(ArrayValueMut::new(value_bytes, field_info, length)) + } + ValueKind::Sequence => { + ValueMut::Sequence(SequenceValueMut::new(value_bytes, field_info)) + } + ValueKind::BoundedSequence { upper_bound } => ValueMut::BoundedSequence( + BoundedSequenceValueMut::new(value_bytes, field_info, upper_bound), + ), + } + } +} diff --git a/rclrs/src/dynamic_message/field_access/dynamic_bounded_string.rs b/rclrs/src/dynamic_message/field_access/dynamic_bounded_string.rs new file mode 100644 index 000000000..431c8eb69 --- /dev/null +++ b/rclrs/src/dynamic_message/field_access/dynamic_bounded_string.rs @@ -0,0 +1,256 @@ +use std::convert::AsMut; +use std::fmt::{self, Display}; +use std::num::NonZeroUsize; +use std::ops::{Deref, DerefMut}; + +use rosidl_runtime_rs::StringExceedsBoundsError; + +use super::{DynamicSequenceElementMut, Proxy, ProxyMut, ProxySequence}; + +/// A bounded String whose upper bound is only known at runtime. +/// +/// It derefs to a [`rosidl_runtime_rs::String`], which allows conversion +/// into a regular string, and more. +#[derive(Debug, Hash, PartialOrd, PartialEq, Eq, Ord)] +pub struct DynamicBoundedString<'msg> { + pub(super) inner: &'msg rosidl_runtime_rs::String, + pub(super) upper_bound: NonZeroUsize, +} + +/// A bounded WString whose upper bound is only known at runtime. +/// +/// It derefs to a [`rosidl_runtime_rs::WString`], which allows conversion +/// into a regular string, and more. +#[derive(Debug, Hash, PartialOrd, PartialEq, Eq, Ord)] +pub struct DynamicBoundedWString<'msg> { + pub(super) inner: &'msg rosidl_runtime_rs::WString, + pub(super) upper_bound: NonZeroUsize, +} + +/// A bounded String whose upper bound is only known at runtime. +/// +/// Like its immutable counterpart [`DynamicBoundedString`], it derefs to a +/// [`rosidl_runtime_rs::String`], but not mutably. This is to make sure that +/// methods which change the string's length can not be accessed, so that the +/// length never exceeds the upper bound. +/// Instead, this type provides its own mutation methods, which check the length, +/// an an [`AsMut`][1] instance. +/// +/// [1]: std::convert::AsMut +#[derive(Debug, Hash, PartialOrd, PartialEq, Eq, Ord)] +pub struct DynamicBoundedStringMut<'msg> { + pub(super) inner: &'msg mut rosidl_runtime_rs::String, + pub(super) upper_bound: NonZeroUsize, +} + +/// A bounded WString whose upper bound is only known at runtime. +/// +/// Like its immutable counterpart [`DynamicBoundedWString`], it derefs to a +/// [`rosidl_runtime_rs::WString`], but not mutably. This is to make sure that +/// methods which change the string's length can not be accessed, so that the +/// length never exceeds the upper bound. +/// Instead, this type provides its own mutation methods, which check the length, +/// an an [`AsMut`][1] instance. +/// +/// [1]: std::convert::AsMut +#[derive(Debug, Hash, PartialOrd, PartialEq, Eq, Ord)] +pub struct DynamicBoundedWStringMut<'msg> { + pub(super) inner: &'msg mut rosidl_runtime_rs::WString, + pub(super) upper_bound: NonZeroUsize, +} + +// ========================= impl for DynamicBounded(W)String ========================= + +impl<'msg> Deref for DynamicBoundedString<'msg> { + type Target = rosidl_runtime_rs::String; + fn deref(&self) -> &Self::Target { + self.inner + } +} + +impl<'msg> Display for DynamicBoundedString<'msg> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.inner.fmt(f) + } +} + +unsafe impl<'msg> Proxy<'msg> for DynamicBoundedString<'msg> { + type Metadata = NonZeroUsize; // String upper bound + + fn size_in_memory(_: NonZeroUsize) -> usize { + std::mem::size_of::() + } + + unsafe fn new(bytes: &'msg [u8], string_upper_bound: NonZeroUsize) -> Self { + let inner = &*(bytes.as_ptr() as *const rosidl_runtime_rs::String); + Self { + inner, + upper_bound: string_upper_bound, + } + } +} + +impl<'msg> DynamicBoundedString<'msg> { + /// Returns the maximum length of this string. + pub fn upper_bound(&self) -> NonZeroUsize { + self.upper_bound + } +} + +impl<'msg> DynamicBoundedWString<'msg> { + /// Returns the maximum length of this string. + pub fn upper_bound(&self) -> NonZeroUsize { + self.upper_bound + } +} + +impl<'msg> Deref for DynamicBoundedWString<'msg> { + type Target = rosidl_runtime_rs::WString; + fn deref(&self) -> &Self::Target { + self.inner + } +} + +impl<'msg> Display for DynamicBoundedWString<'msg> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.inner.fmt(f) + } +} + +unsafe impl<'msg> Proxy<'msg> for DynamicBoundedWString<'msg> { + type Metadata = NonZeroUsize; // String upper bound + + fn size_in_memory(_: NonZeroUsize) -> usize { + std::mem::size_of::() + } + + unsafe fn new(bytes: &'msg [u8], string_upper_bound: NonZeroUsize) -> Self { + let inner = &*(bytes.as_ptr() as *const rosidl_runtime_rs::WString); + Self { + inner, + upper_bound: string_upper_bound, + } + } +} + +// ========================= impl for DynamicBounded(W)StringMut ========================= + +impl<'msg> AsMut<[std::os::raw::c_char]> for DynamicBoundedStringMut<'msg> { + fn as_mut(&mut self) -> &mut [std::os::raw::c_char] { + self.inner.deref_mut() + } +} + +impl<'msg> Deref for DynamicBoundedStringMut<'msg> { + type Target = rosidl_runtime_rs::String; + fn deref(&self) -> &Self::Target { + self.inner + } +} + +impl<'msg> Display for DynamicBoundedStringMut<'msg> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.inner.fmt(f) + } +} + +impl<'msg> DynamicSequenceElementMut<'msg> for DynamicBoundedStringMut<'msg> { + type InnerSequence = ProxySequence<'msg, Self>; +} + +unsafe impl<'msg> ProxyMut<'msg> for DynamicBoundedStringMut<'msg> { + type Metadata = NonZeroUsize; // String upper bound + + fn size_in_memory(_: NonZeroUsize) -> usize { + std::mem::size_of::() + } + + unsafe fn new(bytes: &'msg mut [u8], string_upper_bound: NonZeroUsize) -> Self { + let inner = &mut *(bytes.as_mut_ptr() as *mut rosidl_runtime_rs::String); + Self { + inner, + upper_bound: string_upper_bound, + } + } +} + +impl<'msg> DynamicBoundedStringMut<'msg> { + /// Returns the maximum length of this string. + pub fn upper_bound(&self) -> NonZeroUsize { + self.upper_bound + } + + /// If the given string is not too long, assign it to self. + pub fn try_assign(&mut self, s: &str) -> Result<(), StringExceedsBoundsError> { + let length = s.chars().count(); + if length <= self.upper_bound.into() { + *self.inner = rosidl_runtime_rs::String::from(s); + Ok(()) + } else { + Err(StringExceedsBoundsError { + len: length, + upper_bound: self.upper_bound.into(), + }) + } + } +} + +impl<'msg> AsMut<[std::os::raw::c_ushort]> for DynamicBoundedWStringMut<'msg> { + fn as_mut(&mut self) -> &mut [std::os::raw::c_ushort] { + self.inner.deref_mut() + } +} + +impl<'msg> Deref for DynamicBoundedWStringMut<'msg> { + type Target = rosidl_runtime_rs::WString; + fn deref(&self) -> &Self::Target { + self.inner + } +} + +impl<'msg> Display for DynamicBoundedWStringMut<'msg> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.inner.fmt(f) + } +} + +impl<'msg> DynamicSequenceElementMut<'msg> for DynamicBoundedWStringMut<'msg> { + type InnerSequence = ProxySequence<'msg, Self>; +} + +unsafe impl<'msg> ProxyMut<'msg> for DynamicBoundedWStringMut<'msg> { + type Metadata = NonZeroUsize; // String upper bound + + fn size_in_memory(_: NonZeroUsize) -> usize { + std::mem::size_of::() + } + + unsafe fn new(bytes: &'msg mut [u8], string_upper_bound: NonZeroUsize) -> Self { + let inner = &mut *(bytes.as_mut_ptr() as *mut rosidl_runtime_rs::WString); + Self { + inner, + upper_bound: string_upper_bound, + } + } +} + +impl<'msg> DynamicBoundedWStringMut<'msg> { + /// Returns the maximum length of this string. + pub fn upper_bound(&self) -> NonZeroUsize { + self.upper_bound + } + + /// If the given string is not too long, assign it to self. + pub fn try_assign(&mut self, s: &str) -> Result<(), StringExceedsBoundsError> { + let length = s.chars().count(); + if length <= self.upper_bound.into() { + *self.inner = rosidl_runtime_rs::WString::from(s); + Ok(()) + } else { + Err(StringExceedsBoundsError { + len: length, + upper_bound: self.upper_bound.into(), + }) + } + } +} diff --git a/rclrs/src/dynamic_message/field_access/dynamic_message_view.rs b/rclrs/src/dynamic_message/field_access/dynamic_message_view.rs new file mode 100644 index 000000000..eb59d1da9 --- /dev/null +++ b/rclrs/src/dynamic_message/field_access/dynamic_message_view.rs @@ -0,0 +1,199 @@ +use std::fmt::{self, Debug}; +use std::ops::Deref; + +use super::super::MessageStructure; +use super::{DynamicSequenceElementMut, Proxy, ProxyMut, ProxySequence, Value, ValueMut}; + +/// A view of a single message. Used for nested messages. +/// +/// This allows reading the fields of the message, but not modifying them. +#[derive(PartialEq, Eq)] +pub struct DynamicMessageView<'msg> { + pub(crate) structure: &'msg MessageStructure, + pub(crate) storage: &'msg [u8], +} + +/// A mutable view of a single message. Used for nested messages. +/// +/// This allows reading and modifying the fields of the message. +#[derive(PartialEq, Eq)] +pub struct DynamicMessageViewMut<'msg> { + pub(crate) structure: &'msg MessageStructure, + pub(crate) storage: &'msg mut [u8], +} + +// ========================= impl for DynamicMessageView ========================= + +impl<'msg> Debug for DynamicMessageView<'msg> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + let mut struct_ = f.debug_struct(&self.structure().type_name); + for field in &self.structure().fields { + let value = self.get(&field.name).unwrap(); + struct_.field(&field.name, &value as &dyn Debug); + } + struct_.finish() + } +} + +impl<'msg> Deref for DynamicMessageView<'msg> { + type Target = MessageStructure; + fn deref(&self) -> &Self::Target { + self.structure + } +} + +unsafe impl<'msg> Proxy<'msg> for DynamicMessageView<'msg> { + type Metadata = &'msg MessageStructure; + + fn size_in_memory(structure: Self::Metadata) -> usize { + structure.size + } + + unsafe fn new(bytes: &'msg [u8], structure: Self::Metadata) -> Self { + DynamicMessageView { + structure, + storage: bytes, + } + } +} + +impl<'msg> DynamicMessageView<'msg> { + /// Tries to access a field in the message. + /// + /// If no field of that name exists, `None` is returned. + pub fn get(&self, field_name: &str) -> Option> { + let field_info = self.structure.get_field_info(field_name)?; + // For the unwrap_or, see DynamicMessageViewMut::get_mut + let size = field_info.size().unwrap_or(1); + let bytes = &self.storage[field_info.offset..field_info.offset + size]; + // SAFETY: The bytes contain a valid field of the type recorded in field_info. + unsafe { Value::new(bytes, field_info) } + } + + /// Returns a description of the message structure. + pub fn structure(&self) -> &MessageStructure { + self.structure + } + + /// Iterate over all fields in declaration order. + pub fn iter(&self) -> impl Iterator)> + '_ { + self.structure.fields.iter().map(|field_info| { + let value = self.get(&field_info.name).unwrap(); + (field_info.name.as_str(), value) + }) + } +} + +// ========================= impl for DynamicMessageViewMut ========================= + +impl<'msg> Debug for DynamicMessageViewMut<'msg> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + DynamicMessageView { + structure: self.structure, + storage: &*self.storage, + } + .fmt(f) + } +} + +impl<'msg> Deref for DynamicMessageViewMut<'msg> { + type Target = MessageStructure; + fn deref(&self) -> &Self::Target { + self.structure + } +} + +impl<'msg> DynamicSequenceElementMut<'msg> for DynamicMessageViewMut<'msg> { + type InnerSequence = ProxySequence<'msg, Self>; +} + +unsafe impl<'msg> ProxyMut<'msg> for DynamicMessageViewMut<'msg> { + type Metadata = &'msg MessageStructure; + + fn size_in_memory(structure: Self::Metadata) -> usize { + structure.size + } + + unsafe fn new(bytes: &'msg mut [u8], structure: Self::Metadata) -> Self { + DynamicMessageViewMut { + structure, + storage: bytes, + } + } +} + +impl<'msg> DynamicMessageViewMut<'msg> { + /// Tries to access a field in the message. + /// + /// If no field of that name exists, `None` is returned. + pub fn get(&self, field_name: &str) -> Option> { + let field_info = self.structure.get_field_info(field_name)?; + // For the unwrap_or, see DynamicMessageViewMut::get_mut + let size = field_info.size().unwrap_or(1); + let bytes = &self.storage[field_info.offset..field_info.offset + size]; + // SAFETY: The bytes contain a valid field of the type recorded in field_info. + unsafe { Value::new(bytes, field_info) } + } + + /// Tries to mutably access a field in the message. + /// + /// If no field of that name exists, `None` is returned. + pub fn get_mut(&mut self, field_name: &str) -> Option> { + let field_info = self.structure.get_field_info(field_name)?; + // The size is None for LongDouble, which has platform-dependent size. + // It's fine to pass in 1 here – the length of the slice isn't strictly needed + // by this function, especially not for a LongDouble value. + let size = field_info.size().unwrap_or(1); + let bytes = &mut self.storage[field_info.offset..field_info.offset + size]; + // SAFETY: The bytes contain a valid field of the type recorded in field_info. + Some(unsafe { ValueMut::new(bytes, field_info) }) + } + + /// Returns a description of the message structure. + pub fn structure(&self) -> &MessageStructure { + self.structure + } + + /// Iterate over all fields in declaration order. + pub fn iter(&self) -> impl Iterator)> + '_ { + self.structure.fields.iter().map(|field_info| { + let value = self.get(&field_info.name).unwrap(); + (field_info.name.as_str(), value) + }) + } + + /// Iterate over all fields in declaration order (mutable version). + /// + /// Note that, unusually for an `iter_mut()` method, this method takes `self` + /// and not `&mut self`. This is because the values should borrow directly from + /// the message, not the `MessageViewMut`. + pub fn iter_mut(self) -> impl Iterator)> + 'msg { + // This looks different from iter() because naively calling self.get_mut() for + // each field would create multiple mutable references into the same slice. + // Create an iterator that contains the _end_ offset of each field. + let offsets_next = self + .structure + .fields + .iter() + .map(|field_info| field_info.offset) + .chain([self.structure.size]) + .skip(1); + // By zipping, we have info about the start offset and end offset of each field. + self.structure.fields.iter().zip(offsets_next).scan( + self.storage, + |remainder: &mut &'msg mut [u8], (field_info, next_field_offset)| { + // Chop off bytes of the field's size from the front. + // remainder is of type &'closure mut &'a mut [i32], + // and calling remainder.split_at_mut would move out of + // the outer reference, so it's forbidden + let rem = std::mem::take(remainder); + let (value_bytes, rem) = rem.split_at_mut(next_field_offset - field_info.offset); + *remainder = rem; + + // SAFETY: The bytes contain a valid field of the type recorded in field_info. + let value = unsafe { ValueMut::new(value_bytes, field_info) }; + Some((field_info.name.as_str(), value)) + }, + ) + } +} diff --git a/rclrs/src/dynamic_message/field_access/dynamic_sequence.rs b/rclrs/src/dynamic_message/field_access/dynamic_sequence.rs new file mode 100644 index 000000000..7c5fa23ab --- /dev/null +++ b/rclrs/src/dynamic_message/field_access/dynamic_sequence.rs @@ -0,0 +1,600 @@ +use std::fmt::{self, Debug}; +use std::marker::PhantomData; +use std::ops::{Deref, DerefMut}; + +use rosidl_runtime_rs::{Sequence, SequenceAlloc, SequenceExceedsBoundsError}; + +use super::check; + +// We cannot always use &Sequence and &mut Sequence as types for accessing sequence fields. +// This is for two reasons: +// 1. The type T might be a sub-message, or a bounded string/wstring. These types have some +// associated metadata, namely the message structure and the string length bound, which needs +// to be available when accessing/modifying the type T. Therefore, fields of this type are +// accessed via their "proxy" wrappers, such as `DynamicMessageView`, which include the +// metadata. The memory layout of the proxy type doesn't match that of T, so we cannot cast +// Sequence to Sequence. +// 2a. The sequence might be bounded. The API of Sequence allows unchecked length changes. +// 2b. For a bounded sequence, it would be nice if the sequence type had a getter for the upper +// bound, but Sequence doesn't know the upper bound. +// +// So, following these criteria, these are the possible sequence types for each combination of +// bounded/unbounded, mutable/immutable, and native/proxy element type: +// +// seq kind | mutability | element type | possible sequence types +// ----------+------------+--------------+-------------- +// unbounded | immutable | native | &Sequence or &[T]* +// unbounded | immutable | proxy | Box* +// unbounded | mutable | native | &mut Sequence* +// unbounded | mutable | proxy | custom type that can store proxy objects +// bounded | immutable | native | (usize, Box) or (usize, &Sequence)* +// bounded | immutable | proxy | (usize, Box)* +// bounded | mutable | native | custom type that enforces upper bound +// bounded | mutable | proxy | custom type that enforces upper bound and can store proxy objects +// +// * or an equivalent custom type +// +// This module chooses to expose the following types for this purpose: +// +// seq kind | mutability | element type | sequence type +// ----------+------------+--------------+-------------- +// unbounded | immutable | native | &Sequence +// unbounded | immutable | proxy | DynamicSequence (newtype of Box) +// unbounded | mutable | native | &mut Sequence +// unbounded | mutable | proxy | DynamicSequenceMut +// bounded | immutable | native | DynamicBoundedSequence (similar to Cow<[T]>) +// bounded | immutable | proxy | DynamicBoundedSequence (similar to Cow<[T]>) +// bounded | mutable | native | DynamicBoundedSequenceMut (based on DynamicSequenceMut) +// bounded | mutable | proxy | DynamicBoundedSequenceMut (based on DynamicSequenceMut) +// +// That means that DynamicBoundedSequence and DynamicBoundedSequenceMut must be able to hold +// both native and proxy objects. + +// ========================= Abstracting over different proxy types ========================= + +// These traits abstract over types that are "proxy objects" for the actual data stored +// in a message. See also the explanation above for context. +// +// There are three types implementing these traits (dynamic versions of String, WString and messages). +// Without these traits, you'd have to e.g. write three versions of the ProxySequence struct, and +// of its implementation of the InnerSequence trait, and much more. + +/// An immutable proxy object. +/// +/// This trait is unsafe because memory errors will occur if size_in_memory() is +/// implemented incorrectly. +#[doc(hidden)] +pub unsafe trait Proxy<'msg> { + // In the case of strings, this is the string length upper bound, + // and in the case of messages, it is its structure. + type Metadata: 'msg + Copy; + // How many bytes does each element take up in the underlying sequence? + fn size_in_memory(metadata: Self::Metadata) -> usize; + // This function is unsafe because the bytes must correspond to the proxied object. + unsafe fn new(bytes: &'msg [u8], metadata: Self::Metadata) -> Self; +} + +/// An mutable proxy object. +/// +/// This trait is unsafe because memory errors will occur if size_in_memory() is +/// implemented incorrectly. +#[doc(hidden)] +pub unsafe trait ProxyMut<'msg> { + // In the case of strings, this is the string length upper bound, + // and in the case of messages, it is its structure. + type Metadata: 'msg + Copy; + // How many bytes does each element take up in the underlying sequence? + fn size_in_memory(metadata: Self::Metadata) -> usize; + // This function is unsafe because the bytes must correspond to the proxied object. + unsafe fn new(bytes: &'msg mut [u8], metadata: Self::Metadata) -> Self; +} + +// ========================= Abstracting over proxy vs direct sequences ========================= + +// This trait abstracts over &mut Sequence vs ProxySequence. +#[doc(hidden)] +pub trait InnerSequence: PartialEq { + fn as_slice(&self) -> &[T]; + fn as_mut_slice(&mut self) -> &mut [T]; + // "Unchecked" means that it doesn't know about the upper bound of the sequence. + fn resize_unchecked(&mut self, resize_function: ResizeFunction, len: usize); +} + +#[doc(hidden)] +pub struct ProxySequence<'msg, T: ProxyMut<'msg>> { + // The underlying storage + sequence: &'msg mut TypeErasedSequence, + // The user-facing objects + proxies: Vec, + // To recreate the proxies + metadata: T::Metadata, +} + +impl<'msg, T> InnerSequence for ProxySequence<'msg, T> +where + T: PartialEq + ProxyMut<'msg>, +{ + fn as_slice(&self) -> &[T] { + self.proxies.as_slice() + } + + fn as_mut_slice(&mut self) -> &mut [T] { + self.proxies.as_mut_slice() + } + + /// This will fini all messages in the sequence and re-initialize it from scratch. + fn resize_unchecked(&mut self, resize_function: ResizeFunction, len: usize) { + let is_ok = + unsafe { resize_function(self.sequence as *mut _ as *mut std::os::raw::c_void, len) }; + assert!(is_ok); + + // Recalculate the message proxies + self.proxies = unsafe { self.sequence.proxy_elems_mut(self.metadata) }; + } +} + +impl<'msg, T> InnerSequence for &'msg mut Sequence +where + T: PartialEq + SequenceAlloc, +{ + fn as_slice(&self) -> &[T] { + // self.as_slice() would call this trait method itself + Sequence::as_slice(self) + } + + fn as_mut_slice(&mut self) -> &mut [T] { + // self.as_mut_slice() would call this trait method itself + Sequence::as_mut_slice(self) + } + + /// This will fini all messages in the sequence and re-initialize it from scratch. + fn resize_unchecked(&mut self, resize_function: ResizeFunction, len: usize) { + let is_ok = unsafe { resize_function(self as *mut _ as *mut std::os::raw::c_void, len) }; + assert!(is_ok); + } +} + +impl<'msg, T> PartialEq for ProxySequence<'msg, T> +where + T: PartialEq + ProxyMut<'msg>, +{ + fn eq(&self, other: &Self) -> bool { + self.proxies.eq(&other.proxies) + } +} + +// This links the element type T to the inner sequence type: &mut Sequence or ProxySequence. +#[doc(hidden)] +pub trait DynamicSequenceElementMut<'msg>: Debug + PartialEq + Sized { + type InnerSequence: InnerSequence; +} + +// If the element type is an rosidl_runtime_rs type, the sequence type is &mut Sequence +impl<'msg, T> DynamicSequenceElementMut<'msg> for T +where + T: Debug + PartialEq + SequenceAlloc + 'static, +{ + type InnerSequence = &'msg mut Sequence; +} + +// ========================= The TypeErasedSequence helper ========================= + +/// A Sequence whose type is not statically known. +/// +/// This is an internal helper struct whose layout, like rosidl_runtime_rs::Sequence, +/// matches that of the type generated by rosidl_generator_c. +#[repr(C)] +pub(crate) struct TypeErasedSequence { + pub(super) data: *mut std::os::raw::c_void, + pub(super) size: usize, + pub(super) capacity: usize, +} + +impl TypeErasedSequence { + pub(super) unsafe fn proxy_elems<'msg, T>(&self, metadata: T::Metadata) -> Vec + where + T: Proxy<'msg>, + { + let element_size = T::size_in_memory(metadata); + let sequence_data = + std::slice::from_raw_parts(self.data as *const u8, self.size * element_size); + check::(sequence_data); + sequence_data + .chunks(element_size) + .map(|bytes| T::new(bytes, metadata)) + .collect() + } + + pub(super) unsafe fn proxy_elems_mut<'msg, T>(&self, metadata: T::Metadata) -> Vec + where + T: ProxyMut<'msg>, + { + let element_size = T::size_in_memory(metadata); + let sequence_data = + std::slice::from_raw_parts_mut(self.data as *mut u8, self.size * element_size); + check::(sequence_data); + sequence_data + .chunks_mut(element_size) + .map(|bytes| T::new(bytes, metadata)) + .collect() + } +} + +// ========================================================================== +// ======================== Immutable sequence types ======================== +// ========================================================================== + +/// An unbounded sequence. +/// +/// This type dereferences to `&[T]`. +#[derive(PartialEq, Eq)] +pub struct DynamicSequence<'msg, T> +where + T: Proxy<'msg>, +{ + elements: Box<[T]>, + // Not sure if this is strictly needed, but it's nice to be consistent + phantom: PhantomData<&'msg u8>, +} + +// BorrowedOrOwnedSlice – a specialized version of Cow. +// Cow cannot be used because it requires T to be Clone. +#[derive(PartialEq, Eq)] +enum BooSlice<'msg, T> { + Borrowed(&'msg [T]), + Owned(Box<[T]>), +} + +/// A bounded sequence whose upper bound is only known at runtime. +#[derive(PartialEq, Eq)] +pub struct DynamicBoundedSequence<'msg, T> { + boo: BooSlice<'msg, T>, + upper_bound: usize, +} + +// ------------------------- impl for DynamicSequence ------------------------- + +impl<'msg, T> Debug for DynamicSequence<'msg, T> +where + T: Debug + Proxy<'msg>, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.elements.iter().fmt(f) + } +} + +impl<'msg, T> Deref for DynamicSequence<'msg, T> +where + T: Proxy<'msg>, +{ + type Target = [T]; + fn deref(&self) -> &Self::Target { + &*self.elements + } +} + +impl<'msg, T> DynamicSequence<'msg, T> +where + T: Proxy<'msg>, +{ + pub(super) unsafe fn new_proxy(bytes: &'msg [u8], metadata: T::Metadata) -> Self { + let sequence = &*(bytes.as_ptr() as *const TypeErasedSequence); + let elements = sequence.proxy_elems(metadata).into_boxed_slice(); + Self { + elements, + phantom: PhantomData, + } + } + + /// See [`Sequence::as_slice()`][1]. + /// + /// [1]: rosidl_runtime_rs::Sequence::as_slice + pub fn as_slice(&self) -> &[T] { + &*self.elements + } +} + +// ------------------------- impl for DynamicBoundedSequence ------------------------- + +impl<'msg, T> BooSlice<'msg, T> { + fn as_slice(&self) -> &[T] { + match self { + BooSlice::Borrowed(slice) => slice, + BooSlice::Owned(boxed_slice) => &**boxed_slice, + } + } +} + +impl<'msg, T> Debug for DynamicBoundedSequence<'msg, T> +where + T: Debug, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.boo.as_slice().fmt(f) + } +} + +impl<'msg, T> Deref for DynamicBoundedSequence<'msg, T> { + type Target = [T]; + fn deref(&self) -> &Self::Target { + self.boo.as_slice() + } +} + +impl<'msg, T> DynamicBoundedSequence<'msg, T> +where + T: SequenceAlloc, +{ + pub(super) unsafe fn new_primitive(bytes: &'msg [u8], upper_bound: usize) -> Self { + let sequence = &*(bytes.as_ptr() as *const Sequence); + let slice = sequence.as_slice(); + Self { + boo: BooSlice::Borrowed(slice), + upper_bound, + } + } +} + +impl<'msg, T> DynamicBoundedSequence<'msg, T> +where + T: Proxy<'msg>, +{ + pub(super) unsafe fn new_proxy( + bytes: &'msg [u8], + upper_bound: usize, + metadata: T::Metadata, + ) -> Self { + let sequence = &*(bytes.as_ptr() as *const TypeErasedSequence); + Self { + boo: BooSlice::Owned(sequence.proxy_elems(metadata).into_boxed_slice()), + upper_bound, + } + } +} + +impl<'msg, T: SequenceAlloc> DynamicBoundedSequence<'msg, T> { + /// See [`Sequence::as_slice()`][1]. + /// + /// [1]: rosidl_runtime_rs::Sequence::as_slice + pub fn as_slice(&self) -> &[T] { + self.boo.as_slice() + } + + /// Returns the maximum length of this sequence. + pub fn upper_bound(&self) -> usize { + self.upper_bound + } +} + +// ========================================================================== +// ========================= Mutable sequence types ========================= +// ========================================================================== + +// The resize function from the type support library does not preserve the elements. +// It just calls fini + init. +pub(super) type ResizeFunction = + unsafe extern "C" fn(arg1: *mut std::os::raw::c_void, size: usize) -> bool; + +/// An unbounded sequence. +/// +/// This type dereferences to `&[T]` and `&mut [T]`. +#[derive(PartialEq)] +pub struct DynamicSequenceMut<'msg, T: DynamicSequenceElementMut<'msg>> { + // This is either &mut Sequence or ProxySequence + sequence: T::InnerSequence, + resize_function: ResizeFunction, +} + +/// A bounded sequence whose upper bound is only known at runtime. +/// +/// This is conceptually the same as a [`BoundedSequence`][1]. +/// +/// This type dereferences to `&[T]` and `&mut [T]`. +/// +/// [1]: rosidl_runtime_rs::BoundedSequence +#[derive(PartialEq)] +pub struct DynamicBoundedSequenceMut<'msg, T: DynamicSequenceElementMut<'msg>> { + inner: DynamicSequenceMut<'msg, T>, + upper_bound: usize, +} + +// ------------------------- impl for DynamicSequenceMut ------------------------- + +impl<'msg, T> Debug for DynamicSequenceMut<'msg, T> +where + T: DynamicSequenceElementMut<'msg>, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.sequence.as_slice().fmt(f) + } +} + +impl<'msg, T> Deref for DynamicSequenceMut<'msg, T> +where + T: DynamicSequenceElementMut<'msg>, +{ + type Target = [T]; + fn deref(&self) -> &Self::Target { + self.sequence.as_slice() + } +} + +impl<'msg, T> DerefMut for DynamicSequenceMut<'msg, T> +where + T: DynamicSequenceElementMut<'msg>, +{ + fn deref_mut(&mut self) -> &mut Self::Target { + self.sequence.as_mut_slice() + } +} + +impl<'msg, T> DynamicSequenceMut<'msg, T> +where + T: SequenceAlloc + + DynamicSequenceElementMut<'msg, InnerSequence = &'msg mut Sequence> + + 'static, +{ + pub(super) unsafe fn new_primitive( + bytes: &'msg mut [u8], + resize_function: ResizeFunction, + ) -> Self { + let sequence = &mut *(bytes.as_mut_ptr() as *mut Sequence); + Self { + sequence, + resize_function, + } + } +} + +impl<'msg, T> DynamicSequenceMut<'msg, T> +where + T: ProxyMut<'msg> + DynamicSequenceElementMut<'msg, InnerSequence = ProxySequence<'msg, T>>, +{ + pub(super) unsafe fn new_proxy( + bytes: &'msg mut [u8], + metadata: T::Metadata, + resize_function: ResizeFunction, + ) -> Self { + // SAFETY: TypeErasedSequence has the same layout as any + // rosidl-generated C sequence type, and the lifetime is correct too. + let sequence = &mut *(bytes.as_mut_ptr() as *mut TypeErasedSequence); + let proxies = sequence.proxy_elems_mut(metadata); + let sequence = ProxySequence { + sequence, + proxies, + metadata, + }; + Self { + sequence, + resize_function, + } + } +} + +impl<'msg, T> DynamicSequenceMut<'msg, T> +where + T: DynamicSequenceElementMut<'msg>, +{ + /// See [`Sequence::as_slice()`][1]. + /// + /// [1]: rosidl_runtime_rs::Sequence::as_slice + pub fn as_slice(&self) -> &[T] { + self.sequence.as_slice() + } + + /// See [`Sequence::as_mut_slice()`][1]. + /// + /// [1]: rosidl_runtime_rs::Sequence::as_mut_slice + pub fn as_mut_slice(&mut self) -> &mut [T] { + self.sequence.as_mut_slice() + } + + /// Resets this sequence to an empty sequence. + pub fn clear(&mut self) { + self.reset(0); + } + + /// Resets this sequence to a new sequence of `len` elements with default values. + pub fn reset(&mut self, len: usize) { + self.sequence.resize_unchecked(self.resize_function, len) + } +} + +// ------------------------- impl for DynamicBoundedSequenceMut ------------------------- + +impl<'msg, T> Debug for DynamicBoundedSequenceMut<'msg, T> +where + T: DynamicSequenceElementMut<'msg>, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + self.inner.fmt(f) + } +} + +impl<'msg, T: DynamicSequenceElementMut<'msg>> Deref for DynamicBoundedSequenceMut<'msg, T> { + type Target = [T]; + fn deref(&self) -> &Self::Target { + self.inner.deref() + } +} + +impl<'msg, T: DynamicSequenceElementMut<'msg>> DerefMut for DynamicBoundedSequenceMut<'msg, T> { + fn deref_mut(&mut self) -> &mut Self::Target { + self.inner.deref_mut() + } +} + +impl<'msg, T> DynamicBoundedSequenceMut<'msg, T> +where + T: SequenceAlloc + + DynamicSequenceElementMut<'msg, InnerSequence = &'msg mut Sequence> + + 'static, +{ + pub(super) unsafe fn new_primitive( + bytes: &'msg mut [u8], + upper_bound: usize, + resize_function: ResizeFunction, + ) -> Self { + let inner = DynamicSequenceMut::new_primitive(bytes, resize_function); + Self { inner, upper_bound } + } +} + +impl<'msg, T> DynamicBoundedSequenceMut<'msg, T> +where + T: ProxyMut<'msg> + DynamicSequenceElementMut<'msg, InnerSequence = ProxySequence<'msg, T>>, +{ + pub(super) unsafe fn new_proxy( + bytes: &'msg mut [u8], + metadata: T::Metadata, + upper_bound: usize, + resize_function: ResizeFunction, + ) -> Self { + let inner = DynamicSequenceMut::new_proxy(bytes, metadata, resize_function); + Self { inner, upper_bound } + } +} + +impl<'msg, T: DynamicSequenceElementMut<'msg>> DynamicBoundedSequenceMut<'msg, T> { + /// See [`Sequence::as_slice()`][1]. + /// + /// [1]: rosidl_runtime_rs::Sequence::as_slice + pub fn as_slice(&self) -> &[T] { + self.inner.as_slice() + } + + /// See [`Sequence::as_mut_slice()`][1]. + /// + /// [1]: rosidl_runtime_rs::Sequence::as_mut_slice + pub fn as_mut_slice(&mut self) -> &mut [T] { + self.inner.as_mut_slice() + } + + /// Returns the maximum length of this sequence. + pub fn upper_bound(&self) -> usize { + self.upper_bound + } + + /// Resets this sequence to an empty sequence. + pub fn clear(&mut self) { + self.inner.clear(); + } + + /// Tries to reset this sequence to a new sequence of `len` elements with default values. + /// + /// This is only successful if `len` is less than or equal to the [upper bound][1], otherwise + /// the sequence is unmodified. + /// + /// [1]: Self::upper_bound + pub fn try_reset(&mut self, len: usize) -> Result<(), SequenceExceedsBoundsError> { + if len > self.upper_bound { + Err(SequenceExceedsBoundsError { + len, + upper_bound: self.upper_bound, + }) + } else { + self.inner.reset(len); + Ok(()) + } + } +} diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs new file mode 100644 index 000000000..c5a67a12a --- /dev/null +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -0,0 +1,1144 @@ +use std::ffi::CStr; +use std::num::NonZeroUsize; + +use super::TypeErasedSequence; +use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMember as rosidl_message_member_t; +use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; +use crate::rcl_bindings::*; + +/// Possible base types for fields in a message. +// The field variants are self-explaining, no need to add redundant documentation. +#[allow(missing_docs)] +#[derive(Clone, Debug, PartialEq, Eq)] +pub enum BaseType { + /// AKA `float32` in ROS .msg files. + Float, + /// AKA `float64` in ROS .msg files. + Double, + LongDouble, + Char, + WChar, + Boolean, + /// AKA `byte` in ROS .msg files. + Octet, + /// AKA `char` in ROS .msg files + Uint8, + Int8, + Uint16, + Int16, + Uint32, + Int32, + Uint64, + Int64, + String, + BoundedString { + upper_bound: NonZeroUsize, + }, + WString, + BoundedWString { + upper_bound: NonZeroUsize, + }, + Message(Box), +} + +/// A description of a single field in a [`DynamicMessage`][1]. +/// +/// The concrete type of a field is the combination of its [`BaseType`] with its [`ValueKind`]. +/// That is, the base types exist as single values, arrays, bounded sequences and unbounded sequences. +/// +/// [1]: crate::dynamic_message::DynamicMessage +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct MessageFieldInfo { + /// The field name. + pub name: String, + /// The base type – number, string, etc. + pub base_type: BaseType, + /// Whether the field is a simple value, an array, or a (bounded) sequence. + pub value_kind: ValueKind, + pub(crate) string_upper_bound: usize, + pub(crate) resize_function: + Option bool>, + pub(crate) offset: usize, +} + +/// A description of the structure of a message. +/// +/// Namely, the list of fields and their types. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct MessageStructure { + /// The set of fields in the message, ordered by their offset in the message. + /// + /// A `Vec` is easier to handle and faster than a `HashMap` for typical numbers of message fields. + /// If you need a `HashMap`, simply create your own from this `Vec`. + pub fields: Vec, + /// The size of this structure in bytes. + pub size: usize, + /// The namespace of this type. This is something like `geometry_msgs__msg`. + pub namespace: String, + /// The name of this type. This does not contain the package name. + pub type_name: String, +} + +/// Information on whether a field is a single value or a list of some kind. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum ValueKind { + /// This field is a single value, which includes string values. + Simple, + /// This field is an array of values. + Array { + /// The array length. + length: usize, + }, + /// This field is a [`Sequence`][1] of values. + /// + /// [1]: rosidl_runtime_rs::Sequence + Sequence, + /// This field is a [`BoundedSequence`][1] of values. + /// + /// [1]: rosidl_runtime_rs::BoundedSequence + BoundedSequence { + /// The maximum sequence length. + upper_bound: usize, + }, +} + +// ========================= impl for BaseType ========================= + +impl BaseType { + // The inner message type support will be nullptr except for the case of a nested message. + // That function must be unsafe, since it is possible to safely create a garbage non-null + // pointer. + unsafe fn new( + type_id: u8, + string_upper_bound: Option, + inner: *const rosidl_message_type_support_t, + ) -> Self { + use rosidl_typesupport_introspection_c_field_types::*; + match u32::from(type_id) { + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT as u32 => Self::Float, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_DOUBLE as u32 => Self::Double, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_LONG_DOUBLE as u32 => { + Self::LongDouble + } + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_CHAR as u32 => Self::Char, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_WCHAR as u32 => Self::WChar, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_BOOLEAN as u32 => Self::Boolean, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_OCTET as u32 => Self::Octet, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_UINT8 as u32 => Self::Uint8, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_INT8 as u32 => Self::Int8, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_UINT16 as u32 => Self::Uint16, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_INT16 as u32 => Self::Int16, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_UINT32 as u32 => Self::Uint32, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_INT32 as u32 => Self::Int32, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_UINT64 as u32 => Self::Uint64, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_INT64 as u32 => Self::Int64, + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u32 => { + match string_upper_bound { + None => Self::String, + Some(upper_bound) => Self::BoundedString { upper_bound }, + } + } + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_WSTRING as u32 => { + match string_upper_bound { + None => Self::WString, + Some(upper_bound) => Self::BoundedWString { upper_bound }, + } + } + x if x == rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE as u32 => { + assert!(!inner.is_null()); + let type_support: &rosidl_message_type_support_t = &*inner; + let message_members: &rosidl_message_members_t = + // SAFETY: The data pointer is supposed to be always valid. + &*(type_support.data as *const rosidl_message_members_t); + let structure = MessageStructure::from_rosidl_message_members(message_members); + Self::Message(Box::new(structure)) + } + _ => panic!("Invalid field type"), + } + } + + /// Returns the size of a single element of this base type. + /// + /// None is returned for LongDouble, which is of platform-dependent size. + pub(crate) fn size(&self) -> Option { + match self { + BaseType::Float => Some(4), + BaseType::Double => Some(8), + BaseType::LongDouble => None, + BaseType::Char => Some(1), + BaseType::WChar => Some(2), + BaseType::Boolean => Some(1), + BaseType::Octet => Some(1), + BaseType::Uint8 => Some(1), + BaseType::Int8 => Some(1), + BaseType::Uint16 => Some(2), + BaseType::Int16 => Some(2), + BaseType::Uint32 => Some(4), + BaseType::Int32 => Some(4), + BaseType::Uint64 => Some(8), + BaseType::Int64 => Some(8), + BaseType::String => Some(std::mem::size_of::()), + BaseType::BoundedString { .. } => { + Some(std::mem::size_of::()) + } + BaseType::WString => Some(std::mem::size_of::()), + BaseType::BoundedWString { .. } => { + Some(std::mem::size_of::()) + } + BaseType::Message(structure) => Some(structure.size), + } + } +} + +// ========================= impl for MessageFieldInfo ========================= + +impl MessageFieldInfo { + // That function must be unsafe, since it is possible to safely create a garbage non-null + // pointer and store it in a rosidl_message_member_t. + unsafe fn from(rosidl_message_member: &rosidl_message_member_t) -> Self { + debug_assert!(!rosidl_message_member.name_.is_null()); + let name = /*unsafe*/ { CStr::from_ptr(rosidl_message_member.name_) } + .to_string_lossy() + .into_owned(); + let value_kind = match ( + rosidl_message_member.is_array_, + rosidl_message_member.resize_function.is_some(), + rosidl_message_member.is_upper_bound_, + ) { + (false, _, _) => ValueKind::Simple, + (true, false, _) => ValueKind::Array { + length: rosidl_message_member.array_size_, + }, + (true, true, false) => { + assert_eq!(rosidl_message_member.array_size_, 0); + ValueKind::Sequence + } + (true, true, true) => ValueKind::BoundedSequence { + upper_bound: rosidl_message_member.array_size_, + }, + }; + Self { + name, + base_type: BaseType::new( + rosidl_message_member.type_id_, + NonZeroUsize::new(rosidl_message_member.string_upper_bound_), + rosidl_message_member.members_, + ), + value_kind, + string_upper_bound: rosidl_message_member.string_upper_bound_, + resize_function: rosidl_message_member.resize_function, + offset: usize::try_from(rosidl_message_member.offset_).unwrap(), + } + } +} + +impl MessageFieldInfo { + /// Returns the size of the field in the message. + /// + /// For sequences, it's the size of the sequence struct (ptr + size + capacity), + /// not the size that the elements take up in memory. + pub(crate) fn size(&self) -> Option { + match self.value_kind { + ValueKind::Simple => self.base_type.size(), + ValueKind::Array { length } => self.base_type.size().map(|size| length * size), + ValueKind::Sequence | ValueKind::BoundedSequence { .. } => { + Some(std::mem::size_of::()) + } + } + } +} + +// ========================= impl for MessageStructure ========================= + +impl MessageStructure { + /// Parses the C struct containing a list of fields. + // That function must be unsafe, since it is possible to safely create a garbage non-null + // pointer and store it in a rosidl_message_members_t. + pub(crate) unsafe fn from_rosidl_message_members( + message_members: &rosidl_message_members_t, + ) -> Self { + debug_assert!(!message_members.members_.is_null()); + let num_fields: usize = usize::try_from(message_members.member_count_).unwrap(); + let mut fields: Vec<_> = (0..num_fields) + .map(|i| { + // SAFETY: This is an array as per the documentation + let rosidl_message_member: &rosidl_message_member_t = + /*unsafe*/ { &*message_members.members_.add(i) }; + // SAFETY: This is a valid string pointer + MessageFieldInfo::from(rosidl_message_member) + }) + .collect(); + fields.sort_by_key(|field_info| field_info.offset); + // SAFETY: Immediate conversion into owned string. + let namespace = /*unsafe*/ { + CStr::from_ptr(message_members.message_namespace_) + .to_string_lossy() + .into_owned() + }; + // SAFETY: Immediate conversion into owned string. + let type_name = /*unsafe*/ { + CStr::from_ptr(message_members.message_name_) + .to_string_lossy() + .into_owned() + }; + Self { + fields, + size: message_members.size_of_, + namespace, + type_name, + } + } + + /// Gets the field info corresponding to the specified field name, if any. + pub fn get_field_info(&self, field_name: &str) -> Option<&MessageFieldInfo> { + self.fields.iter().find(|field| field.name == field_name) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::dynamic_message::*; + use test_msgs::msg; + + use std::num::NonZeroUsize; + + #[test] + fn max_alignment_is_8() { + // The DynamicMessage type makes sure that its storage is aligned to 8 + let alignments = [ + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + ]; + assert_eq!(alignments.into_iter().max().unwrap(), 8); + } + + #[test] + fn message_structure_is_accurate() { + let arrays_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); + let arrays_structure = Box::new(arrays_metadata.structure().clone()); + let builtins_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Builtins".try_into().unwrap()).unwrap(); + let builtins_structure = Box::new(builtins_metadata.structure().clone()); + let duration_metadata = + DynamicMessageMetadata::new("builtin_interfaces/msg/Duration".try_into().unwrap()) + .unwrap(); + let duration_structure = Box::new(duration_metadata.structure().clone()); + let empty_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Empty".try_into().unwrap()).unwrap(); + let empty_structure = Box::new(empty_metadata.structure().clone()); + let time_metadata = + DynamicMessageMetadata::new("builtin_interfaces/msg/Time".try_into().unwrap()).unwrap(); + let time_structure = Box::new(time_metadata.structure().clone()); + let basic_types_metadata = + DynamicMessageMetadata::new("test_msgs/msg/BasicTypes".try_into().unwrap()).unwrap(); + let basic_types_structure = Box::new(basic_types_metadata.structure().clone()); + let bounded_sequences_metadata = + DynamicMessageMetadata::new("test_msgs/msg/BoundedSequences".try_into().unwrap()) + .unwrap(); + let bounded_sequences_structure = Box::new(bounded_sequences_metadata.structure().clone()); + let constants_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Constants".try_into().unwrap()).unwrap(); + let constants_structure = Box::new(constants_metadata.structure().clone()); + let multi_nested_metadata = + DynamicMessageMetadata::new("test_msgs/msg/MultiNested".try_into().unwrap()).unwrap(); + let multi_nested_structure = Box::new(multi_nested_metadata.structure().clone()); + let nested_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Nested".try_into().unwrap()).unwrap(); + let nested_structure = Box::new(nested_metadata.structure().clone()); + let defaults_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Defaults".try_into().unwrap()).unwrap(); + let defaults_structure = Box::new(defaults_metadata.structure().clone()); + let strings_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Strings".try_into().unwrap()).unwrap(); + let strings_structure = Box::new(strings_metadata.structure().clone()); + let wstrings_metadata = + DynamicMessageMetadata::new("test_msgs/msg/WStrings".try_into().unwrap()).unwrap(); + let wstrings_structure = Box::new(wstrings_metadata.structure().clone()); + let unbounded_sequences_metadata = + DynamicMessageMetadata::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()) + .unwrap(); + let unbounded_sequences_structure = + Box::new(unbounded_sequences_metadata.structure().clone()); + + let mut message_structures_and_fields = vec![]; + + // --------------------- Arrays --------------------- + + let arrays_fields = vec![ + ( + "bool_values", + BaseType::Boolean, + ValueKind::Array { length: 3 }, + ), + ( + "byte_values", + BaseType::Octet, + ValueKind::Array { length: 3 }, + ), + ( + "char_values", + BaseType::Uint8, // the msg to idl conversion converts char to uint8 + ValueKind::Array { length: 3 }, + ), + ( + "float32_values", + BaseType::Float, + ValueKind::Array { length: 3 }, + ), + ( + "float64_values", + BaseType::Double, + ValueKind::Array { length: 3 }, + ), + ( + "int8_values", + BaseType::Int8, + ValueKind::Array { length: 3 }, + ), + ( + "uint8_values", + BaseType::Uint8, + ValueKind::Array { length: 3 }, + ), + ( + "int16_values", + BaseType::Int16, + ValueKind::Array { length: 3 }, + ), + ( + "uint16_values", + BaseType::Uint16, + ValueKind::Array { length: 3 }, + ), + ( + "int32_values", + BaseType::Int32, + ValueKind::Array { length: 3 }, + ), + ( + "uint32_values", + BaseType::Uint32, + ValueKind::Array { length: 3 }, + ), + ( + "int64_values", + BaseType::Int64, + ValueKind::Array { length: 3 }, + ), + ( + "uint64_values", + BaseType::Uint64, + ValueKind::Array { length: 3 }, + ), + ( + "string_values", + BaseType::String, + ValueKind::Array { length: 3 }, + ), + ( + "basic_types_values", + BaseType::Message(basic_types_structure.clone()), + ValueKind::Array { length: 3 }, + ), + ( + "constants_values", + BaseType::Message(constants_structure.clone()), + ValueKind::Array { length: 3 }, + ), + ( + "defaults_values", + BaseType::Message(defaults_structure.clone()), + ValueKind::Array { length: 3 }, + ), + ( + "bool_values_default", + BaseType::Boolean, + ValueKind::Array { length: 3 }, + ), + ( + "byte_values_default", + BaseType::Octet, + ValueKind::Array { length: 3 }, + ), + ( + "char_values_default", + BaseType::Uint8, // the msg to idl conversion converts char to uint8 + ValueKind::Array { length: 3 }, + ), + ( + "float32_values_default", + BaseType::Float, + ValueKind::Array { length: 3 }, + ), + ( + "float64_values_default", + BaseType::Double, + ValueKind::Array { length: 3 }, + ), + ( + "int8_values_default", + BaseType::Int8, + ValueKind::Array { length: 3 }, + ), + ( + "uint8_values_default", + BaseType::Uint8, + ValueKind::Array { length: 3 }, + ), + ( + "int16_values_default", + BaseType::Int16, + ValueKind::Array { length: 3 }, + ), + ( + "uint16_values_default", + BaseType::Uint16, + ValueKind::Array { length: 3 }, + ), + ( + "int32_values_default", + BaseType::Int32, + ValueKind::Array { length: 3 }, + ), + ( + "uint32_values_default", + BaseType::Uint32, + ValueKind::Array { length: 3 }, + ), + ( + "int64_values_default", + BaseType::Int64, + ValueKind::Array { length: 3 }, + ), + ( + "uint64_values_default", + BaseType::Uint64, + ValueKind::Array { length: 3 }, + ), + ( + "string_values_default", + BaseType::String, + ValueKind::Array { length: 3 }, + ), + ("alignment_check", BaseType::Int32, ValueKind::Simple), + ]; + + message_structures_and_fields.push(("Arrays", arrays_structure.clone(), arrays_fields)); + + // --------------------- BasicTypes --------------------- + + let basic_types_fields = vec![ + ("bool_value", BaseType::Boolean, ValueKind::Simple), + ("byte_value", BaseType::Octet, ValueKind::Simple), + ("char_value", BaseType::Uint8, ValueKind::Simple), + ("float32_value", BaseType::Float, ValueKind::Simple), + ("float64_value", BaseType::Double, ValueKind::Simple), + ("int8_value", BaseType::Int8, ValueKind::Simple), + ("uint8_value", BaseType::Uint8, ValueKind::Simple), + ("int16_value", BaseType::Int16, ValueKind::Simple), + ("uint16_value", BaseType::Uint16, ValueKind::Simple), + ("int32_value", BaseType::Int32, ValueKind::Simple), + ("uint32_value", BaseType::Uint32, ValueKind::Simple), + ("int64_value", BaseType::Int64, ValueKind::Simple), + ("uint64_value", BaseType::Uint64, ValueKind::Simple), + ]; + + message_structures_and_fields.push(( + "BasicTypes", + basic_types_structure.clone(), + basic_types_fields, + )); + + // --------------------- BoundedSequences --------------------- + + let bounded_sequences_fields = vec![ + ( + "bool_values", + BaseType::Boolean, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "byte_values", + BaseType::Octet, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "char_values", + BaseType::Uint8, // the msg to idl conversion converts char to uint8 + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "float32_values", + BaseType::Float, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "float64_values", + BaseType::Double, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int8_values", + BaseType::Int8, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint8_values", + BaseType::Uint8, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int16_values", + BaseType::Int16, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint16_values", + BaseType::Uint16, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int32_values", + BaseType::Int32, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint32_values", + BaseType::Uint32, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int64_values", + BaseType::Int64, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint64_values", + BaseType::Uint64, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "string_values", + BaseType::String, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "basic_types_values", + BaseType::Message(basic_types_structure.clone()), + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "constants_values", + BaseType::Message(constants_structure.clone()), + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "defaults_values", + BaseType::Message(defaults_structure.clone()), + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "bool_values_default", + BaseType::Boolean, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "byte_values_default", + BaseType::Octet, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "char_values_default", + BaseType::Uint8, // the msg to idl conversion converts char to uint8 + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "float32_values_default", + BaseType::Float, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "float64_values_default", + BaseType::Double, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int8_values_default", + BaseType::Int8, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint8_values_default", + BaseType::Uint8, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int16_values_default", + BaseType::Int16, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint16_values_default", + BaseType::Uint16, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int32_values_default", + BaseType::Int32, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint32_values_default", + BaseType::Uint32, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "int64_values_default", + BaseType::Int64, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "uint64_values_default", + BaseType::Uint64, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "string_values_default", + BaseType::String, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ("alignment_check", BaseType::Int32, ValueKind::Simple), + ]; + + message_structures_and_fields.push(( + "BoundedSequences", + bounded_sequences_structure.clone(), + bounded_sequences_fields, + )); + + // --------------------- Builtins --------------------- + + let builtins_fields = vec![ + ( + "duration_value", + BaseType::Message(duration_structure.clone()), + ValueKind::Simple, + ), + ( + "time_value", + BaseType::Message(time_structure.clone()), + ValueKind::Simple, + ), + ]; + + message_structures_and_fields.push(( + "Builtins", + builtins_structure.clone(), + builtins_fields, + )); + + // --------------------- Constants --------------------- + + let constants_fields: Vec<(&'static str, BaseType, ValueKind)> = vec![( + "structure_needs_at_least_one_member", + BaseType::Uint8, + ValueKind::Simple, + )]; + + message_structures_and_fields.push(( + "Constants", + constants_structure.clone(), + constants_fields, + )); + + // --------------------- Defaults --------------------- + + let defaults_fields = vec![ + ("bool_value", BaseType::Boolean, ValueKind::Simple), + ("byte_value", BaseType::Octet, ValueKind::Simple), + // the msg to idl conversion converts char to uint8 + ("char_value", BaseType::Uint8, ValueKind::Simple), + ("float32_value", BaseType::Float, ValueKind::Simple), + ("float64_value", BaseType::Double, ValueKind::Simple), + ("int8_value", BaseType::Int8, ValueKind::Simple), + ("uint8_value", BaseType::Uint8, ValueKind::Simple), + ("int16_value", BaseType::Int16, ValueKind::Simple), + ("uint16_value", BaseType::Uint16, ValueKind::Simple), + ("int32_value", BaseType::Int32, ValueKind::Simple), + ("uint32_value", BaseType::Uint32, ValueKind::Simple), + ("int64_value", BaseType::Int64, ValueKind::Simple), + ("uint64_value", BaseType::Uint64, ValueKind::Simple), + ]; + + message_structures_and_fields.push(( + "Defaults", + defaults_structure.clone(), + defaults_fields, + )); + + // --------------------- Empty --------------------- + + let empty_fields: Vec<(&'static str, BaseType, ValueKind)> = vec![( + "structure_needs_at_least_one_member", + BaseType::Uint8, + ValueKind::Simple, + )]; + + message_structures_and_fields.push(("Empty", empty_structure.clone(), empty_fields)); + + // --------------------- MultiNested --------------------- + + let multi_nested_fields = vec![ + ( + "array_of_arrays", + BaseType::Message(arrays_structure.clone()), + ValueKind::Array { length: 3 }, + ), + ( + "array_of_bounded_sequences", + BaseType::Message(bounded_sequences_structure.clone()), + ValueKind::Array { length: 3 }, + ), + ( + "array_of_unbounded_sequences", + BaseType::Message(unbounded_sequences_structure.clone()), + ValueKind::Array { length: 3 }, + ), + ( + "bounded_sequence_of_arrays", + BaseType::Message(arrays_structure.clone()), + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "bounded_sequence_of_bounded_sequences", + BaseType::Message(bounded_sequences_structure.clone()), + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "bounded_sequence_of_unbounded_sequences", + BaseType::Message(unbounded_sequences_structure.clone()), + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "unbounded_sequence_of_arrays", + BaseType::Message(arrays_structure.clone()), + ValueKind::Sequence, + ), + ( + "unbounded_sequence_of_bounded_sequences", + BaseType::Message(bounded_sequences_structure.clone()), + ValueKind::Sequence, + ), + ( + "unbounded_sequence_of_unbounded_sequences", + BaseType::Message(unbounded_sequences_structure.clone()), + ValueKind::Sequence, + ), + ]; + + message_structures_and_fields.push(( + "MultiNested", + multi_nested_structure.clone(), + multi_nested_fields, + )); + + // --------------------- Nested --------------------- + + let nested_fields = vec![( + "basic_types_value", + BaseType::Message(basic_types_structure.clone()), + ValueKind::Simple, + )]; + + message_structures_and_fields.push(("Nested", nested_structure.clone(), nested_fields)); + + // --------------------- Strings --------------------- + + let strings_fields = vec![ + ("string_value", BaseType::String, ValueKind::Simple), + ("string_value_default1", BaseType::String, ValueKind::Simple), + ("string_value_default2", BaseType::String, ValueKind::Simple), + ("string_value_default3", BaseType::String, ValueKind::Simple), + ("string_value_default4", BaseType::String, ValueKind::Simple), + ("string_value_default5", BaseType::String, ValueKind::Simple), + ( + "bounded_string_value", + BaseType::BoundedString { + upper_bound: NonZeroUsize::new(22).unwrap(), + }, + ValueKind::Simple, + ), + ( + "bounded_string_value_default1", + BaseType::BoundedString { + upper_bound: NonZeroUsize::new(22).unwrap(), + }, + ValueKind::Simple, + ), + ( + "bounded_string_value_default2", + BaseType::BoundedString { + upper_bound: NonZeroUsize::new(22).unwrap(), + }, + ValueKind::Simple, + ), + ( + "bounded_string_value_default3", + BaseType::BoundedString { + upper_bound: NonZeroUsize::new(22).unwrap(), + }, + ValueKind::Simple, + ), + ( + "bounded_string_value_default4", + BaseType::BoundedString { + upper_bound: NonZeroUsize::new(22).unwrap(), + }, + ValueKind::Simple, + ), + ( + "bounded_string_value_default5", + BaseType::BoundedString { + upper_bound: NonZeroUsize::new(22).unwrap(), + }, + ValueKind::Simple, + ), + ]; + + message_structures_and_fields.push(("Strings", strings_structure.clone(), strings_fields)); + + // --------------------- UnboundedSequences --------------------- + + let unbounded_sequences_fields = vec![ + ("bool_values", BaseType::Boolean, ValueKind::Sequence), + ("byte_values", BaseType::Octet, ValueKind::Sequence), + // the msg to idl conversion converts char to uint8 + ("char_values", BaseType::Uint8, ValueKind::Sequence), + ("float32_values", BaseType::Float, ValueKind::Sequence), + ("float64_values", BaseType::Double, ValueKind::Sequence), + ("int8_values", BaseType::Int8, ValueKind::Sequence), + ("uint8_values", BaseType::Uint8, ValueKind::Sequence), + ("int16_values", BaseType::Int16, ValueKind::Sequence), + ("uint16_values", BaseType::Uint16, ValueKind::Sequence), + ("int32_values", BaseType::Int32, ValueKind::Sequence), + ("uint32_values", BaseType::Uint32, ValueKind::Sequence), + ("int64_values", BaseType::Int64, ValueKind::Sequence), + ("uint64_values", BaseType::Uint64, ValueKind::Sequence), + ("string_values", BaseType::String, ValueKind::Sequence), + ( + "basic_types_values", + BaseType::Message(basic_types_structure.clone()), + ValueKind::Sequence, + ), + ( + "constants_values", + BaseType::Message(constants_structure.clone()), + ValueKind::Sequence, + ), + ( + "defaults_values", + BaseType::Message(defaults_structure.clone()), + ValueKind::Sequence, + ), + ( + "bool_values_default", + BaseType::Boolean, + ValueKind::Sequence, + ), + ("byte_values_default", BaseType::Octet, ValueKind::Sequence), + // the msg to idl conversion converts char to uint8 + ("char_values_default", BaseType::Uint8, ValueKind::Sequence), + ( + "float32_values_default", + BaseType::Float, + ValueKind::Sequence, + ), + ( + "float64_values_default", + BaseType::Double, + ValueKind::Sequence, + ), + ("int8_values_default", BaseType::Int8, ValueKind::Sequence), + ("uint8_values_default", BaseType::Uint8, ValueKind::Sequence), + ("int16_values_default", BaseType::Int16, ValueKind::Sequence), + ( + "uint16_values_default", + BaseType::Uint16, + ValueKind::Sequence, + ), + ("int32_values_default", BaseType::Int32, ValueKind::Sequence), + ( + "uint32_values_default", + BaseType::Uint32, + ValueKind::Sequence, + ), + ("int64_values_default", BaseType::Int64, ValueKind::Sequence), + ( + "uint64_values_default", + BaseType::Uint64, + ValueKind::Sequence, + ), + ( + "string_values_default", + BaseType::String, + ValueKind::Sequence, + ), + ("alignment_check", BaseType::Int32, ValueKind::Simple), + ]; + + message_structures_and_fields.push(( + "UnboundedSequences", + unbounded_sequences_structure.clone(), + unbounded_sequences_fields, + )); + + // --------------------- WStrings --------------------- + + let wstrings_fields = vec![ + ("wstring_value", BaseType::WString, ValueKind::Simple), + ( + "wstring_value_default1", + BaseType::WString, + ValueKind::Simple, + ), + ( + "wstring_value_default2", + BaseType::WString, + ValueKind::Simple, + ), + ( + "wstring_value_default3", + BaseType::WString, + ValueKind::Simple, + ), + ( + "array_of_wstrings", + BaseType::WString, + ValueKind::Array { length: 3 }, + ), + ( + "bounded_sequence_of_wstrings", + BaseType::WString, + ValueKind::BoundedSequence { upper_bound: 3 }, + ), + ( + "unbounded_sequence_of_wstrings", + BaseType::WString, + ValueKind::Sequence, + ), + ]; + + message_structures_and_fields.push(( + "WStrings", + wstrings_structure.clone(), + wstrings_fields, + )); + + // --------------------- Running the tests --------------------- + + for (message_name, structure, fields) in message_structures_and_fields { + assert_eq!( + structure + .fields + .iter() + .map(|field_info| field_info.name.to_owned()) + .collect::>(), + fields + .iter() + .map(|pair| pair.0.to_owned()) + .collect::>(), + "in message '{}'", + message_name + ); + + for (field_name, expected_base_type, expected_value_kind) in fields { + let field_type = structure.get_field_info(field_name).unwrap(); + assert_eq!( + field_type.base_type, expected_base_type, + "for field '{}' in message '{}'", + field_name, message_name + ); + assert_eq!( + field_type.value_kind, expected_value_kind, + "for field '{}' in message '{}'", + field_name, message_name, + ); + } + } + } + + #[test] + fn dynamic_message_has_defaults() { + let dyn_msg = DynamicMessage::new("test_msgs/msg/Defaults".try_into().unwrap()).unwrap(); + assert_eq!( + dyn_msg.get("bool_value"), + Some(Value::Simple(SimpleValue::Boolean(&true))) + ); + assert_eq!( + dyn_msg.get("byte_value"), + Some(Value::Simple(SimpleValue::Octet(&50u8))) + ); + assert_eq!( + dyn_msg.get("char_value"), + Some(Value::Simple(SimpleValue::Uint8(&100u8))) + ); + assert_eq!( + dyn_msg.get("float32_value"), + Some(Value::Simple(SimpleValue::Float(&1.125f32))) + ); + assert_eq!( + dyn_msg.get("float64_value"), + Some(Value::Simple(SimpleValue::Double(&1.125f64))) + ); + assert_eq!( + dyn_msg.get("int8_value"), + Some(Value::Simple(SimpleValue::Int8(&-50i8))) + ); + assert_eq!( + dyn_msg.get("uint8_value"), + Some(Value::Simple(SimpleValue::Uint8(&200u8))) + ); + assert_eq!( + dyn_msg.get("int16_value"), + Some(Value::Simple(SimpleValue::Int16(&-1000i16))) + ); + assert_eq!( + dyn_msg.get("uint16_value"), + Some(Value::Simple(SimpleValue::Uint16(&2000u16))) + ); + assert_eq!( + dyn_msg.get("int32_value"), + Some(Value::Simple(SimpleValue::Int32(&-30000i32))) + ); + assert_eq!( + dyn_msg.get("uint32_value"), + Some(Value::Simple(SimpleValue::Uint32(&60000u32))) + ); + assert_eq!( + dyn_msg.get("int64_value"), + Some(Value::Simple(SimpleValue::Int64(&-40000000i64))) + ); + assert_eq!( + dyn_msg.get("uint64_value"), + Some(Value::Simple(SimpleValue::Uint64(&50000000u64))) + ); + + let _dyn_msg = DynamicMessage::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); + let _dyn_msg = + DynamicMessage::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()).unwrap(); + let _dyn_msg = + DynamicMessage::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); + } +} diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index b177aaafa..146bbc70d 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -4,6 +4,8 @@ use std::{ fmt::{self, Display}, }; +#[cfg(feature = "dyn_msg")] +use crate::dynamic_message::DynamicMessageError; use crate::{rcl_bindings::*, DeclarationError}; /// The main error type. @@ -32,6 +34,12 @@ pub enum RclrsError { }, /// It was attempted to add a waitable to a wait set twice. AlreadyAddedToWaitSet, + #[cfg(feature = "dyn_msg")] + /// An error while creating dynamic message. + DynamicMessageError { + /// The error containing more detailed information. + err: DynamicMessageError, + }, /// A negative duration was obtained from rcl which should have been positive. /// /// The value represents nanoseconds. @@ -94,6 +102,10 @@ impl Display for RclrsError { "Could not add entity to wait set because it was already added to a wait set" ) } + #[cfg(feature = "dyn_msg")] + RclrsError::DynamicMessageError { .. } => { + write!(f, "Could not create dynamic message") + } RclrsError::NegativeDuration(duration) => { write!( f, @@ -122,6 +134,13 @@ impl Display for RclrsError { } } +#[cfg(feature = "dyn_msg")] +impl From for RclrsError { + fn from(err: DynamicMessageError) -> Self { + Self::DynamicMessageError { err } + } +} + /// Struct encapsulating an error message from the rcl layer or below. /// /// This struct is intended to be returned by the `source` method in the implementation of the @@ -152,6 +171,8 @@ impl Error for RclrsError { // TODO(@mxgrey): We should provide source information for these other types. // It should be easy to do this using the thiserror crate. RclrsError::AlreadyAddedToWaitSet => None, + #[cfg(feature = "dyn_msg")] + RclrsError::DynamicMessageError { err } => Some(err).map(|e| e as &dyn Error), RclrsError::NegativeDuration(_) => None, RclrsError::UnownedGuardCondition => None, RclrsError::InvalidPayload { .. } => None, diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 366e499b8..3b0dc1a7a 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -200,6 +200,8 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; +#[cfg(feature = "dyn_msg")] +pub use dynamic_message::*; pub use arguments::*; pub use client::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index dd01d0605..5babc3c0c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -5,6 +5,18 @@ mod primitive_options; pub use primitive_options::*; mod graph; +#[cfg(feature = "dyn_msg")] +use crate::{ + dynamic_message::{ + DynamicMessage, DynamicPublisher, DynamicPublisherState, DynamicSubscription, + DynamicSubscriptionState, MessageTypeName, NodeAsyncDynamicSubscriptionCallback, + NodeDynamicSubscriptionCallback, + }, + MessageInfo, +}; +#[cfg(feature = "dyn_msg")] +use futures::future::BoxFuture; + pub use graph::*; mod node_graph_task; @@ -394,6 +406,33 @@ impl NodeState { PublisherState::::create(options, Arc::clone(&self.handle)) } + /// Creates a [`DynamicPublisher`], a publisher whose type is only known at runtime. + /// + /// Refer to [`Node::create_publisher`][1] for the API, the only key difference is that the + /// publisher's message type is passed as a [`crate::MessageTypeName`] parameter. + /// + /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// + /// [1]: crate::NodeState::create_publisher + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let publisher = node.create_dynamic_publisher( + /// "test_msgs/msg/Empty".try_into().unwrap(), + /// "my_topic" + /// .keep_last(100) + /// ) + /// .unwrap(); + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_publisher<'a>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + ) -> Result { + DynamicPublisherState::create(topic_type, options, Arc::clone(&self.handle)) + } + /// Creates a [`Service`] with an ordinary callback. /// /// # Behavior @@ -785,6 +824,138 @@ impl NodeState { ) } + /// Creates a [`DynamicSubscription`] with an ordinary callback. + /// + /// For the behavior and API refer to [`Node::create_subscription`][1], except two key + /// differences: + /// + /// - The message type is determined at runtime through the `topic_type` function parameter. + /// - Only one type of callback is supported (returning both [`crate::DynamicMessage`] and + /// [`crate::MessageInfo`]). + /// + /// # Message type passing + /// + /// The message type can be passed as a [`crate::MessageTypeName`] struct. The struct also implements `TryFrom<&str>` + /// + /// [1]: crate::NodeState::create_publisher + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let subscription = node.create_dynamic_subscription( + /// MessageTypeName { + /// package_name: "test_msgs".to_owned(), + /// type_name: "Empty".to_owned(), + /// }, + /// "my_topic" + /// .transient_local(), + /// |_msg: DynamicMessage, _info: MessageInfo| { + /// println!("Received message!"); + /// }, + /// ); + /// + /// let subscription = node.create_dynamic_subscription( + /// "test_msgs/msg/Empty".try_into().unwrap(), + /// "my_topic", + /// |_msg: DynamicMessage, _info: MessageInfo| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_subscription<'a, F>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + callback: F, + ) -> Result + where + F: Fn(DynamicMessage, MessageInfo) + Send + Sync + 'static, + { + DynamicSubscriptionState::::create( + topic_type, + options, + NodeDynamicSubscriptionCallback::new(callback), + &self.handle, + self.commands.async_worker_commands(), + ) + } + + /// Creates a [`DynamicSubscription`] with an async callback. + /// + /// For the behavior and API refer to [`Node::create_async_subscription`][1], except two key + /// differences: + /// + /// - The message type is determined at runtime through the `topic_type` function parameter. + /// - Only one type of callback is supported (returning both [`crate::DynamicMessage`] and + /// [`crate::MessageInfo`]. + /// + /// # Message type passing + /// + /// The message type can be passed as a [`crate::MessageTypeName`] struct. The struct also implements `TryFrom<&str>` + /// + /// [1]: crate::NodeState::create_async_subscription + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::Arc; + /// + /// let count_worker = node.create_worker(0_usize); + /// let data_worker = node.create_worker(String::new()); + /// + /// let service = node.create_async_dynamic_subscription( + /// "example_interfaces/msg/String".try_into()?, + /// "topic", + /// move |msg: DynamicMessage, _info: MessageInfo| { + /// // Clone the workers so they can be captured into the async block + /// let count_worker = Arc::clone(&count_worker); + /// let data_worker = Arc::clone(&data_worker); + /// Box::pin(async move { + /// // Update the message count + /// let current_count = count_worker.run(move |count: &mut usize| { + /// *count += 1; + /// *count + /// }).await.unwrap(); + /// + /// // Change the data in the data_worker and get back the data + /// // that was previously put in there. + /// let previous = data_worker.run(move |data: &mut String| { + /// let value = msg.get("data").unwrap(); + /// let Value::Simple(value) = value else { + /// panic!("Unexpected value type, expected Simple value"); + /// }; + /// let SimpleValue::String(value) = value else { + /// panic!("Unexpected value type, expected String"); + /// }; + /// std::mem::replace(data, value.to_string()) + /// }).await.unwrap(); + /// + /// println!("Current count is {current_count}, data was previously {previous}"); + /// }) + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + #[cfg(feature = "dyn_msg")] + pub fn create_async_dynamic_subscription<'a, F>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + callback: F, + ) -> Result + where + F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + Sync + 'static, + { + DynamicSubscriptionState::::create( + topic_type, + options, + NodeAsyncDynamicSubscriptionCallback::new(callback), + &self.handle, + self.commands.async_worker_commands(), + ) + } + /// Creates a [`Subscription`] with an async callback. /// /// # Behavior diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 29a7e1579..8439420b8 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -26,9 +26,9 @@ unsafe impl Send for rcl_publisher_t {} /// [dropped after][1] the `rcl_publisher_t`. /// /// [1]: -struct PublisherHandle { - rcl_publisher: Mutex, - node_handle: Arc, +pub(crate) struct PublisherHandle { + pub(crate) rcl_publisher: Mutex, + pub(crate) node_handle: Arc, } impl Drop for PublisherHandle { @@ -43,6 +43,35 @@ impl Drop for PublisherHandle { } } +// Functions shared between normal and dynamic publishers +impl PublisherHandle { + pub(crate) fn topic_name(&self) -> String { + // SAFETY: No preconditions for the functions called. + // The unsafe variables created get converted to safe types before being returned + unsafe { + let raw_topic_pointer = + rcl_publisher_get_topic_name(&*self.rcl_publisher.lock().unwrap()); + CStr::from_ptr(raw_topic_pointer) + .to_string_lossy() + .into_owned() + } + } + + /// Returns the number of subscriptions of the publisher. + pub(crate) fn get_subscription_count(&self) -> Result { + let mut subscription_count = 0; + // SAFETY: No preconditions for the function called. + unsafe { + rcl_publisher_get_subscription_count( + &*self.rcl_publisher.lock().unwrap(), + &mut subscription_count, + ) + .ok()? + }; + Ok(subscription_count) + } +} + /// Struct for sending messages of type `T`. /// /// Create a publisher using [`Node::create_publisher`][1]. @@ -152,29 +181,12 @@ where /// This returns the topic name after remapping, so it is not necessarily the /// topic name which was used when creating the publisher. pub fn topic_name(&self) -> String { - // SAFETY: No preconditions for the functions called. - // The unsafe variables created get converted to safe types before being returned - unsafe { - let raw_topic_pointer = - rcl_publisher_get_topic_name(&*self.handle.rcl_publisher.lock().unwrap()); - CStr::from_ptr(raw_topic_pointer) - .to_string_lossy() - .into_owned() - } + self.handle.topic_name() } /// Returns the number of subscriptions of the publisher. pub fn get_subscription_count(&self) -> Result { - let mut subscription_count = 0; - // SAFETY: No preconditions for the function called. - unsafe { - rcl_publisher_get_subscription_count( - &*self.handle.rcl_publisher.lock().unwrap(), - &mut subscription_count, - ) - .ok()? - }; - Ok(subscription_count) + self.handle.get_subscription_count() } /// Publishes a message. diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index e57c542f6..124254766 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -100,14 +100,7 @@ where /// This returns the topic name after remapping, so it is not necessarily the /// topic name which was used when creating the subscription. pub fn topic_name(&self) -> String { - // SAFETY: The subscription handle is valid because its lifecycle is managed by an Arc. - // The unsafe variables get converted to safe types before being returned - unsafe { - let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); - CStr::from_ptr(raw_topic_pointer) - } - .to_string_lossy() - .into_owned() + self.handle.topic_name() } /// Used by [`Node`][crate::Node] to create a new subscription. @@ -286,16 +279,27 @@ unsafe impl Send for rcl_subscription_t {} /// [dropped after][1] the `rcl_subscription_t`. /// /// [1]: -struct SubscriptionHandle { - rcl_subscription: Mutex, - node_handle: Arc, +pub(crate) struct SubscriptionHandle { + pub(crate) rcl_subscription: Mutex, + pub(crate) node_handle: Arc, } impl SubscriptionHandle { - fn lock(&self) -> MutexGuard { + pub(crate) fn lock(&self) -> MutexGuard { self.rcl_subscription.lock().unwrap() } + pub(crate) fn topic_name(&self) -> String { + // SAFETY: The subscription handle is valid because its lifecycle is managed by an Arc. + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.lock()); + CStr::from_ptr(raw_topic_pointer) + } + .to_string_lossy() + .into_owned() + } + /// Fetches a new message. /// /// When there is no new message, this will return a diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index f89da20fc..1223175bc 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -1,3 +1,11 @@ +#[cfg(feature = "dyn_msg")] +use crate::{ + dynamic_message::{ + DynamicMessage, DynamicSubscriptionState, MessageTypeName, WorkerDynamicSubscription, + WorkerDynamicSubscriptionCallback, + }, + MessageInfo, +}; use crate::{ log_fatal, IntoWorkerServiceCallback, IntoWorkerSubscriptionCallback, Node, Promise, RclrsError, ServiceOptions, ServiceState, SubscriptionOptions, SubscriptionState, @@ -264,6 +272,64 @@ impl WorkerState { ) } + /// Creates a [`WorkerDynamicSubscription`], whose message type is only know at runtime. + /// + /// Refer to ['Worker::create_subscription`] for the API and behavior except two key + /// differences: + /// + /// - The message type is determined at runtime through the `topic_type` function parameter. + /// - Only one type of callback is supported (returning both [`crate::DynamicMessage`] and + /// [`crate::MessageInfo`]). + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// // The worker's payload is data that we want to share with other callbacks. + /// let worker = node.create_worker::>(None); + /// + /// // This variable will be the mutable internal state of the subscription + /// // callback. + /// let mut count = 0_usize; + /// + /// let subscription = worker.create_dynamic_subscription( + /// "example_interfaces/msg/String".try_into()?, + /// "topic", + /// move |data: &mut Option, msg: DynamicMessage, _msg_info: MessageInfo| { + /// count += 1; + /// let value = msg.get("data").unwrap(); + /// let Value::Simple(value) = value else { + /// panic!("Unexpected value type, expected Simple value"); + /// }; + /// let SimpleValue::String(value) = value else { + /// panic!("Unexpected value type, expected String"); + /// }; + /// println!("#{count} | I heard: '{}'", value); + /// + /// *data = Some(value.to_string()); + /// }, + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_subscription<'a, F>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + callback: F, + ) -> Result, RclrsError> + where + F: FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send + Sync + 'static, + { + DynamicSubscriptionState::>::create( + topic_type, + options, + WorkerDynamicSubscriptionCallback::new(callback), + self.node.handle(), + &self.commands, + ) + } + /// Creates a [`WorkerService`]. /// /// Unlike services created from a [`Node`], the callbacks for these services @@ -511,6 +577,8 @@ mod tests { struct TestPayload { subscription_count: usize, service_count: usize, + #[cfg(feature = "dyn_msg")] + dynamic_subscription_count: usize, } #[test] @@ -525,6 +593,15 @@ mod tests { }, ); + #[cfg(feature = "dyn_msg")] + let _count_dynamic_sub = worker.create_dynamic_subscription( + "test_msgs/msg/Empty".try_into().unwrap(), + "test_worker_topic", + |payload: &mut TestPayload, _, _| { + payload.dynamic_subscription_count += 1; + }, + ); + let _count_srv = worker.create_service::( "test_worker_service", |payload: &mut TestPayload, _req: Empty_Request| { @@ -535,6 +612,13 @@ mod tests { let promise = worker.listen_until(move |payload| { if payload.service_count > 0 && payload.subscription_count > 0 { + #[cfg(feature = "dyn_msg")] + if payload.dynamic_subscription_count > 0 { + Some(*payload) + } else { + None + } + #[cfg(not(feature = "dyn_msg"))] Some(*payload) } else { None