diff --git a/examples/dynamic_pub_sub/Cargo.toml b/examples/dynamic_pub_sub/Cargo.toml new file mode 100644 index 000000000..1565cbb7e --- /dev/null +++ b/examples/dynamic_pub_sub/Cargo.toml @@ -0,0 +1,10 @@ +[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] +rclrs = { version = "0.3", 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..6aae57fdc --- /dev/null +++ b/examples/dynamic_pub_sub/src/main.rs @@ -0,0 +1,22 @@ +use anyhow::{Error, Result}; +use std::env; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let mut node = rclrs::create_node(&context, "dynamic_subscriber")?; + + let mut num_messages: usize = 0; + + let _subscription = node.create_dynamic_subscription( + "topic", + "rclrs_example_msgs/msg/VariousTypes", + rclrs::QOS_PROFILE_DEFAULT, + move |msg| { + num_messages += 1; + println!("I heard: '{:#?}'", msg.structure()); + }, + )?; + + rclrs::spin(&node).map_err(|err| err.into()) +} diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 1251636c9..9701bf580 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -6,17 +6,28 @@ //! The central type of this module is [`DynamicMessage`]. use std::fmt::{self, Display}; +use std::ops::Deref; use std::path::PathBuf; use std::sync::Arc; +use rosidl_runtime_rs::RmwMessage; + #[cfg(any(ros_distro = "foxy", ros_distro = "galactic"))] use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers_s as rosidl_message_members_t; use crate::rcl_bindings::*; +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. /// @@ -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 {} @@ -250,6 +287,203 @@ impl DynamicMessageMetadata { let pkg = DynamicMessagePackage::new(package_name)?; pkg.message_metadata(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. + pub fn new(full_message_type: &str) -> Result { + DynamicMessageMetadata::new(full_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 mut dyn_msg = Self::new(::TYPE_NAME)?; + 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) + } + } } #[cfg(test)] @@ -263,6 +497,8 @@ mod tests { fn all_types_are_sync_and_send() { assert_send::(); assert_sync::(); + assert_send::(); + assert_sync::(); } #[test] diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs new file mode 100644 index 000000000..de59b2f32 --- /dev/null +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -0,0 +1,167 @@ +use std::ffi::CStr; +use std::ffi::CString; +use std::sync::{Arc, Mutex}; + +use super::{ + get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageError, + DynamicMessageMetadata, +}; +use crate::error::{RclrsError, ToResult}; +use crate::qos::QoSProfile; +use crate::rcl_bindings::*; +use crate::Node; + +/// Struct for sending messages of type `T`. +/// +/// Multiple publishers can be created for the same topic, in different nodes or the same node. +/// +/// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared +/// memory, or intraprocess). +/// +/// Sending messages does not require calling [`spin`][1] on the publisher's node. +/// +/// [1]: crate::spin +pub struct DynamicPublisher { + rcl_publisher_mtx: Mutex, + rcl_node_mtx: Arc>, + metadata: DynamicMessageMetadata, + // This is the regular type support library, not the introspection one. + #[allow(dead_code)] + type_support_library: Arc, +} + +impl Drop for DynamicPublisher { + fn drop(&mut self) { + unsafe { + // SAFETY: No preconditions for this function (besides the arguments being valid). + rcl_publisher_fini( + self.rcl_publisher_mtx.get_mut().unwrap(), + &mut *self.rcl_node_mtx.lock().unwrap(), + ); + } + } +} + +// 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 DynamicPublisher {} +// SAFETY: The type_support_ptr prevents the default Sync impl. +// rosidl_message_type_support_t is a read-only type without interior mutability. +// unsafe impl Sync for DynamicPublisher {} + +impl DynamicPublisher { + /// Creates a new `DynamicPublisher`. + /// + /// Node and namespace changes are always applied _before_ topic remapping. + pub fn new( + node: &Node, + topic: &str, + topic_type: &str, + qos: QoSProfile, + ) -> Result { + // This loads the introspection type support library. + let metadata = DynamicMessageMetadata::new(topic_type)?; + // 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(), + })?; + let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap(); + + // SAFETY: No preconditions for this function. + let mut publisher_options = unsafe { rcl_publisher_get_default_options() }; + publisher_options.qos = qos.into(); + 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(Self { + rcl_publisher_mtx: Mutex::new(rcl_publisher), + rcl_node_mtx: Arc::clone(&node.rcl_node_mtx), + 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 { + // 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_mtx.lock().unwrap()); + CStr::from_ptr(raw_topic_pointer) + .to_string_lossy() + .into_owned() + } + } + + /// 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.rcl_publisher_mtx.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::*; + + fn assert_send() {} + fn assert_sync() {} + + #[test] + fn dynamic_publisher_is_sync_and_send() { + assert_send::(); + assert_sync::(); + } +} diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs new file mode 100644 index 000000000..43d70236b --- /dev/null +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -0,0 +1,196 @@ +use std::boxed::Box; +use std::ffi::{CStr, CString}; +use std::sync::atomic::AtomicBool; +use std::sync::{Arc, Mutex}; + +use super::{ + get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageMetadata, + MessageStructure, +}; +use crate::rcl_bindings::*; +use crate::{ + Node, QoSProfile, RclReturnCode, RclrsError, SubscriptionBase, SubscriptionHandle, ToResult, +}; + +/// Struct for receiving messages whose type is only known at runtime. +pub struct DynamicSubscription { + pub(crate) handle: Arc, + /// The callback function that runs when a message was received. + pub callback: Mutex>, + metadata: DynamicMessageMetadata, + // This is the regular type support library, not the introspection one. + #[allow(dead_code)] + type_support_library: Arc, +} + +impl DynamicSubscription { + /// Creates a new dynamic subscription. + /// + /// This is not a public function, by the same rationale as `Subscription::new()`. + pub(crate) fn new( + node: &Node, + topic: &str, + topic_type: &str, + qos: QoSProfile, + callback: F, + ) -> Result + where + F: FnMut(DynamicMessage) + 'static + Send, + { + // This loads the introspection type support library. + let metadata = DynamicMessageMetadata::new(topic_type)?; + // 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(), + })?; + let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap(); + + // SAFETY: No preconditions for this function. + let mut subscription_options = unsafe { rcl_subscription_get_default_options() }; + subscription_options.qos = qos.into(); + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; + unsafe { + // SAFETY: The rcl_subscription 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_subscription_init( + &mut rcl_subscription, + rcl_node, + type_support_ptr, + topic_c_string.as_ptr(), + &subscription_options, + ) + .ok()?; + } + + let handle = Arc::new(SubscriptionHandle { + rcl_subscription_mtx: Mutex::new(rcl_subscription), + rcl_node_mtx: node.rcl_node_mtx.clone(), + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + + Ok(Self { + handle, + callback: Mutex::new(Box::new(callback)), + 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 { + // SAFETY: No preconditions for the function used + // 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() + } + } + + /// Returns a description of the message structure. + pub fn structure(&self) -> &MessageStructure { + &self.metadata.structure + } + + /// Fetches a new message. + /// + /// When there is no new message, this will return a + /// [`SubscriptionTakeFailed`][1]. + /// + /// [1]: crate::RclrsError + // + // ```text + // +-------------+ + // | rclrs::take | + // +------+------+ + // | + // | + // +------v------+ + // | rcl_take | + // +------+------+ + // | + // | + // +------v------+ + // | rmw_take | + // +-------------+ + // ``` + pub fn take(&self) -> Result { + let mut dynamic_message = self.metadata.create()?; + let rmw_message = dynamic_message.storage.as_mut_ptr(); + 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 _, + std::ptr::null_mut(), + std::ptr::null_mut(), + ) + .ok()? + }; + Ok(dynamic_message) + } +} + +impl SubscriptionBase for DynamicSubscription { + fn handle(&self) -> &SubscriptionHandle { + &self.handle + } + + fn execute(&self) -> Result<(), RclrsError> { + let msg = match self.take() { + Ok(msg) => msg, + Err(RclrsError::RclError { + code: RclReturnCode::SubscriptionTakeFailed, + .. + }) => { + // Spurious wakeup – this may happen even when a waitset indicated that this + // subscription was ready, so it shouldn't be an error. + return Ok(()); + } + Err(e) => return Err(e), + }; + (*self.callback.lock().unwrap())(msg); + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn assert_send() {} + fn assert_sync() {} + + #[test] + fn dynamic_subscription_is_sync_and_send() { + assert_send::(); + assert_sync::(); + } +} 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..5c8a51d1c --- /dev/null +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -0,0 +1,303 @@ +use std::ffi::CStr; +use std::num::NonZeroUsize; + +use super::TypeErasedSequence; +#[cfg(any(ros_distro = "foxy", ros_distro = "galactic"))] +use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMember as rosidl_message_member_t; +#[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] +use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMember_s as rosidl_message_member_t; +#[cfg(any(ros_distro = "foxy", ros_distro = "galactic"))] +use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; +#[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] +use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers_s 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 { + #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] + 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) + } +} diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index b84f4d4d3..9b52d1054 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -2,10 +2,11 @@ use std::error::Error; use std::ffi::{CStr, NulError}; use std::fmt::{self, Display}; +use crate::dynamic_message::DynamicMessageError; use crate::rcl_bindings::*; /// The main error type. -#[derive(Debug, PartialEq, Eq)] +#[derive(Debug)] pub enum RclrsError { /// An error originating in the `rcl` layer. RclError { @@ -30,6 +31,11 @@ pub enum RclrsError { }, /// It was attempted to add a waitable to a wait set twice. AlreadyAddedToWaitSet, + /// An error while creating dynamic message. + DynamicMessageError { + /// The error containing more detailed information. + err: DynamicMessageError, + }, } impl Display for RclrsError { @@ -46,10 +52,19 @@ impl Display for RclrsError { "Could not add entity to wait set because it was already added to a wait set" ) } + RclrsError::DynamicMessageError { .. } => { + write!(f, "Could not create dynamic message") + } } } } +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 @@ -78,6 +93,7 @@ impl Error for RclrsError { RclrsError::UnknownRclError { msg, .. } => msg.as_ref().map(|e| e as &dyn Error), RclrsError::StringContainsNul { err, .. } => Some(err).map(|e| e as &dyn Error), RclrsError::AlreadyAddedToWaitSet => None, + RclrsError::DynamicMessageError { err } => Some(err).map(|e| e as &dyn Error), } } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index cc0ecc665..60c4118fe 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,5 +1,6 @@ mod builder; mod graph; + use std::cmp::PartialEq; use std::ffi::CStr; use std::fmt; @@ -11,6 +12,8 @@ use rosidl_runtime_rs::Message; pub use self::builder::*; pub use self::graph::*; +#[cfg(feature = "dyn_msg")] +use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; use crate::rcl_bindings::*; use crate::{ Client, ClientBase, Context, GuardCondition, ParameterOverrideMap, Publisher, QoSProfile, @@ -284,6 +287,29 @@ impl Node { Ok(subscription) } + /// Creates a [`DynamicSubscription`][1]. + /// + /// [1]: crate::dynamic_message::DynamicSubscription + // TODO: make subscription's lifetime depend on node's lifetime + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_subscription( + &mut self, + topic: &str, + topic_type: &str, + qos: QoSProfile, + callback: F, + ) -> Result, RclrsError> + where + F: FnMut(DynamicMessage) + 'static + Send, + { + let subscription = Arc::new(DynamicSubscription::new( + self, topic, topic_type, qos, callback, + )?); + self.subscriptions + .push(Arc::downgrade(&subscription) as Weak); + Ok(subscription) + } + /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { self.subscriptions diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 5a55c5f85..64524c87a 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -24,8 +24,8 @@ unsafe impl Send for rcl_subscription_t {} /// Internal struct used by subscriptions. pub struct SubscriptionHandle { - rcl_subscription_mtx: Mutex, - rcl_node_mtx: Arc>, + pub(crate) rcl_subscription_mtx: Mutex, + pub(crate) rcl_node_mtx: Arc>, pub(crate) in_use_by_wait_set: Arc, } diff --git a/rclrs/test_header.h b/rclrs/test_header.h new file mode 100644 index 000000000..392e56386 --- /dev/null +++ b/rclrs/test_header.h @@ -0,0 +1,5 @@ +#include + +struct Foo { + char16_t i; +}; diff --git a/rclrs_example_msgs/msg/VariousTypes.msg b/rclrs_example_msgs/msg/VariousTypes.msg index fc42d4110..ad6f48505 100644 --- a/rclrs_example_msgs/msg/VariousTypes.msg +++ b/rclrs_example_msgs/msg/VariousTypes.msg @@ -3,6 +3,7 @@ bool bool_member true int8 int8_member 1 uint8 uint8_member 2 byte byte_member 3 +char char_member 3 float32 float32_member 1e-2 # Array/sequence of primitive type diff --git a/rclrs_tests/Cargo.toml b/rclrs_tests/Cargo.toml index 51708f4e7..16052414b 100644 --- a/rclrs_tests/Cargo.toml +++ b/rclrs_tests/Cargo.toml @@ -8,8 +8,8 @@ edition = "2021" path = "src/lib.rs" [dependencies] -anyhow = {version = "1", features = ["backtrace"]} -test_msgs = {version = "*"} +test_msgs = "*" +quickcheck = "1" [dependencies.rclrs] version = "*" diff --git a/rclrs_tests/build.rs b/rclrs_tests/build.rs new file mode 100644 index 000000000..d3f93e214 --- /dev/null +++ b/rclrs_tests/build.rs @@ -0,0 +1,19 @@ +use std::env; + +const ROS_DISTRO: &str = "ROS_DISTRO"; + +fn get_env_var_or_abort(env_var: &'static str) -> String { + if let Ok(value) = env::var(env_var) { + value + } else { + panic!( + "{} environment variable not set - please source ROS 2 installation first.", + env_var + ); + } +} + +fn main() { + let ros_distro = get_env_var_or_abort(ROS_DISTRO); + println!("cargo:rustc-cfg=ros_distro=\"{ros_distro}\""); +} diff --git a/rclrs_tests/src/dynamic_message_tests.rs b/rclrs_tests/src/dynamic_message_tests.rs new file mode 100644 index 000000000..d650e8bec --- /dev/null +++ b/rclrs_tests/src/dynamic_message_tests.rs @@ -0,0 +1,846 @@ +use std::num::NonZeroUsize; + +use rclrs::dynamic_message::*; +use test_msgs::msg; + +// #[test] +// fn conversion_roundtrip() { +// let msg = +// } + +#[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").unwrap(); + let arrays_structure = Box::new(arrays_metadata.structure().clone()); + let builtins_metadata = DynamicMessageMetadata::new("test_msgs/msg/Builtins").unwrap(); + let builtins_structure = Box::new(builtins_metadata.structure().clone()); + let duration_metadata = DynamicMessageMetadata::new("builtin_interfaces/msg/Duration").unwrap(); + let duration_structure = Box::new(duration_metadata.structure().clone()); + let empty_metadata = DynamicMessageMetadata::new("test_msgs/msg/Empty").unwrap(); + let empty_structure = Box::new(empty_metadata.structure().clone()); + let time_metadata = DynamicMessageMetadata::new("builtin_interfaces/msg/Time").unwrap(); + let time_structure = Box::new(time_metadata.structure().clone()); + let basic_types_metadata = DynamicMessageMetadata::new("test_msgs/msg/BasicTypes").unwrap(); + let basic_types_structure = Box::new(basic_types_metadata.structure().clone()); + let bounded_sequences_metadata = + DynamicMessageMetadata::new("test_msgs/msg/BoundedSequences").unwrap(); + let bounded_sequences_structure = Box::new(bounded_sequences_metadata.structure().clone()); + let constants_metadata = DynamicMessageMetadata::new("test_msgs/msg/Constants").unwrap(); + let constants_structure = Box::new(constants_metadata.structure().clone()); + let multi_nested_metadata = DynamicMessageMetadata::new("test_msgs/msg/MultiNested").unwrap(); + let multi_nested_structure = Box::new(multi_nested_metadata.structure().clone()); + let nested_metadata = DynamicMessageMetadata::new("test_msgs/msg/Nested").unwrap(); + let nested_structure = Box::new(nested_metadata.structure().clone()); + let defaults_metadata = DynamicMessageMetadata::new("test_msgs/msg/Defaults").unwrap(); + let defaults_structure = Box::new(defaults_metadata.structure().clone()); + let strings_metadata = DynamicMessageMetadata::new("test_msgs/msg/Strings").unwrap(); + let strings_structure = Box::new(strings_metadata.structure().clone()); + let wstrings_metadata = DynamicMessageMetadata::new("test_msgs/msg/WStrings").unwrap(); + let wstrings_structure = Box::new(wstrings_metadata.structure().clone()); + let unbounded_sequences_metadata = + DynamicMessageMetadata::new("test_msgs/msg/UnboundedSequences").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, + ); + } + } + + // Explicitly drop to avoid clippy warnings + drop(arrays_structure); + drop(builtins_structure); + drop(duration_structure); + drop(empty_structure); + drop(time_structure); + drop(basic_types_structure); + drop(bounded_sequences_structure); + drop(constants_structure); + drop(multi_nested_structure); + drop(nested_structure); + drop(defaults_structure); + drop(strings_structure); + drop(wstrings_structure); + drop(unbounded_sequences_structure); +} + +#[test] +fn dynamic_message_has_defaults() { + let dyn_msg = DynamicMessage::new("test_msgs/msg/Defaults").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").unwrap(); + let _dyn_msg = DynamicMessage::new("test_msgs/msg/UnboundedSequences").unwrap(); + let _dyn_msg = DynamicMessage::new("test_msgs/msg/BoundedSequences").unwrap(); +} + +// #[test] +// fn test_mut_value_same_as_value() { + +// } + +// #[test] +// fn test_setting_value() { + +// } diff --git a/rclrs_tests/src/lib.rs b/rclrs_tests/src/lib.rs index 7cabab86c..c45f95777 100644 --- a/rclrs_tests/src/lib.rs +++ b/rclrs_tests/src/lib.rs @@ -1,5 +1,8 @@ #![cfg(test)] mod client_service_tests; +// Disabled in Foxy due to https://github.com/ros2/rosidl/issues/598 +#[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] +mod dynamic_message_tests; mod graph_tests; mod pub_sub_tests; diff --git a/rosidl_runtime_rs/src/sequence.rs b/rosidl_runtime_rs/src/sequence.rs index 3ac2127b8..5c62dd6bd 100644 --- a/rosidl_runtime_rs/src/sequence.rs +++ b/rosidl_runtime_rs/src/sequence.rs @@ -137,6 +137,8 @@ impl Drop for Sequence { impl Eq for Sequence {} +// If you update this Extend implementation, please also update the one in +// dynamic_sequence.rs in rclrs since they share code. impl Extend for Sequence { fn extend(&mut self, iter: I) where