From 2936a40f9288770c6003fbfaf8250008c8195d4c Mon Sep 17 00:00:00 2001 From: nnmm Date: Tue, 19 Jul 2022 18:14:28 +0200 Subject: [PATCH 01/33] Add dynamic message functionality --- examples/dynamic_pub_sub/Cargo.toml | 10 + examples/dynamic_pub_sub/src/main.rs | 22 + rclrs/src/dynamic_message.rs | 242 ++++- .../src/dynamic_message/dynamic_publisher.rs | 167 +++ .../dynamic_message/dynamic_subscription.rs | 196 ++++ rclrs/src/dynamic_message/field_access.rs | 988 ++++++++++++++++++ .../field_access/dynamic_bounded_string.rs | 256 +++++ .../field_access/dynamic_message_view.rs | 199 ++++ .../field_access/dynamic_sequence.rs | 600 +++++++++++ .../src/dynamic_message/message_structure.rs | 303 ++++++ rclrs/src/error.rs | 18 +- rclrs/src/node.rs | 26 + rclrs/src/subscription.rs | 4 +- rclrs/test_header.h | 5 + rclrs_example_msgs/msg/VariousTypes.msg | 1 + rclrs_tests/Cargo.toml | 4 +- rclrs_tests/build.rs | 19 + rclrs_tests/src/dynamic_message_tests.rs | 846 +++++++++++++++ rclrs_tests/src/lib.rs | 3 + rosidl_runtime_rs/src/sequence.rs | 2 + 20 files changed, 3903 insertions(+), 8 deletions(-) create mode 100644 examples/dynamic_pub_sub/Cargo.toml create mode 100644 examples/dynamic_pub_sub/src/main.rs create mode 100644 rclrs/src/dynamic_message/dynamic_publisher.rs create mode 100644 rclrs/src/dynamic_message/dynamic_subscription.rs create mode 100644 rclrs/src/dynamic_message/field_access.rs create mode 100644 rclrs/src/dynamic_message/field_access/dynamic_bounded_string.rs create mode 100644 rclrs/src/dynamic_message/field_access/dynamic_message_view.rs create mode 100644 rclrs/src/dynamic_message/field_access/dynamic_sequence.rs create mode 100644 rclrs/src/dynamic_message/message_structure.rs create mode 100644 rclrs/test_header.h create mode 100644 rclrs_tests/build.rs create mode 100644 rclrs_tests/src/dynamic_message_tests.rs 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 From aaadbe4f018b239af79c349573d2f95d3abaf981 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 17 Jun 2025 17:50:32 +0800 Subject: [PATCH 02/33] Initial stab, builds docs Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 108 ++++++++++-------- rclrs/src/node.rs | 22 ++++ rclrs/src/subscription.rs | 76 ++++++------ 3 files changed, 120 insertions(+), 86 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 43d70236b..af95bd5de 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -9,14 +9,23 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ - Node, QoSProfile, RclReturnCode, RclrsError, SubscriptionBase, SubscriptionHandle, ToResult, + Node, QoSProfile, RclReturnCode, RclrsError, ToResult, NodeHandle, WorkerCommands, WaitableLifecycle, SubscriptionHandle, }; /// 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. + /// This handle is used to access the data that rcl holds for this subscription. + handle: Arc, + /// This allows us to replace the callback in the subscription task. + /// + /// Holding onto this sender will keep the subscription task alive. Once + /// this sender is dropped, the subscription task will end itself. + // callback: Arc>>, pub callback: Mutex>, + /// Holding onto this keeps the waiter for this subscription alive in the + /// wait set of the executor. + #[allow(unused)] + lifecycle: WaitableLifecycle, metadata: DynamicMessageMetadata, // This is the regular type support library, not the introspection one. #[allow(dead_code)] @@ -27,16 +36,18 @@ 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, + pub(crate) fn create_dynamic( topic: &str, topic_type: &str, qos: QoSProfile, callback: F, - ) -> Result + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> where F: FnMut(DynamicMessage) + 'static + Send, { + // TODO(luca) a lot of duplication with nomral, refactor // This loads the introspection type support library. let metadata = DynamicMessageMetadata::new(topic_type)?; // However, we also need the regular type support library – @@ -62,38 +73,62 @@ impl DynamicSubscription { 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(); + let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() }; + rcl_subscription_options.qos = qos.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; - 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 rcl_node = node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: + // * The rcl_subscription is zero-initialized as mandated by this function. + // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the subscription. + // * The topic name and the options are copied by this function, so they can be dropped afterwards. + // * The entity lifecycle mutex is locked to protect against the risk of global + // variables in the rmw implementation being unsafely modified during cleanup. + rcl_subscription_init( + &mut rcl_subscription, + &*rcl_node, + type_support_ptr, + topic_c_string.as_ptr(), + &rcl_subscription_options, + ) + .ok()?; + } } let handle = Arc::new(SubscriptionHandle { - rcl_subscription_mtx: Mutex::new(rcl_subscription), - rcl_node_mtx: node.rcl_node_mtx.clone(), - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + rcl_subscription: Mutex::new(rcl_subscription), + node_handle: Arc::clone(node_handle), }); + let (waitable, lifecycle) = Waitable::new( + Box::new(SubscriptionExecutable { + handle: Arc::clone(&handle), + callback: Arc::clone(&callback), + commands: Arc::clone(commands), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + commands.add_to_wait_set(waitable); + + Ok(Arc::new(Self { + handle, + callback, + lifecycle, + metadata, + type_support_library, + })) + + /* Ok(Self { handle, callback: Mutex::new(Box::new(callback)), metadata, type_support_library, }) + */ } /// Returns the topic name of the subscription. @@ -158,29 +193,6 @@ impl DynamicSubscription { } } -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::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 2f0753f97..c1c91fa69 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -12,6 +12,7 @@ pub use self::builder::*; pub use self::graph::*; ======= */ +use crate::QoSProfile; #[cfg(feature = "dyn_msg")] use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; @@ -796,6 +797,27 @@ impl NodeState { ) } + // TODO(luca) introduce subscription options and perhaps an into callback + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_subscription( + &self, + // options: impl Into>, + 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) + } + /// Creates a [`DynamicSubscription`][1]. /// /// [1]: crate::dynamic_message::DynamicSubscription diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 89e4ca5f8..632abb6cf 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -12,6 +12,8 @@ use crate::{ WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; +use crate::dynamic_message::DynamicMessage; + mod any_subscription_callback; pub use any_subscription_callback::*; @@ -33,43 +35,6 @@ pub use message_info::*; mod readonly_loaned_message; pub use readonly_loaned_message::*; -/* -// 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 rcl_subscription_t {} - -/// Internal struct used by subscriptions. -pub struct SubscriptionHandle { - pub(crate) rcl_subscription_mtx: Mutex, - pub(crate) rcl_node_mtx: Arc>, - pub(crate) in_use_by_wait_set: Arc, -} - -impl SubscriptionHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_subscription_mtx.lock().unwrap() - } -} - -impl Drop for SubscriptionHandle { - fn drop(&mut self) { - let rcl_subscription = self.rcl_subscription_mtx.get_mut().unwrap(); - let rcl_node = &mut *self.rcl_node_mtx.lock().unwrap(); - // SAFETY: No preconditions for this function (besides the arguments being valid). - unsafe { - rcl_subscription_fini(rcl_subscription, rcl_node); - } - } -} - -/// Trait to be implemented by concrete [`Subscription`]s. -pub trait SubscriptionBase: Send + Sync { - /// Internal function to get a reference to the `rcl` handle. - fn handle(&self) -> &SubscriptionHandle; - /// Tries to take a new message and run the callback with it. - fn execute(&self) -> Result<(), RclrsError>; -} -*/ mod worker_subscription_callback; pub use worker_subscription_callback::*; @@ -288,6 +253,41 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { } } +/// `SubscriptionOptions` are used by [`Node::create_subscription`][1] to initialize +/// a [`Subscription`]. +/// +/// [1]: crate::NodeState::create_subscription +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct DynamicSubscriptionOptions<'a> { + /// The topic name for the subscription. + pub topic: &'a str, + /// The topic type for the subscription. + pub topic_type: &'a str, + /// The quality of service settings for the subscription. + pub qos: QoSProfile, +} + +impl<'a> DynamicSubscriptionOptions<'a> { + /// Initialize a new [`SubscriptionOptions`] with default settings. + pub fn new(topic: &'a str, topic_type: &'a str) -> Self { + Self { + topic, + topic_type, + qos: QoSProfile::topics_default(), + } + } +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for DynamicSubscriptionOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply_to(&mut options.qos); + options + } +} + struct SubscriptionExecutable { handle: Arc, callback: Arc>>, @@ -323,7 +323,7 @@ unsafe impl Send for rcl_subscription_t {} /// [dropped after][1] the `rcl_subscription_t`. /// /// [1]: -struct SubscriptionHandle { +pub(crate) struct SubscriptionHandle { rcl_subscription: Mutex, node_handle: Arc, } From d5ad50fdbd0d1cc2785df7e676d141d28fc6a317 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 18 Jun 2025 09:27:02 +0800 Subject: [PATCH 03/33] WIP work through compile errors Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/dynamic_subscription.rs | 4 ++-- rclrs/src/dynamic_message/message_structure.rs | 1 + rclrs/src/node.rs | 9 ++++----- rclrs/src/subscription.rs | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index af95bd5de..ddb67f51e 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -9,6 +9,7 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ + ENTITY_LIFECYCLE_MUTEX, Waitable, Node, QoSProfile, RclReturnCode, RclrsError, ToResult, NodeHandle, WorkerCommands, WaitableLifecycle, SubscriptionHandle, }; @@ -36,7 +37,7 @@ impl DynamicSubscription { /// Creates a new dynamic subscription. /// /// This is not a public function, by the same rationale as `Subscription::new()`. - pub(crate) fn create_dynamic( + pub(crate) fn new( topic: &str, topic_type: &str, qos: QoSProfile, @@ -70,7 +71,6 @@ impl DynamicSubscription { err, s: topic.into(), })?; - let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap(); // SAFETY: No preconditions for this function. let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() }; diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 56da10d00..9fcdcdf39 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -5,6 +5,7 @@ use super::TypeErasedSequence; use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMember as rosidl_message_member_t; use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; use crate::rcl_bindings::*; +use crate::rcl_bindings::rosidl_typesupport_introspection_c_field_types::*; /// Possible base types for fields in a message. // The field variants are self-explaining, no need to add redundant documentation. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index c1c91fa69..93b3cead8 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -810,11 +810,10 @@ impl NodeState { 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); + let subscription = DynamicSubscription::new( + topic, topic_type, qos, callback, &self.handle, self.commands.async_worker_commands(), + )?; + // TODO(luca) similar API to above? Ok(subscription) } diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 632abb6cf..14b2b995c 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -329,7 +329,7 @@ pub(crate) struct SubscriptionHandle { } impl SubscriptionHandle { - fn lock(&self) -> MutexGuard { + pub(crate) fn lock(&self) -> MutexGuard { self.rcl_subscription.lock().unwrap() } From d140dbe61a8faf0f0f16c492be91d8e47e9efff2 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 18 Jun 2025 13:53:42 +0800 Subject: [PATCH 04/33] Clear warnings Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 2 -- rclrs/src/dynamic_message/message_structure.rs | 4 ++++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 6eec34acc..5a83dd2db 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -5,10 +5,8 @@ //! //! The central type of this module is [`DynamicMessage`]. - use std::{ fmt::{self, Display}, - ops::Deref, path::PathBuf, sync::Arc, }; diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 9fcdcdf39..8a0790a28 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -114,6 +114,10 @@ impl BaseType { string_upper_bound: Option, inner: *const rosidl_message_type_support_t, ) -> Self { +<<<<<<< HEAD +======= + use rosidl_typesupport_introspection_c_field_types::*; +>>>>>>> 4e1c0be (Clear warnings) 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, From f7aa5c0f26a212b5dc9b3a96ac36751ebdb0f019 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 18 Jun 2025 09:43:22 +0800 Subject: [PATCH 05/33] Bring back tests Signed-off-by: Luca Della Vedova --- .../src/dynamic_message/message_structure.rs | 788 ++++++++++++++++++ 1 file changed, 788 insertions(+) diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 8a0790a28..8c9225644 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -298,3 +298,791 @@ impl MessageStructure { self.fields.iter().find(|field| field.name == field_name) } } + +#[cfg(test)] +mod tests { + use super::*; + use crate::dynamic_message::*; + use test_msgs::msg; + + use std::num::NonZeroUsize; + + #[test] + fn max_alignment_is_8() { + // The DynamicMessage type makes sure that its storage is aligned to 8 + let alignments = [ + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + std::mem::align_of::(), + ]; + assert_eq!(alignments.into_iter().max().unwrap(), 8); + } + + #[test] + fn message_structure_is_accurate() { + let arrays_metadata = DynamicMessageMetadata::new("test_msgs/msg/Arrays").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); + } +} From 236b9998ec6143c3d7edd985e8581b6c5bea8e73 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 18 Jun 2025 12:56:17 +0800 Subject: [PATCH 06/33] Minor cleanup Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 2 +- rclrs/src/dynamic_message/message_structure.rs | 16 ---------------- 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 5a83dd2db..f220b9648 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -474,7 +474,7 @@ impl DynamicMessage { 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); + 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; diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 8c9225644..1f7c672d8 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -1068,21 +1068,5 @@ mod tests { ); } } - - // 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); } } From 780c660934bfb610de0acb767560004701972a3e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 18 Jun 2025 13:58:45 +0800 Subject: [PATCH 07/33] Remove spurious files Signed-off-by: Luca Della Vedova --- .../src/dynamic_message/message_structure.rs | 75 +- rclrs_tests/build.rs | 19 - rclrs_tests/src/dynamic_message_tests.rs | 846 ------------------ 3 files changed, 71 insertions(+), 869 deletions(-) delete mode 100644 rclrs_tests/build.rs delete mode 100644 rclrs_tests/src/dynamic_message_tests.rs diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 1f7c672d8..df204a8c2 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -5,7 +5,6 @@ use super::TypeErasedSequence; use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMember as rosidl_message_member_t; use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; use crate::rcl_bindings::*; -use crate::rcl_bindings::rosidl_typesupport_introspection_c_field_types::*; /// Possible base types for fields in a message. // The field variants are self-explaining, no need to add redundant documentation. @@ -114,10 +113,7 @@ impl BaseType { string_upper_bound: Option, inner: *const rosidl_message_type_support_t, ) -> Self { -<<<<<<< HEAD -======= use rosidl_typesupport_introspection_c_field_types::*; ->>>>>>> 4e1c0be (Clear warnings) 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, @@ -1070,3 +1066,74 @@ mod tests { } } } + +#[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/build.rs b/rclrs_tests/build.rs deleted file mode 100644 index d3f93e214..000000000 --- a/rclrs_tests/build.rs +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index d650e8bec..000000000 --- a/rclrs_tests/src/dynamic_message_tests.rs +++ /dev/null @@ -1,846 +0,0 @@ -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() { - -// } From 4fd40591510d558709ce73c3108b6844ec5fad46 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 18 Jun 2025 16:53:15 +0800 Subject: [PATCH 08/33] WIP working through subscriptions Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 1 + rclrs/src/dynamic_message/dynamic_publisher.rs | 15 ++++++++------- rclrs/src/dynamic_message/dynamic_subscription.rs | 6 ++++-- rclrs/src/error.rs | 5 +++++ rclrs/src/subscription.rs | 10 ++++++++++ 5 files changed, 28 insertions(+), 9 deletions(-) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index f220b9648..93b5871e1 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -7,6 +7,7 @@ use std::{ fmt::{self, Display}, + ops::Deref, path::PathBuf, sync::Arc, }; diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index de59b2f32..63820a259 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -9,7 +9,7 @@ use super::{ use crate::error::{RclrsError, ToResult}; use crate::qos::QoSProfile; use crate::rcl_bindings::*; -use crate::Node; +use crate::NodeHandle; /// Struct for sending messages of type `T`. /// @@ -23,7 +23,7 @@ use crate::Node; /// [1]: crate::spin pub struct DynamicPublisher { rcl_publisher_mtx: Mutex, - rcl_node_mtx: Arc>, + node_handle: Arc, metadata: DynamicMessageMetadata, // This is the regular type support library, not the introspection one. #[allow(dead_code)] @@ -32,11 +32,12 @@ pub struct DynamicPublisher { impl Drop for DynamicPublisher { fn drop(&mut self) { + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); 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(), + &mut *rcl_node, ); } } @@ -54,7 +55,7 @@ impl DynamicPublisher { /// /// Node and namespace changes are always applied _before_ topic remapping. pub fn new( - node: &Node, + node_handle: &Arc, topic: &str, topic_type: &str, qos: QoSProfile, @@ -83,7 +84,7 @@ impl DynamicPublisher { err, s: topic.into(), })?; - let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap(); + let rcl_node = node_handle.rcl_node.lock().unwrap(); // SAFETY: No preconditions for this function. let mut publisher_options = unsafe { rcl_publisher_get_default_options() }; @@ -96,7 +97,7 @@ impl DynamicPublisher { // TODO: type support? rcl_publisher_init( &mut rcl_publisher, - rcl_node, + &*rcl_node, type_support_ptr, topic_c_string.as_ptr(), &publisher_options, @@ -106,7 +107,7 @@ impl DynamicPublisher { Ok(Self { rcl_publisher_mtx: Mutex::new(rcl_publisher), - rcl_node_mtx: Arc::clone(&node.rcl_node_mtx), + node_handle: node_handle.clone(), metadata, type_support_library, }) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index ddb67f51e..dd6ba4500 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -21,8 +21,8 @@ pub struct DynamicSubscription { /// /// Holding onto this sender will keep the subscription task alive. Once /// this sender is dropped, the subscription task will end itself. - // callback: Arc>>, - pub callback: Mutex>, + pub callback: Arc>>, + // pub callback: Mutex>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. #[allow(unused)] @@ -103,6 +103,8 @@ impl DynamicSubscription { node_handle: Arc::clone(node_handle), }); + let callback = Arc::new(Mutex::new(callback)); + let (waitable, lifecycle) = Waitable::new( Box::new(SubscriptionExecutable { handle: Arc::clone(&handle), diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 74acb8816..41ada9348 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -4,6 +4,7 @@ use std::{ fmt::{self, Display}, }; +#[cfg(feature = "dyn_msg")] use crate::dynamic_message::DynamicMessageError; use crate::{rcl_bindings::*, DeclarationError}; @@ -33,6 +34,7 @@ pub enum RclrsError { }, /// It was attempted to add a waitable to a wait set twice. AlreadyAddedToWaitSet, + #[cfg(feature = "dyn_msg")] /// An error while creating dynamic message. DynamicMessageError { /// The error containing more detailed information. @@ -100,6 +102,7 @@ impl Display for RclrsError { "Could not add entity to wait set because it was already added to a wait set" ) } + #[cfg(feature = "dyn_msg")] RclrsError::DynamicMessageError { .. } => { write!(f, "Could not create dynamic message") } @@ -131,6 +134,7 @@ impl Display for RclrsError { } } +#[cfg(feature = "dyn_msg")] impl From for RclrsError { fn from(err: DynamicMessageError) -> Self { Self::DynamicMessageError { err } @@ -167,6 +171,7 @@ impl Error for RclrsError { // TODO(@mxgrey): We should provide source information for these other types. // It should be easy to do this using the thiserror crate. RclrsError::AlreadyAddedToWaitSet => None, + #[cfg(feature = "dyn_msg")] RclrsError::DynamicMessageError { err } => Some(err).map(|e| e as &dyn Error), RclrsError::NegativeDuration(_) => None, RclrsError::UnownedGuardCondition => None, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 14b2b995c..fe11305b5 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -12,6 +12,7 @@ use crate::{ WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; +#[cfg(feature = "dyn_msg")] use crate::dynamic_message::DynamicMessage; mod any_subscription_callback; @@ -279,6 +280,7 @@ impl<'a> DynamicSubscriptionOptions<'a> { } } +/* impl<'a, T: IntoPrimitiveOptions<'a>> From for DynamicSubscriptionOptions<'a> { fn from(value: T) -> Self { let primitive = value.into_primitive_options(); @@ -287,6 +289,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for DynamicSubscriptionOptions<'a> options } } +*/ struct SubscriptionExecutable { handle: Arc, @@ -361,6 +364,13 @@ impl SubscriptionHandle { Ok((T::from_rmw_message(rmw_message), message_info)) } + /* + #[cfg(feature = "dyn_msg")] + fn take_dynamic(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> { + + } + */ + /// This is a version of take() that returns a boxed message. /// /// This can be more efficient for messages containing large arrays. From e79f46fe421aee462fcfd2852e49687f516d0549 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 10:48:56 +0800 Subject: [PATCH 09/33] First integration with async workers Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 162 ++++++++++++++++-- rclrs/src/dynamic_message/error.rs | 1 + rclrs/src/node.rs | 11 +- rclrs/src/subscription.rs | 4 +- 4 files changed, 159 insertions(+), 19 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index dd6ba4500..e0b87dcce 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -1,52 +1,188 @@ +use std::any::Any; use std::boxed::Box; use std::ffi::{CStr, CString}; +use std::ops::{Deref, DerefMut}; use std::sync::atomic::AtomicBool; use std::sync::{Arc, Mutex}; +use futures::future::BoxFuture; + use super::{ get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageMetadata, MessageStructure, }; use crate::rcl_bindings::*; use crate::{ - ENTITY_LIFECYCLE_MUTEX, Waitable, + ENTITY_LIFECYCLE_MUTEX, Waitable, RclPrimitive, MessageInfo, RclPrimitiveKind, RclPrimitiveHandle, WorkScope, Node, QoSProfile, RclReturnCode, RclrsError, ToResult, NodeHandle, WorkerCommands, WaitableLifecycle, SubscriptionHandle, }; +struct DynamicSubscriptionExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, + metadata: Arc, +} + +// TODO(luca) consider making these enums if we want different callback types +// TODO(luca) make fields private +pub struct NodeDynamicSubscriptionCallback(pub Box BoxFuture<'static, ()> + Send>); +pub struct WorkerDynamicSubscriptionCallback(pub Box); + +impl Deref for NodeDynamicSubscriptionCallback { + type Target = Box BoxFuture<'static, ()> + Send>; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl DerefMut for NodeDynamicSubscriptionCallback { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +impl Deref for WorkerDynamicSubscriptionCallback { + type Target = Box; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl DerefMut for WorkerDynamicSubscriptionCallback { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +//pub trait NodeDynamicSubscriptionCallback = FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send; +//pub trait WorkerDynamicSubscriptionCallback = FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send; + +pub enum DynamicSubscriptionCallback { + /// A callback with the message and the message info as arguments. + Node(NodeDynamicSubscriptionCallback), + /// A callback with the payload, message, and the message info as arguments. + Worker(WorkerDynamicSubscriptionCallback), +} + +impl From for DynamicSubscriptionCallback<()> { + fn from(value: NodeDynamicSubscriptionCallback) -> Self { + DynamicSubscriptionCallback::Node(value) + } +} + +impl From> for DynamicSubscriptionCallback { + fn from(value: WorkerDynamicSubscriptionCallback) -> Self { + DynamicSubscriptionCallback::Worker(value) + } +} + +impl DynamicSubscriptionCallback { + pub(super) fn execute( + &mut self, + executable: &DynamicSubscriptionExecutable, + any_payload: &mut dyn Any, + commands: &WorkerCommands, + ) -> Result<(), RclrsError> { + let Some(payload) = any_payload.downcast_mut::() else { + return Err(RclrsError::InvalidPayload { + expected: std::any::TypeId::of::(), + received: (*any_payload).type_id(), + }); + }; + match self { + Self::Node(cb) => { + let (msg, msg_info) = executable.take()?; + commands.run_async(cb(msg, msg_info)); + } + Self::Worker(cb) => { + let (msg, msg_info) = executable.take()?; + cb(payload, msg, msg_info); + } + } + Ok(()) + } +} + +impl DynamicSubscriptionExecutable { + pub fn take(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> { + let mut dynamic_message = self.metadata.create()?; + let rmw_message = dynamic_message.storage.as_mut_ptr(); + let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; + let rcl_subscription = &mut *self.handle.lock(); + unsafe { + // SAFETY: The first two pointers are valid/initialized, and do not need to be valid + // beyond the function call. + // The latter two pointers are explicitly allowed to be NULL. + rcl_take( + rcl_subscription, + rmw_message as *mut _, + &mut message_info, + std::ptr::null_mut(), + ) + .ok()? + }; + Ok((dynamic_message, MessageInfo::from_rmw_message_info(&message_info))) + } +} + +impl RclPrimitive for DynamicSubscriptionExecutable +{ + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + self.callback + .lock() + .unwrap() + .execute(&self, payload, &self.commands) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Subscription + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Subscription(self.handle.lock()) + } +} + /// Struct for receiving messages whose type is only known at runtime. -pub struct DynamicSubscription { +pub struct DynamicSubscription +where + Scope: WorkScope, +{ /// This handle is used to access the data that rcl holds for this subscription. handle: Arc, /// This allows us to replace the callback in the subscription task. /// /// Holding onto this sender will keep the subscription task alive. Once /// this sender is dropped, the subscription task will end itself. - pub callback: Arc>>, - // pub callback: Mutex>, + // pub callback: Arc>>, + // pub callback: Arc>>, + callback: Arc>>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. #[allow(unused)] lifecycle: WaitableLifecycle, - metadata: DynamicMessageMetadata, + metadata: Arc, // This is the regular type support library, not the introspection one. #[allow(dead_code)] type_support_library: Arc, } -impl DynamicSubscription { +impl DynamicSubscription +where + Scope: WorkScope +{ /// Creates a new dynamic subscription. /// /// This is not a public function, by the same rationale as `Subscription::new()`. - pub(crate) fn new( + pub(crate) fn new( topic: &str, topic_type: &str, qos: QoSProfile, - callback: F, + callback: impl Into>, node_handle: &Arc, commands: &Arc, ) -> Result, RclrsError> - where - F: FnMut(DynamicMessage) + 'static + Send, { // TODO(luca) a lot of duplication with nomral, refactor // This loads the introspection type support library. @@ -103,13 +239,15 @@ impl DynamicSubscription { node_handle: Arc::clone(node_handle), }); - let callback = Arc::new(Mutex::new(callback)); + let callback = Arc::new(Mutex::new(callback.into())); + let metadata = Arc::new(metadata); let (waitable, lifecycle) = Waitable::new( - Box::new(SubscriptionExecutable { + Box::new(DynamicSubscriptionExecutable { handle: Arc::clone(&handle), callback: Arc::clone(&callback), commands: Arc::clone(commands), + metadata: Arc::clone(&metadata), }), Some(Arc::clone(commands.get_guard_condition())), ); diff --git a/rclrs/src/dynamic_message/error.rs b/rclrs/src/dynamic_message/error.rs index 1303e9e30..79e5eefeb 100644 --- a/rclrs/src/dynamic_message/error.rs +++ b/rclrs/src/dynamic_message/error.rs @@ -1,6 +1,7 @@ use std::{error::Error, fmt}; /// An error related to creating a dynamic message based on the name of the message's type. +// TODO(luca) we need PartialEq (and maybe Eq?) for testing #[derive(Debug)] pub enum DynamicMessageError { /// The type support library was not found because no matching prefix was sourced. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 93b3cead8..be1feb84a 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -18,6 +18,7 @@ use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; pub use graph::*; +use futures::future::BoxFuture; mod node_graph_task; use node_graph_task::*; @@ -43,8 +44,8 @@ use rosidl_runtime_rs::Message; use crate::{ rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, - IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, - ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, + IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, dynamic_message::NodeDynamicSubscriptionCallback, + ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, MessageInfo, Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX, }; @@ -806,12 +807,12 @@ impl NodeState { topic_type: &str, qos: QoSProfile, callback: F, - ) -> Result, RclrsError> + ) -> Result>, RclrsError> where - F: FnMut(DynamicMessage) + 'static + Send, + F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + 'static, { let subscription = DynamicSubscription::new( - topic, topic_type, qos, callback, &self.handle, self.commands.async_worker_commands(), + topic, topic_type, qos, NodeDynamicSubscriptionCallback(Box::new(callback)), &self.handle, self.commands.async_worker_commands(), )?; // TODO(luca) similar API to above? Ok(subscription) diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fe11305b5..2283dcb9b 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -327,8 +327,8 @@ unsafe impl Send for rcl_subscription_t {} /// /// [1]: pub(crate) struct SubscriptionHandle { - rcl_subscription: Mutex, - node_handle: Arc, + pub(crate) rcl_subscription: Mutex, + pub(crate) node_handle: Arc, } impl SubscriptionHandle { From 73eec6b1fc66b9eb3f1e23692c6280749a12b6b3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 13:33:43 +0800 Subject: [PATCH 10/33] Add a PartialEq implementation for tests Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/error.rs | 12 ++++++++++++ rclrs/src/error.rs | 2 +- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/rclrs/src/dynamic_message/error.rs b/rclrs/src/dynamic_message/error.rs index 79e5eefeb..e4eee09ef 100644 --- a/rclrs/src/dynamic_message/error.rs +++ b/rclrs/src/dynamic_message/error.rs @@ -51,3 +51,15 @@ impl Error for DynamicMessageError { } } } + +impl PartialEq for DynamicMessageError { + fn eq(&self, other: &Self) -> bool { + if std::mem::discriminant(self) != std::mem::discriminant(other) { + return false; + } + // TODO(luca) this is not very efficient, revisit + return self.to_string() == other.to_string(); + } +} + +impl Eq for DynamicMessageError { } diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 41ada9348..146bbc70d 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -9,7 +9,7 @@ use crate::dynamic_message::DynamicMessageError; use crate::{rcl_bindings::*, DeclarationError}; /// The main error type. -#[derive(Debug)] +#[derive(Debug, PartialEq, Eq)] pub enum RclrsError { /// An error originating in the `rcl` layer. RclError { From 76e18d5eeed56194e1d3e0a967b135cda19334bf Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 14:10:08 +0800 Subject: [PATCH 11/33] Fix test code Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 35 ++-- .../src/dynamic_message/dynamic_publisher.rs | 4 +- .../dynamic_message/dynamic_subscription.rs | 8 +- .../src/dynamic_message/message_structure.rs | 149 +++++++++--------- rclrs/src/node.rs | 4 +- 5 files changed, 100 insertions(+), 100 deletions(-) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 93b5871e1..0cd6e0822 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -50,7 +50,7 @@ struct DynamicMessagePackage { /// A parsed/validated message type name of the form `/msg/`. #[derive(Clone, Debug, PartialEq, Eq)] -struct MessageTypeName { +pub struct MessageTypeName { /// The package name, which acts as a namespace. pub package_name: String, /// The name of the message type in the package. @@ -279,13 +279,9 @@ impl DynamicMessageMetadata { /// Loads the metadata for the given message type. /// /// See [`DynamicMessage::new()`] for the expected format of the `full_message_type`. - pub fn new(full_message_type: &str) -> Result { - let MessageTypeName { - package_name, - type_name, - } = full_message_type.try_into()?; - let pkg = DynamicMessagePackage::new(package_name)?; - pkg.message_metadata(type_name) + pub fn new(message_type: MessageTypeName) -> Result { + let pkg = DynamicMessagePackage::new(&message_type.package_name)?; + pkg.message_metadata(&message_type.type_name) } /// Instantiates a new message. @@ -363,8 +359,8 @@ impl DynamicMessage { /// `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() + pub fn new(message_type: MessageTypeName) -> Result { + DynamicMessageMetadata::new(message_type)?.create() } /// See [`DynamicMessageView::get()`][1]. @@ -436,7 +432,8 @@ impl DynamicMessage { where T: RmwMessage, { - let mut dyn_msg = Self::new(::TYPE_NAME)?; + let message_type = MessageTypeName::try_from(::TYPE_NAME)?; + let mut dyn_msg = Self::new(message_type)?; let align = std::mem::align_of::(); assert_eq!(dyn_msg.storage.as_ptr().align_offset(align), 0); { @@ -503,24 +500,26 @@ mod tests { #[test] fn invalid_message_type_name() { assert!(matches!( - DynamicMessageMetadata::new("x"), + MessageTypeName::try_from("x"), Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); assert!(matches!( - DynamicMessageMetadata::new("x/y"), + MessageTypeName::try_from("x/y"), Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); assert!(matches!( - DynamicMessageMetadata::new("x//y"), + MessageTypeName::try_from("x//y"), Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); assert!(matches!( - DynamicMessageMetadata::new("x/msg/y"), - Err(DynamicMessageError::RequiredPrefixNotSourced { .. }) + MessageTypeName::try_from("x/msg/y/z"), + Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) )); + // This is valid, but not found in the prefix + let message_type = MessageTypeName::try_from("x/msg/y").unwrap(); assert!(matches!( - DynamicMessageMetadata::new("x/msg/y/z"), - Err(DynamicMessageError::InvalidMessageTypeSyntax { .. }) + DynamicMessage::new(message_type), + Err(DynamicMessageError::RequiredPrefixNotSourced { .. }) )); } } diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 63820a259..87294a93a 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -4,7 +4,7 @@ use std::sync::{Arc, Mutex}; use super::{ get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageError, - DynamicMessageMetadata, + DynamicMessageMetadata, MessageTypeName, }; use crate::error::{RclrsError, ToResult}; use crate::qos::QoSProfile; @@ -57,7 +57,7 @@ impl DynamicPublisher { pub fn new( node_handle: &Arc, topic: &str, - topic_type: &str, + topic_type: MessageTypeName, qos: QoSProfile, ) -> Result { // This loads the introspection type support library. diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index e0b87dcce..ec9a04066 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -9,7 +9,7 @@ use futures::future::BoxFuture; use super::{ get_type_support_handle, get_type_support_library, DynamicMessage, DynamicMessageMetadata, - MessageStructure, + MessageStructure, MessageTypeName, }; use crate::rcl_bindings::*; use crate::{ @@ -155,8 +155,6 @@ where /// /// Holding onto this sender will keep the subscription task alive. Once /// this sender is dropped, the subscription task will end itself. - // pub callback: Arc>>, - // pub callback: Arc>>, callback: Arc>>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. @@ -177,7 +175,7 @@ where /// This is not a public function, by the same rationale as `Subscription::new()`. pub(crate) fn new( topic: &str, - topic_type: &str, + topic_type: MessageTypeName, qos: QoSProfile, callback: impl Into>, node_handle: &Arc, @@ -340,9 +338,11 @@ mod tests { 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/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index df204a8c2..5f11cc0b3 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -325,37 +325,37 @@ mod tests { #[test] fn message_structure_is_accurate() { - let arrays_metadata = DynamicMessageMetadata::new("test_msgs/msg/Arrays").unwrap(); + let arrays_metadata = DynamicMessageMetadata::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); let arrays_structure = Box::new(arrays_metadata.structure().clone()); - let builtins_metadata = DynamicMessageMetadata::new("test_msgs/msg/Builtins").unwrap(); + let builtins_metadata = DynamicMessageMetadata::new("test_msgs/msg/Builtins".try_into().unwrap()).unwrap(); let builtins_structure = Box::new(builtins_metadata.structure().clone()); let duration_metadata = - DynamicMessageMetadata::new("builtin_interfaces/msg/Duration").unwrap(); + DynamicMessageMetadata::new("builtin_interfaces/msg/Duration".try_into().unwrap()).unwrap(); let duration_structure = Box::new(duration_metadata.structure().clone()); - let empty_metadata = DynamicMessageMetadata::new("test_msgs/msg/Empty").unwrap(); + let empty_metadata = DynamicMessageMetadata::new("test_msgs/msg/Empty".try_into().unwrap()).unwrap(); let empty_structure = Box::new(empty_metadata.structure().clone()); - let time_metadata = DynamicMessageMetadata::new("builtin_interfaces/msg/Time").unwrap(); + let time_metadata = DynamicMessageMetadata::new("builtin_interfaces/msg/Time".try_into().unwrap()).unwrap(); let time_structure = Box::new(time_metadata.structure().clone()); - let basic_types_metadata = DynamicMessageMetadata::new("test_msgs/msg/BasicTypes").unwrap(); + let basic_types_metadata = DynamicMessageMetadata::new("test_msgs/msg/BasicTypes".try_into().unwrap()).unwrap(); let basic_types_structure = Box::new(basic_types_metadata.structure().clone()); let bounded_sequences_metadata = - DynamicMessageMetadata::new("test_msgs/msg/BoundedSequences").unwrap(); + DynamicMessageMetadata::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); let bounded_sequences_structure = Box::new(bounded_sequences_metadata.structure().clone()); - let constants_metadata = DynamicMessageMetadata::new("test_msgs/msg/Constants").unwrap(); + let constants_metadata = DynamicMessageMetadata::new("test_msgs/msg/Constants".try_into().unwrap()).unwrap(); let constants_structure = Box::new(constants_metadata.structure().clone()); let multi_nested_metadata = - DynamicMessageMetadata::new("test_msgs/msg/MultiNested").unwrap(); + DynamicMessageMetadata::new("test_msgs/msg/MultiNested".try_into().unwrap()).unwrap(); let multi_nested_structure = Box::new(multi_nested_metadata.structure().clone()); - let nested_metadata = DynamicMessageMetadata::new("test_msgs/msg/Nested").unwrap(); + let nested_metadata = DynamicMessageMetadata::new("test_msgs/msg/Nested".try_into().unwrap()).unwrap(); let nested_structure = Box::new(nested_metadata.structure().clone()); - let defaults_metadata = DynamicMessageMetadata::new("test_msgs/msg/Defaults").unwrap(); + let defaults_metadata = DynamicMessageMetadata::new("test_msgs/msg/Defaults".try_into().unwrap()).unwrap(); let defaults_structure = Box::new(defaults_metadata.structure().clone()); - let strings_metadata = DynamicMessageMetadata::new("test_msgs/msg/Strings").unwrap(); + let strings_metadata = DynamicMessageMetadata::new("test_msgs/msg/Strings".try_into().unwrap()).unwrap(); let strings_structure = Box::new(strings_metadata.structure().clone()); - let wstrings_metadata = DynamicMessageMetadata::new("test_msgs/msg/WStrings").unwrap(); + let wstrings_metadata = DynamicMessageMetadata::new("test_msgs/msg/WStrings".try_into().unwrap()).unwrap(); let wstrings_structure = Box::new(wstrings_metadata.structure().clone()); let unbounded_sequences_metadata = - DynamicMessageMetadata::new("test_msgs/msg/UnboundedSequences").unwrap(); + DynamicMessageMetadata::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()).unwrap(); let unbounded_sequences_structure = Box::new(unbounded_sequences_metadata.structure().clone()); @@ -1065,69 +1065,70 @@ mod tests { } } } -} -#[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 dynamic_message_has_defaults() { + let dyn_msg = DynamicMessage::new("test_msgs/msg/Defaults".try_into().unwrap()).unwrap(); + assert_eq!( + dyn_msg.get("bool_value"), + Some(Value::Simple(SimpleValue::Boolean(&true))) + ); + assert_eq!( + dyn_msg.get("byte_value"), + Some(Value::Simple(SimpleValue::Octet(&50u8))) + ); + assert_eq!( + dyn_msg.get("char_value"), + Some(Value::Simple(SimpleValue::Uint8(&100u8))) + ); + assert_eq!( + dyn_msg.get("float32_value"), + Some(Value::Simple(SimpleValue::Float(&1.125f32))) + ); + assert_eq!( + dyn_msg.get("float64_value"), + Some(Value::Simple(SimpleValue::Double(&1.125f64))) + ); + assert_eq!( + dyn_msg.get("int8_value"), + Some(Value::Simple(SimpleValue::Int8(&-50i8))) + ); + assert_eq!( + dyn_msg.get("uint8_value"), + Some(Value::Simple(SimpleValue::Uint8(&200u8))) + ); + assert_eq!( + dyn_msg.get("int16_value"), + Some(Value::Simple(SimpleValue::Int16(&-1000i16))) + ); + assert_eq!( + dyn_msg.get("uint16_value"), + Some(Value::Simple(SimpleValue::Uint16(&2000u16))) + ); + assert_eq!( + dyn_msg.get("int32_value"), + Some(Value::Simple(SimpleValue::Int32(&-30000i32))) + ); + assert_eq!( + dyn_msg.get("uint32_value"), + Some(Value::Simple(SimpleValue::Uint32(&60000u32))) + ); + assert_eq!( + dyn_msg.get("int64_value"), + Some(Value::Simple(SimpleValue::Int64(&-40000000i64))) + ); + assert_eq!( + dyn_msg.get("uint64_value"), + Some(Value::Simple(SimpleValue::Uint64(&50000000u64))) + ); + + let _dyn_msg = DynamicMessage::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); + let _dyn_msg = DynamicMessage::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()).unwrap(); + let _dyn_msg = DynamicMessage::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); + } } + // #[test] // fn test_mut_value_same_as_value() { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index be1feb84a..f02df3585 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -44,7 +44,7 @@ use rosidl_runtime_rs::Message; use crate::{ rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, - IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, dynamic_message::NodeDynamicSubscriptionCallback, + IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, dynamic_message::{NodeDynamicSubscriptionCallback, MessageTypeName}, ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, MessageInfo, Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX, @@ -804,7 +804,7 @@ impl NodeState { &self, // options: impl Into>, topic: &str, - topic_type: &str, + topic_type: MessageTypeName, qos: QoSProfile, callback: F, ) -> Result>, RclrsError> From 5b513bad2bbba12e76657897b4e4ab794888ed13 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 14:10:48 +0800 Subject: [PATCH 12/33] Format Signed-off-by: Luca Della Vedova --- .../src/dynamic_message/dynamic_publisher.rs | 5 +- .../dynamic_message/dynamic_subscription.rs | 34 ++++++++------ rclrs/src/dynamic_message/error.rs | 2 +- .../src/dynamic_message/message_structure.rs | 46 ++++++++++++------- rclrs/src/node.rs | 22 ++++++--- 5 files changed, 68 insertions(+), 41 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 87294a93a..238014289 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -35,10 +35,7 @@ impl Drop for DynamicPublisher { let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); unsafe { // SAFETY: No preconditions for this function (besides the arguments being valid). - rcl_publisher_fini( - self.rcl_publisher_mtx.get_mut().unwrap(), - &mut *rcl_node, - ); + rcl_publisher_fini(self.rcl_publisher_mtx.get_mut().unwrap(), &mut *rcl_node); } } } diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index ec9a04066..bbac66490 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -13,8 +13,9 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ - ENTITY_LIFECYCLE_MUTEX, Waitable, RclPrimitive, MessageInfo, RclPrimitiveKind, RclPrimitiveHandle, WorkScope, - Node, QoSProfile, RclReturnCode, RclrsError, ToResult, NodeHandle, WorkerCommands, WaitableLifecycle, SubscriptionHandle, + MessageInfo, Node, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, + RclReturnCode, RclrsError, SubscriptionHandle, ToResult, Waitable, WaitableLifecycle, + WorkScope, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; struct DynamicSubscriptionExecutable { @@ -26,11 +27,15 @@ struct DynamicSubscriptionExecutable { // TODO(luca) consider making these enums if we want different callback types // TODO(luca) make fields private -pub struct NodeDynamicSubscriptionCallback(pub Box BoxFuture<'static, ()> + Send>); -pub struct WorkerDynamicSubscriptionCallback(pub Box); +pub struct NodeDynamicSubscriptionCallback( + pub Box BoxFuture<'static, ()> + Send>, +); +pub struct WorkerDynamicSubscriptionCallback( + pub Box, +); impl Deref for NodeDynamicSubscriptionCallback { - type Target = Box BoxFuture<'static, ()> + Send>; + type Target = Box BoxFuture<'static, ()> + Send>; fn deref(&self) -> &Self::Target { &self.0 } @@ -58,7 +63,7 @@ impl DerefMut for WorkerDynamicSubscriptionCallback { //pub trait NodeDynamicSubscriptionCallback = FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send; //pub trait WorkerDynamicSubscriptionCallback = FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send; -pub enum DynamicSubscriptionCallback { +pub enum DynamicSubscriptionCallback { /// A callback with the message and the message info as arguments. Node(NodeDynamicSubscriptionCallback), /// A callback with the payload, message, and the message info as arguments. @@ -71,7 +76,9 @@ impl From for DynamicSubscriptionCallback<()> { } } -impl From> for DynamicSubscriptionCallback { +impl From> + for DynamicSubscriptionCallback +{ fn from(value: WorkerDynamicSubscriptionCallback) -> Self { DynamicSubscriptionCallback::Worker(value) } @@ -122,12 +129,14 @@ impl DynamicSubscriptionExecutable { ) .ok()? }; - Ok((dynamic_message, MessageInfo::from_rmw_message_info(&message_info))) + Ok(( + dynamic_message, + MessageInfo::from_rmw_message_info(&message_info), + )) } } -impl RclPrimitive for DynamicSubscriptionExecutable -{ +impl RclPrimitive for DynamicSubscriptionExecutable { unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { self.callback .lock() @@ -168,7 +177,7 @@ where impl DynamicSubscription where - Scope: WorkScope + Scope: WorkScope, { /// Creates a new dynamic subscription. /// @@ -180,8 +189,7 @@ where callback: impl Into>, node_handle: &Arc, commands: &Arc, - ) -> Result, RclrsError> - { + ) -> Result, RclrsError> { // TODO(luca) a lot of duplication with nomral, refactor // This loads the introspection type support library. let metadata = DynamicMessageMetadata::new(topic_type)?; diff --git a/rclrs/src/dynamic_message/error.rs b/rclrs/src/dynamic_message/error.rs index e4eee09ef..777305b4c 100644 --- a/rclrs/src/dynamic_message/error.rs +++ b/rclrs/src/dynamic_message/error.rs @@ -62,4 +62,4 @@ impl PartialEq for DynamicMessageError { } } -impl Eq for DynamicMessageError { } +impl Eq for DynamicMessageError {} diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 5f11cc0b3..810c68d6d 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -325,37 +325,50 @@ mod tests { #[test] fn message_structure_is_accurate() { - let arrays_metadata = DynamicMessageMetadata::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); + let arrays_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); let arrays_structure = Box::new(arrays_metadata.structure().clone()); - let builtins_metadata = DynamicMessageMetadata::new("test_msgs/msg/Builtins".try_into().unwrap()).unwrap(); + let builtins_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Builtins".try_into().unwrap()).unwrap(); let builtins_structure = Box::new(builtins_metadata.structure().clone()); let duration_metadata = - DynamicMessageMetadata::new("builtin_interfaces/msg/Duration".try_into().unwrap()).unwrap(); + DynamicMessageMetadata::new("builtin_interfaces/msg/Duration".try_into().unwrap()) + .unwrap(); let duration_structure = Box::new(duration_metadata.structure().clone()); - let empty_metadata = DynamicMessageMetadata::new("test_msgs/msg/Empty".try_into().unwrap()).unwrap(); + let empty_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Empty".try_into().unwrap()).unwrap(); let empty_structure = Box::new(empty_metadata.structure().clone()); - let time_metadata = DynamicMessageMetadata::new("builtin_interfaces/msg/Time".try_into().unwrap()).unwrap(); + let time_metadata = + DynamicMessageMetadata::new("builtin_interfaces/msg/Time".try_into().unwrap()).unwrap(); let time_structure = Box::new(time_metadata.structure().clone()); - let basic_types_metadata = DynamicMessageMetadata::new("test_msgs/msg/BasicTypes".try_into().unwrap()).unwrap(); + let basic_types_metadata = + DynamicMessageMetadata::new("test_msgs/msg/BasicTypes".try_into().unwrap()).unwrap(); let basic_types_structure = Box::new(basic_types_metadata.structure().clone()); let bounded_sequences_metadata = - DynamicMessageMetadata::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); + DynamicMessageMetadata::new("test_msgs/msg/BoundedSequences".try_into().unwrap()) + .unwrap(); let bounded_sequences_structure = Box::new(bounded_sequences_metadata.structure().clone()); - let constants_metadata = DynamicMessageMetadata::new("test_msgs/msg/Constants".try_into().unwrap()).unwrap(); + let constants_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Constants".try_into().unwrap()).unwrap(); let constants_structure = Box::new(constants_metadata.structure().clone()); let multi_nested_metadata = DynamicMessageMetadata::new("test_msgs/msg/MultiNested".try_into().unwrap()).unwrap(); let multi_nested_structure = Box::new(multi_nested_metadata.structure().clone()); - let nested_metadata = DynamicMessageMetadata::new("test_msgs/msg/Nested".try_into().unwrap()).unwrap(); + let nested_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Nested".try_into().unwrap()).unwrap(); let nested_structure = Box::new(nested_metadata.structure().clone()); - let defaults_metadata = DynamicMessageMetadata::new("test_msgs/msg/Defaults".try_into().unwrap()).unwrap(); + let defaults_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Defaults".try_into().unwrap()).unwrap(); let defaults_structure = Box::new(defaults_metadata.structure().clone()); - let strings_metadata = DynamicMessageMetadata::new("test_msgs/msg/Strings".try_into().unwrap()).unwrap(); + let strings_metadata = + DynamicMessageMetadata::new("test_msgs/msg/Strings".try_into().unwrap()).unwrap(); let strings_structure = Box::new(strings_metadata.structure().clone()); - let wstrings_metadata = DynamicMessageMetadata::new("test_msgs/msg/WStrings".try_into().unwrap()).unwrap(); + let wstrings_metadata = + DynamicMessageMetadata::new("test_msgs/msg/WStrings".try_into().unwrap()).unwrap(); let wstrings_structure = Box::new(wstrings_metadata.structure().clone()); let unbounded_sequences_metadata = - DynamicMessageMetadata::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()).unwrap(); + DynamicMessageMetadata::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()) + .unwrap(); let unbounded_sequences_structure = Box::new(unbounded_sequences_metadata.structure().clone()); @@ -1123,12 +1136,13 @@ mod tests { ); let _dyn_msg = DynamicMessage::new("test_msgs/msg/Arrays".try_into().unwrap()).unwrap(); - let _dyn_msg = DynamicMessage::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()).unwrap(); - let _dyn_msg = DynamicMessage::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); + let _dyn_msg = + DynamicMessage::new("test_msgs/msg/UnboundedSequences".try_into().unwrap()).unwrap(); + let _dyn_msg = + DynamicMessage::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); } } - // #[test] // fn test_mut_value_same_as_value() { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index f02df3585..b0b7bc322 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -12,9 +12,9 @@ pub use self::builder::*; pub use self::graph::*; ======= */ -use crate::QoSProfile; #[cfg(feature = "dyn_msg")] use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; +use crate::QoSProfile; pub use graph::*; @@ -42,12 +42,15 @@ use async_std::future::timeout; use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, + dynamic_message::{MessageTypeName, NodeDynamicSubscriptionCallback}, + rcl_bindings::*, + Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, - IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, dynamic_message::{NodeDynamicSubscriptionCallback, MessageTypeName}, - ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, MessageInfo, - Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState, - TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX, + IntoNodeSubscriptionCallback, LogParams, Logger, MessageInfo, ParameterBuilder, + ParameterInterface, ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, + PublisherState, RclrsError, Service, ServiceOptions, ServiceState, Subscription, + SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, Worker, WorkerOptions, + WorkerState, ENTITY_LIFECYCLE_MUTEX, }; /// A processing unit that can communicate with other nodes. See the API of @@ -812,7 +815,12 @@ impl NodeState { F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + 'static, { let subscription = DynamicSubscription::new( - topic, topic_type, qos, NodeDynamicSubscriptionCallback(Box::new(callback)), &self.handle, self.commands.async_worker_commands(), + topic, + topic_type, + qos, + NodeDynamicSubscriptionCallback(Box::new(callback)), + &self.handle, + self.commands.async_worker_commands(), )?; // TODO(luca) similar API to above? Ok(subscription) From 8c216afb43e543ca5b8335f20e2c282716d52831 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 14:17:50 +0800 Subject: [PATCH 13/33] Minor cleanup Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/dynamic_publisher.rs | 2 +- .../dynamic_message/dynamic_subscription.rs | 18 ++++-------------- rclrs/src/node.rs | 10 +--------- rclrs/src/subscription.rs | 3 --- 4 files changed, 6 insertions(+), 27 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 238014289..d34e05909 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -51,7 +51,7 @@ impl DynamicPublisher { /// Creates a new `DynamicPublisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub fn new( + pub(crate) fn new( node_handle: &Arc, topic: &str, topic_type: MessageTypeName, diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index bbac66490..533a231f3 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -2,7 +2,6 @@ use std::any::Any; use std::boxed::Box; use std::ffi::{CStr, CString}; use std::ops::{Deref, DerefMut}; -use std::sync::atomic::AtomicBool; use std::sync::{Arc, Mutex}; use futures::future::BoxFuture; @@ -13,9 +12,9 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ - MessageInfo, Node, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, - RclReturnCode, RclrsError, SubscriptionHandle, ToResult, Waitable, WaitableLifecycle, - WorkScope, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, + MessageInfo, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, + RclrsError, SubscriptionHandle, ToResult, Waitable, WaitableLifecycle, WorkScope, + WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; struct DynamicSubscriptionExecutable { @@ -85,7 +84,7 @@ impl From> } impl DynamicSubscriptionCallback { - pub(super) fn execute( + fn execute( &mut self, executable: &DynamicSubscriptionExecutable, any_payload: &mut dyn Any, @@ -266,15 +265,6 @@ where metadata, type_support_library, })) - - /* - Ok(Self { - handle, - callback: Mutex::new(Box::new(callback)), - metadata, - type_support_library, - }) - */ } /// Returns the topic name of the subscription. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b0b7bc322..aefadfbd1 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -5,13 +5,6 @@ mod primitive_options; pub use primitive_options::*; mod graph; -/* -<<<<<<< HEAD - -pub use self::builder::*; -pub use self::graph::*; -======= -*/ #[cfg(feature = "dyn_msg")] use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; use crate::QoSProfile; @@ -27,9 +20,8 @@ use std::{ ffi::CStr, fmt, os::raw::c_char, - sync::{atomic::AtomicBool, Arc, Mutex, Weak}, + sync::{atomic::AtomicBool, Arc, Mutex}, time::Duration, - vec::Vec, }; use futures::{ diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 2283dcb9b..3414d7b6e 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -12,9 +12,6 @@ use crate::{ WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; -#[cfg(feature = "dyn_msg")] -use crate::dynamic_message::DynamicMessage; - mod any_subscription_callback; pub use any_subscription_callback::*; From 9ab136b212600b2f81340398dd0d25063d55c812 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 14:35:38 +0800 Subject: [PATCH 14/33] Use SubscriptionOptions Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 10 ++--- rclrs/src/node.rs | 37 ++----------------- 2 files changed, 8 insertions(+), 39 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 533a231f3..e7cd3b8ae 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -12,9 +12,9 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ - MessageInfo, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, + MessageInfo, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, SubscriptionHandle, ToResult, Waitable, WaitableLifecycle, WorkScope, - WorkerCommands, ENTITY_LIFECYCLE_MUTEX, + WorkerCommands, ENTITY_LIFECYCLE_MUTEX, SubscriptionOptions, }; struct DynamicSubscriptionExecutable { @@ -181,10 +181,9 @@ where /// Creates a new dynamic subscription. /// /// This is not a public function, by the same rationale as `Subscription::new()`. - pub(crate) fn new( - topic: &str, + pub(crate) fn new<'a>( topic_type: MessageTypeName, - qos: QoSProfile, + options: impl Into>, callback: impl Into>, node_handle: &Arc, commands: &Arc, @@ -192,6 +191,7 @@ where // TODO(luca) a lot of duplication with nomral, refactor // This loads the introspection type support library. let metadata = DynamicMessageMetadata::new(topic_type)?; + let SubscriptionOptions { topic, qos } = options.into(); // However, we also need the regular type support library – // the rosidl_typesupport_c one. let message_type = &metadata.message_type; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index aefadfbd1..75114427f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -7,7 +7,6 @@ pub use primitive_options::*; mod graph; #[cfg(feature = "dyn_msg")] use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; -use crate::QoSProfile; pub use graph::*; @@ -793,23 +792,19 @@ impl NodeState { ) } - // TODO(luca) introduce subscription options and perhaps an into callback #[cfg(feature = "dyn_msg")] - pub fn create_dynamic_subscription( + pub fn create_dynamic_subscription<'a, F>( &self, - // options: impl Into>, - topic: &str, topic_type: MessageTypeName, - qos: QoSProfile, + options: impl Into>, callback: F, ) -> Result>, RclrsError> where F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + 'static, { let subscription = DynamicSubscription::new( - topic, topic_type, - qos, + options, NodeDynamicSubscriptionCallback(Box::new(callback)), &self.handle, self.commands.async_worker_commands(), @@ -818,32 +813,6 @@ impl NodeState { Ok(subscription) } - /// Creates a [`DynamicSubscription`][1]. - /// - /// [1]: crate::dynamic_message::DynamicSubscription - // TODO: make subscription's lifetime depend on node's lifetime - // TODO(luca) convert to subscription options - /* - #[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) - } - */ - /// Creates a [`Subscription`] with an async callback. /// /// # Behavior From a6b5b48a0adba100422c964979411dd04fedc562 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 14:59:32 +0800 Subject: [PATCH 15/33] Plumbing for dynamic publishers Signed-off-by: Luca Della Vedova --- .../src/dynamic_message/dynamic_publisher.rs | 75 ++++++++----------- rclrs/src/publisher.rs | 56 ++++++++------ 2 files changed, 67 insertions(+), 64 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index d34e05909..3342168ab 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -1,4 +1,3 @@ -use std::ffi::CStr; use std::ffi::CString; use std::sync::{Arc, Mutex}; @@ -9,7 +8,7 @@ use super::{ use crate::error::{RclrsError, ToResult}; use crate::qos::QoSProfile; use crate::rcl_bindings::*; -use crate::NodeHandle; +use crate::{ENTITY_LIFECYCLE_MUTEX, PublisherHandle, NodeHandle}; /// Struct for sending messages of type `T`. /// @@ -22,24 +21,13 @@ use crate::NodeHandle; /// /// [1]: crate::spin pub struct DynamicPublisher { - rcl_publisher_mtx: Mutex, - node_handle: Arc, + handle: PublisherHandle, 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) { - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - unsafe { - // SAFETY: No preconditions for this function (besides the arguments being valid). - rcl_publisher_fini(self.rcl_publisher_mtx.get_mut().unwrap(), &mut *rcl_node); - } - } -} - // 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 {} @@ -52,7 +40,7 @@ impl DynamicPublisher { /// /// Node and namespace changes are always applied _before_ topic remapping. pub(crate) fn new( - node_handle: &Arc, + node_handle: Arc, topic: &str, topic_type: MessageTypeName, qos: QoSProfile, @@ -81,30 +69,36 @@ impl DynamicPublisher { err, s: topic.into(), })?; - let rcl_node = node_handle.rcl_node.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()?; + + { + let rcl_node = node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: The rcl_publisher is zero-initialized as expected by this function. + // The rcl_node is kept alive because it is co-owned by the subscription. + // The topic name and the options are copied by this function, so they can be dropped + // afterwards. + // TODO: type support? + rcl_publisher_init( + &mut rcl_publisher, + &*rcl_node, + type_support_ptr, + topic_c_string.as_ptr(), + &publisher_options, + ) + .ok()?; + } } Ok(Self { - rcl_publisher_mtx: Mutex::new(rcl_publisher), - node_handle: node_handle.clone(), + handle: PublisherHandle { + rcl_publisher: Mutex::new(rcl_publisher), + node_handle, + }, metadata, type_support_library, }) @@ -115,15 +109,12 @@ impl DynamicPublisher { /// 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() - } + self.handle.topic_name() + } + + /// Returns the number of subscriptions of the publisher. + pub fn get_subscription_count(&self) -> Result { + self.handle.get_subscription_count() } /// Publishes a message. @@ -135,7 +126,7 @@ impl DynamicPublisher { if message.metadata.message_type != self.metadata.message_type { return Err(DynamicMessageError::MessageTypeMismatch.into()); } - let rcl_publisher = &mut *self.rcl_publisher_mtx.lock().unwrap(); + let rcl_publisher = &mut *self.handle.rcl_publisher.lock().unwrap(); unsafe { // SAFETY: The message type is guaranteed to match the publisher type by the type system. // The message does not need to be valid beyond the duration of this function call. diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 29a7e1579..8439420b8 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -26,9 +26,9 @@ unsafe impl Send for rcl_publisher_t {} /// [dropped after][1] the `rcl_publisher_t`. /// /// [1]: -struct PublisherHandle { - rcl_publisher: Mutex, - node_handle: Arc, +pub(crate) struct PublisherHandle { + pub(crate) rcl_publisher: Mutex, + pub(crate) node_handle: Arc, } impl Drop for PublisherHandle { @@ -43,6 +43,35 @@ impl Drop for PublisherHandle { } } +// Functions shared between normal and dynamic publishers +impl PublisherHandle { + pub(crate) fn topic_name(&self) -> String { + // SAFETY: No preconditions for the functions called. + // The unsafe variables created get converted to safe types before being returned + unsafe { + let raw_topic_pointer = + rcl_publisher_get_topic_name(&*self.rcl_publisher.lock().unwrap()); + CStr::from_ptr(raw_topic_pointer) + .to_string_lossy() + .into_owned() + } + } + + /// Returns the number of subscriptions of the publisher. + pub(crate) fn get_subscription_count(&self) -> Result { + let mut subscription_count = 0; + // SAFETY: No preconditions for the function called. + unsafe { + rcl_publisher_get_subscription_count( + &*self.rcl_publisher.lock().unwrap(), + &mut subscription_count, + ) + .ok()? + }; + Ok(subscription_count) + } +} + /// Struct for sending messages of type `T`. /// /// Create a publisher using [`Node::create_publisher`][1]. @@ -152,29 +181,12 @@ where /// This returns the topic name after remapping, so it is not necessarily the /// topic name which was used when creating the publisher. pub fn topic_name(&self) -> String { - // SAFETY: No preconditions for the functions called. - // The unsafe variables created get converted to safe types before being returned - unsafe { - let raw_topic_pointer = - rcl_publisher_get_topic_name(&*self.handle.rcl_publisher.lock().unwrap()); - CStr::from_ptr(raw_topic_pointer) - .to_string_lossy() - .into_owned() - } + self.handle.topic_name() } /// Returns the number of subscriptions of the publisher. pub fn get_subscription_count(&self) -> Result { - let mut subscription_count = 0; - // SAFETY: No preconditions for the function called. - unsafe { - rcl_publisher_get_subscription_count( - &*self.handle.rcl_publisher.lock().unwrap(), - &mut subscription_count, - ) - .ok()? - }; - Ok(subscription_count) + self.handle.get_subscription_count() } /// Publishes a message. From 8112a77443b424f84bb6d1a51d6a5b7b4a301e00 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 15:10:12 +0800 Subject: [PATCH 16/33] Add publisher API Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/dynamic_publisher.rs | 18 +++++------------- .../dynamic_message/dynamic_subscription.rs | 6 +++--- rclrs/src/node.rs | 11 ++++++++++- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 3342168ab..05aabc7fe 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -6,9 +6,8 @@ use super::{ DynamicMessageMetadata, MessageTypeName, }; use crate::error::{RclrsError, ToResult}; -use crate::qos::QoSProfile; use crate::rcl_bindings::*; -use crate::{ENTITY_LIFECYCLE_MUTEX, PublisherHandle, NodeHandle}; +use crate::{NodeHandle, PublisherHandle, PublisherOptions, ENTITY_LIFECYCLE_MUTEX}; /// Struct for sending messages of type `T`. /// @@ -28,25 +27,18 @@ pub struct DynamicPublisher { type_support_library: Arc, } -// 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(crate) fn new( - node_handle: Arc, - topic: &str, + pub(crate) fn new<'a>( topic_type: MessageTypeName, - qos: QoSProfile, + options: impl Into>, + node_handle: Arc, ) -> Result { // This loads the introspection type support library. let metadata = DynamicMessageMetadata::new(topic_type)?; + let PublisherOptions { topic, qos } = options.into(); // However, we also need the regular type support library – // the rosidl_typesupport_c one. let message_type = &metadata.message_type; diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index e7cd3b8ae..4d48d2a1a 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -12,9 +12,9 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ - MessageInfo, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, - RclrsError, SubscriptionHandle, ToResult, Waitable, WaitableLifecycle, WorkScope, - WorkerCommands, ENTITY_LIFECYCLE_MUTEX, SubscriptionOptions, + MessageInfo, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, + SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, WaitableLifecycle, WorkScope, + WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; struct DynamicSubscriptionExecutable { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 75114427f..c860e4887 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -6,7 +6,7 @@ pub use primitive_options::*; mod graph; #[cfg(feature = "dyn_msg")] -use crate::dynamic_message::{DynamicMessage, DynamicSubscription}; +use crate::dynamic_message::{DynamicMessage, DynamicPublisher, DynamicSubscription}; pub use graph::*; @@ -401,6 +401,15 @@ impl NodeState { PublisherState::::create(options, Arc::clone(&self.handle)) } + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_publisher<'a>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + ) -> Result { + DynamicPublisher::new(topic_type, options, Arc::clone(&self.handle)) + } + /// Creates a [`Service`] with an ordinary callback. /// /// # Behavior From 73022f704f58fa0307c5e2a39864cbb1c6152982 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 15:28:44 +0800 Subject: [PATCH 17/33] API symmetry with normal pub/sub Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/dynamic_publisher.rs | 16 +++++++++------- .../src/dynamic_message/dynamic_subscription.rs | 12 ++++++------ rclrs/src/node.rs | 16 ++++++++-------- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 05aabc7fe..d73fa25d6 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -9,6 +9,8 @@ use crate::error::{RclrsError, ToResult}; use crate::rcl_bindings::*; use crate::{NodeHandle, PublisherHandle, PublisherOptions, ENTITY_LIFECYCLE_MUTEX}; +pub type DynamicPublisher = Arc; + /// Struct for sending messages of type `T`. /// /// Multiple publishers can be created for the same topic, in different nodes or the same node. @@ -19,7 +21,7 @@ use crate::{NodeHandle, PublisherHandle, PublisherOptions, ENTITY_LIFECYCLE_MUTE /// Sending messages does not require calling [`spin`][1] on the publisher's node. /// /// [1]: crate::spin -pub struct DynamicPublisher { +pub struct DynamicPublisherState { handle: PublisherHandle, metadata: DynamicMessageMetadata, // This is the regular type support library, not the introspection one. @@ -27,15 +29,15 @@ pub struct DynamicPublisher { type_support_library: Arc, } -impl DynamicPublisher { - /// Creates a new `DynamicPublisher`. +impl DynamicPublisherState { + /// Creates a new `DynamicPublisherState`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub(crate) fn new<'a>( + pub(crate) fn create<'a>( topic_type: MessageTypeName, options: impl Into>, node_handle: Arc, - ) -> Result { + ) -> Result, RclrsError> { // This loads the introspection type support library. let metadata = DynamicMessageMetadata::new(topic_type)?; let PublisherOptions { topic, qos } = options.into(); @@ -86,14 +88,14 @@ impl DynamicPublisher { } } - Ok(Self { + Ok(Arc::new(Self { handle: PublisherHandle { rcl_publisher: Mutex::new(rcl_publisher), node_handle, }, metadata, type_support_library, - }) + })) } /// Returns the topic name of the publisher. diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 4d48d2a1a..a70cb99e5 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -12,11 +12,13 @@ use super::{ }; use crate::rcl_bindings::*; use crate::{ - MessageInfo, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, + MessageInfo, Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, WaitableLifecycle, WorkScope, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; +pub type DynamicSubscription = Arc>; + struct DynamicSubscriptionExecutable { handle: Arc, callback: Arc>>, @@ -153,7 +155,7 @@ impl RclPrimitive for DynamicSubscriptionExecutable { } /// Struct for receiving messages whose type is only known at runtime. -pub struct DynamicSubscription +pub struct DynamicSubscriptionState where Scope: WorkScope, { @@ -174,14 +176,14 @@ where type_support_library: Arc, } -impl DynamicSubscription +impl DynamicSubscriptionState where Scope: WorkScope, { /// Creates a new dynamic subscription. /// /// This is not a public function, by the same rationale as `Subscription::new()`. - pub(crate) fn new<'a>( + pub(crate) fn create<'a>( topic_type: MessageTypeName, options: impl Into>, callback: impl Into>, @@ -336,11 +338,9 @@ mod tests { fn assert_send() {} fn assert_sync() {} - /* #[test] fn dynamic_subscription_is_sync_and_send() { assert_send::(); assert_sync::(); } - */ } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index c860e4887..461dd3db5 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -6,7 +6,10 @@ pub use primitive_options::*; mod graph; #[cfg(feature = "dyn_msg")] -use crate::dynamic_message::{DynamicMessage, DynamicPublisher, DynamicSubscription}; +use crate::dynamic_message::{ + DynamicMessage, DynamicPublisher, DynamicPublisherState, DynamicSubscription, + DynamicSubscriptionState, MessageTypeName, NodeDynamicSubscriptionCallback, +}; pub use graph::*; @@ -33,9 +36,7 @@ use async_std::future::timeout; use rosidl_runtime_rs::Message; use crate::{ - dynamic_message::{MessageTypeName, NodeDynamicSubscriptionCallback}, - rcl_bindings::*, - Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, + rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, IntoNodeSubscriptionCallback, LogParams, Logger, MessageInfo, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, @@ -407,7 +408,7 @@ impl NodeState { topic_type: MessageTypeName, options: impl Into>, ) -> Result { - DynamicPublisher::new(topic_type, options, Arc::clone(&self.handle)) + DynamicPublisherState::create(topic_type, options, Arc::clone(&self.handle)) } /// Creates a [`Service`] with an ordinary callback. @@ -807,18 +808,17 @@ impl NodeState { topic_type: MessageTypeName, options: impl Into>, callback: F, - ) -> Result>, RclrsError> + ) -> Result where F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + 'static, { - let subscription = DynamicSubscription::new( + let subscription = DynamicSubscriptionState::::create( topic_type, options, NodeDynamicSubscriptionCallback(Box::new(callback)), &self.handle, self.commands.async_worker_commands(), )?; - // TODO(luca) similar API to above? Ok(subscription) } From 2ccec54f42afff9349a06c9d4f1a189919175399 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 20 Jun 2025 15:36:33 +0800 Subject: [PATCH 18/33] Worker subscriptions Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 4 ++- rclrs/src/node.rs | 5 ++-- rclrs/src/worker.rs | 28 +++++++++++++++++-- 3 files changed, 31 insertions(+), 6 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index a70cb99e5..b02c5db19 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -14,11 +14,13 @@ use crate::rcl_bindings::*; use crate::{ MessageInfo, Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, WaitableLifecycle, WorkScope, - WorkerCommands, ENTITY_LIFECYCLE_MUTEX, + Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; pub type DynamicSubscription = Arc>; +pub type WorkerDynamicSubscription = Arc>>; + struct DynamicSubscriptionExecutable { handle: Arc, callback: Arc>>, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 461dd3db5..2745cb1a1 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -812,14 +812,13 @@ impl NodeState { where F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + 'static, { - let subscription = DynamicSubscriptionState::::create( + DynamicSubscriptionState::::create( topic_type, options, NodeDynamicSubscriptionCallback(Box::new(callback)), &self.handle, self.commands.async_worker_commands(), - )?; - Ok(subscription) + ) } /// Creates a [`Subscription`] with an async callback. diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index f89da20fc..219ad24a8 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -1,6 +1,11 @@ +#[cfg(feature = "dyn_msg")] +use crate::dynamic_message::{ + DynamicMessage, DynamicSubscriptionState, MessageTypeName, WorkerDynamicSubscription, + WorkerDynamicSubscriptionCallback, +}; use crate::{ - log_fatal, IntoWorkerServiceCallback, IntoWorkerSubscriptionCallback, Node, Promise, - RclrsError, ServiceOptions, ServiceState, SubscriptionOptions, SubscriptionState, + log_fatal, IntoWorkerServiceCallback, IntoWorkerSubscriptionCallback, MessageInfo, Node, + Promise, RclrsError, ServiceOptions, ServiceState, SubscriptionOptions, SubscriptionState, WorkerCommands, WorkerService, WorkerSubscription, }; use futures::channel::oneshot; @@ -264,6 +269,25 @@ impl WorkerState { ) } + #[cfg(feature = "dyn_msg")] + pub fn create_dynamic_subscription<'a, F>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + callback: F, + ) -> Result, RclrsError> + where + F: FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send + 'static, + { + DynamicSubscriptionState::>::create( + topic_type, + options, + WorkerDynamicSubscriptionCallback(Box::new(callback)), + self.node.handle(), + &self.commands, + ) + } + /// Creates a [`WorkerService`]. /// /// Unlike services created from a [`Node`], the callbacks for these services From 60a542a01131a8472dcb1ca038e9631e4303359b Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 09:37:12 +0800 Subject: [PATCH 19/33] Add async subscription API Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 112 ++++++++++++++++-- rclrs/src/node.rs | 24 +++- rclrs/src/worker.rs | 2 +- 3 files changed, 127 insertions(+), 11 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index b02c5db19..6229e9f89 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -31,14 +31,25 @@ struct DynamicSubscriptionExecutable { // TODO(luca) consider making these enums if we want different callback types // TODO(luca) make fields private pub struct NodeDynamicSubscriptionCallback( - pub Box BoxFuture<'static, ()> + Send>, + pub Box, +); +pub struct NodeAsyncDynamicSubscriptionCallback( + pub Box BoxFuture<'static, ()> + Send + Sync>, ); pub struct WorkerDynamicSubscriptionCallback( - pub Box, + pub Box, ); impl Deref for NodeDynamicSubscriptionCallback { - type Target = Box BoxFuture<'static, ()> + Send>; + type Target = Box; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl Deref for NodeAsyncDynamicSubscriptionCallback { + type Target = + Box BoxFuture<'static, ()> + Send + Sync>; fn deref(&self) -> &Self::Target { &self.0 } @@ -50,8 +61,14 @@ impl DerefMut for NodeDynamicSubscriptionCallback { } } +impl DerefMut for NodeAsyncDynamicSubscriptionCallback { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + impl Deref for WorkerDynamicSubscriptionCallback { - type Target = Box; + type Target = Box; fn deref(&self) -> &Self::Target { &self.0 } @@ -63,18 +80,29 @@ impl DerefMut for WorkerDynamicSubscriptionCallback { } } -//pub trait NodeDynamicSubscriptionCallback = FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send; -//pub trait WorkerDynamicSubscriptionCallback = FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send; - pub enum DynamicSubscriptionCallback { /// A callback with the message and the message info as arguments. - Node(NodeDynamicSubscriptionCallback), + Node(NodeAsyncDynamicSubscriptionCallback), /// A callback with the payload, message, and the message info as arguments. Worker(WorkerDynamicSubscriptionCallback), } impl From for DynamicSubscriptionCallback<()> { fn from(value: NodeDynamicSubscriptionCallback) -> Self { + let func = Arc::new(value); + DynamicSubscriptionCallback::Node(NodeAsyncDynamicSubscriptionCallback(Box::new( + move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + }, + ))) + } +} + +impl From for DynamicSubscriptionCallback<()> { + fn from(value: NodeAsyncDynamicSubscriptionCallback) -> Self { DynamicSubscriptionCallback::Node(value) } } @@ -336,6 +364,8 @@ where #[cfg(test)] mod tests { use super::*; + use crate::test_helpers::*; + use test_msgs::msg; fn assert_send() {} fn assert_sync() {} @@ -345,4 +375,70 @@ mod tests { assert_send::(); assert_sync::(); } + + #[test] + fn test_dynamic_subscriptions() -> Result<(), RclrsError> { + use crate::TopicEndpointInfo; + + let namespace = "/test_dynamic_subscriptions_graph"; + let graph = construct_test_graph(namespace)?; + + let node_2_empty_subscription = graph + .node2 + .create_dynamic_subscription::<_>("test_msgs/msg/Empty".try_into().unwrap(), "graph_test_topic_1", |_, _| {})?; + let topic1 = node_2_empty_subscription.topic_name(); + /* + let node_2_basic_types_subscription = + graph.node2.create_subscription::( + "graph_test_topic_2", + |_msg: msg::BasicTypes| {}, + )?; + let topic2 = node_2_basic_types_subscription.topic_name(); + let node_1_defaults_subscription = graph.node1.create_subscription::( + "graph_test_topic_3", + |_msg: msg::Defaults| {}, + )?; + let topic3 = node_1_defaults_subscription.topic_name(); + + std::thread::sleep(std::time::Duration::from_millis(100)); + + // Test count_subscriptions() + assert_eq!(graph.node2.count_subscriptions(&topic1)?, 1); + assert_eq!(graph.node2.count_subscriptions(&topic2)?, 1); + + // Test get_subscription_names_and_types_by_node() + let node_1_subscription_names_and_types = graph + .node1 + .get_subscription_names_and_types_by_node(&graph.node1.name(), namespace)?; + + let types = node_1_subscription_names_and_types.get(&topic3).unwrap(); + assert!(types.contains(&"test_msgs/msg/Defaults".to_string())); + + let node_2_subscription_names_and_types = graph + .node2 + .get_subscription_names_and_types_by_node(&graph.node2.name(), namespace)?; + + let types = node_2_subscription_names_and_types.get(&topic1).unwrap(); + assert!(types.contains(&"test_msgs/msg/Empty".to_string())); + + let types = node_2_subscription_names_and_types.get(&topic2).unwrap(); + assert!(types.contains(&"test_msgs/msg/BasicTypes".to_string())); + + // Test get_subscriptions_info_by_topic() + let expected_subscriptions_info = vec![TopicEndpointInfo { + node_name: String::from("graph_test_node_2"), + node_namespace: String::from(namespace), + topic_type: String::from("test_msgs/msg/Empty"), + }]; + assert_eq!( + graph.node1.get_subscriptions_info_by_topic(&topic1)?, + expected_subscriptions_info + ); + assert_eq!( + graph.node2.get_subscriptions_info_by_topic(&topic1)?, + expected_subscriptions_info + ); + */ + Ok(()) + } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 2745cb1a1..29b7a4523 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -8,7 +8,8 @@ mod graph; #[cfg(feature = "dyn_msg")] use crate::dynamic_message::{ DynamicMessage, DynamicPublisher, DynamicPublisherState, DynamicSubscription, - DynamicSubscriptionState, MessageTypeName, NodeDynamicSubscriptionCallback, + DynamicSubscriptionState, MessageTypeName, NodeAsyncDynamicSubscriptionCallback, + NodeDynamicSubscriptionCallback, }; pub use graph::*; @@ -810,7 +811,7 @@ impl NodeState { callback: F, ) -> Result where - F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + 'static, + F: Fn(DynamicMessage, MessageInfo) + Send + Sync + 'static, { DynamicSubscriptionState::::create( topic_type, @@ -821,6 +822,25 @@ impl NodeState { ) } + #[cfg(feature = "dyn_msg")] + pub fn create_async_dynamic_subscription<'a, F>( + &self, + topic_type: MessageTypeName, + options: impl Into>, + callback: F, + ) -> Result + where + F: FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + Sync + 'static, + { + DynamicSubscriptionState::::create( + topic_type, + options, + NodeAsyncDynamicSubscriptionCallback(Box::new(callback)), + &self.handle, + self.commands.async_worker_commands(), + ) + } + /// Creates a [`Subscription`] with an async callback. /// /// # Behavior diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index 219ad24a8..4ed674652 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -277,7 +277,7 @@ impl WorkerState { callback: F, ) -> Result, RclrsError> where - F: FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send + 'static, + F: FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send + Sync + 'static, { DynamicSubscriptionState::>::create( topic_type, From 98381ac2a29ce0ced1822094e8bb510e26f5fd02 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 09:42:08 +0800 Subject: [PATCH 20/33] Basic test for dynamic subscriptions graph Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 6229e9f89..fa2f9793c 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -383,20 +383,24 @@ mod tests { let namespace = "/test_dynamic_subscriptions_graph"; let graph = construct_test_graph(namespace)?; - let node_2_empty_subscription = graph - .node2 - .create_dynamic_subscription::<_>("test_msgs/msg/Empty".try_into().unwrap(), "graph_test_topic_1", |_, _| {})?; + let node_2_empty_subscription = graph.node2.create_dynamic_subscription::<_>( + "test_msgs/msg/Empty".try_into().unwrap(), + "graph_test_topic_1", + |_, _| {}, + )?; let topic1 = node_2_empty_subscription.topic_name(); - /* let node_2_basic_types_subscription = - graph.node2.create_subscription::( + graph.node2.create_dynamic_subscription::<_>( + "test_msgs/msg/BasicTypes".try_into().unwrap(), "graph_test_topic_2", - |_msg: msg::BasicTypes| {}, + |_, _| {}, )?; let topic2 = node_2_basic_types_subscription.topic_name(); - let node_1_defaults_subscription = graph.node1.create_subscription::( + + let node_1_defaults_subscription = graph.node1.create_dynamic_subscription::<_>( + "test_msgs/msg/Defaults".try_into().unwrap(), "graph_test_topic_3", - |_msg: msg::Defaults| {}, + |_, _| {}, )?; let topic3 = node_1_defaults_subscription.topic_name(); @@ -438,7 +442,6 @@ mod tests { graph.node2.get_subscriptions_info_by_topic(&topic1)?, expected_subscriptions_info ); - */ Ok(()) } } From 164639efd9a61f1e684dab9fbf7e96d0f0b8c859 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 09:51:18 +0800 Subject: [PATCH 21/33] Tests for dynamic pub/sub Signed-off-by: Luca Della Vedova --- .../src/dynamic_message/dynamic_publisher.rs | 106 ++++++++++++++++++ .../dynamic_message/dynamic_subscription.rs | 12 +- 2 files changed, 111 insertions(+), 7 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index d73fa25d6..5bce6a0e7 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -138,6 +138,7 @@ impl DynamicPublisherState { #[cfg(test)] mod tests { use super::*; + use crate::test_helpers::*; fn assert_send() {} fn assert_sync() {} @@ -147,4 +148,109 @@ mod tests { assert_send::(); assert_sync::(); } + + #[test] + fn test_dynamic_publishers() -> Result<(), RclrsError> { + use crate::TopicEndpointInfo; + use test_msgs::msg; + + let namespace = "/test_dynamic_publishers_graph"; + let graph = construct_test_graph(namespace)?; + + let node_1_empty_publisher = graph.node1.create_dynamic_publisher( + "test_msgs/msg/Empty".try_into().unwrap(), + "graph_test_topic_1", + )?; + let topic1 = node_1_empty_publisher.topic_name(); + let node_1_basic_types_publisher = graph.node1.create_dynamic_publisher( + "test_msgs/msg/BasicTypes".try_into().unwrap(), + "graph_test_topic_2", + )?; + let topic2 = node_1_basic_types_publisher.topic_name(); + let node_2_default_publisher = graph.node2.create_dynamic_publisher( + "test_msgs/msg/Defaults".try_into().unwrap(), + "graph_test_topic_3", + )?; + let topic3 = node_2_default_publisher.topic_name(); + + std::thread::sleep(std::time::Duration::from_millis(100)); + + // Test count_publishers() + assert_eq!(graph.node1.count_publishers(&topic1)?, 1); + assert_eq!(graph.node1.count_publishers(&topic2)?, 1); + assert_eq!(graph.node1.count_publishers(&topic3)?, 1); + + // Test get_publisher_names_and_types_by_node() + let node_1_publisher_names_and_types = graph + .node1 + .get_publisher_names_and_types_by_node(&graph.node1.name(), namespace)?; + + let types = node_1_publisher_names_and_types.get(&topic1).unwrap(); + assert!(types.contains(&"test_msgs/msg/Empty".to_string())); + + let types = node_1_publisher_names_and_types.get(&topic2).unwrap(); + assert!(types.contains(&"test_msgs/msg/BasicTypes".to_string())); + + let node_2_publisher_names_and_types = graph + .node1 + .get_publisher_names_and_types_by_node(&graph.node2.name(), namespace)?; + + let types = node_2_publisher_names_and_types.get(&topic3).unwrap(); + assert!(types.contains(&"test_msgs/msg/Defaults".to_string())); + + // Test get_publishers_info_by_topic() + let expected_publishers_info = vec![TopicEndpointInfo { + node_name: String::from("graph_test_node_1"), + node_namespace: String::from(namespace), + topic_type: String::from("test_msgs/msg/Empty"), + }]; + assert_eq!( + graph.node1.get_publishers_info_by_topic(&topic1)?, + expected_publishers_info + ); + assert_eq!( + graph.node2.get_publishers_info_by_topic(&topic1)?, + expected_publishers_info + ); + + // Test get_subscription_count() + assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(0)); + assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(0)); + assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(0)); + // Test subscription with static types + let _node_1_empty_subscriber = graph + .node1 + .create_subscription("graph_test_topic_1", |_msg: msg::Empty| {}); + let _node_1_basic_types_subscriber = graph + .node1 + .create_subscription("graph_test_topic_2", |_msg: msg::BasicTypes| {}); + let _node_2_default_subscriber = graph + .node2 + .create_subscription("graph_test_topic_3", |_msg: msg::Defaults| {}); + assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(1)); + assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(1)); + assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(1)); + + // Test subscription with dynamic types + let _node_1_empty_subscriber = graph.node1.create_dynamic_subscription( + "test_msgs/msg/Empty".try_into().unwrap(), + "graph_test_topic_1", + |_, _| {}, + ); + let _node_1_basic_types_subscriber = graph.node1.create_dynamic_subscription( + "test_msgs/msg/BasicTypes".try_into().unwrap(), + "graph_test_topic_2", + |_, _| {}, + ); + let _node_2_default_subscriber = graph.node2.create_dynamic_subscription( + "test_msgs/msg/Defaults".try_into().unwrap(), + "graph_test_topic_3", + |_, _| {}, + ); + assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(2)); + assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(2)); + assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(2)); + + Ok(()) + } } diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index fa2f9793c..b06659b80 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -365,7 +365,6 @@ where mod tests { use super::*; use crate::test_helpers::*; - use test_msgs::msg; fn assert_send() {} fn assert_sync() {} @@ -389,12 +388,11 @@ mod tests { |_, _| {}, )?; let topic1 = node_2_empty_subscription.topic_name(); - let node_2_basic_types_subscription = - graph.node2.create_dynamic_subscription::<_>( - "test_msgs/msg/BasicTypes".try_into().unwrap(), - "graph_test_topic_2", - |_, _| {}, - )?; + let node_2_basic_types_subscription = graph.node2.create_dynamic_subscription::<_>( + "test_msgs/msg/BasicTypes".try_into().unwrap(), + "graph_test_topic_2", + |_, _| {}, + )?; let topic2 = node_2_basic_types_subscription.topic_name(); let node_1_defaults_subscription = graph.node1.create_dynamic_subscription::<_>( From d1c264a3bf7024af35aa20d1430372853d40ae40 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 10:01:21 +0800 Subject: [PATCH 22/33] Worker subscription test Signed-off-by: Luca Della Vedova --- rclrs/src/worker.rs | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index 4ed674652..18a1ff469 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -535,6 +535,7 @@ mod tests { struct TestPayload { subscription_count: usize, service_count: usize, + dynamic_subscription_count: usize, } #[test] @@ -549,6 +550,14 @@ mod tests { }, ); + let _count_dynamic_sub = worker.create_dynamic_subscription( + "test_msgs/msg/Empty".try_into().unwrap(), + "test_worker_topic", + |payload: &mut TestPayload, _, _| { + payload.dynamic_subscription_count += 1; + }, + ); + let _count_srv = worker.create_service::( "test_worker_service", |payload: &mut TestPayload, _req: Empty_Request| { @@ -558,7 +567,10 @@ mod tests { ); let promise = worker.listen_until(move |payload| { - if payload.service_count > 0 && payload.subscription_count > 0 { + if payload.service_count > 0 + && payload.subscription_count > 0 + && payload.dynamic_subscription_count > 0 + { Some(*payload) } else { None From 0c11ea0fd32411da2f0571f969a6b59e3b632358 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 10:47:56 +0800 Subject: [PATCH 23/33] Minor cleanups and refactors Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 88 +++++++------------ rclrs/src/node.rs | 4 +- rclrs/src/subscription.rs | 20 +++-- rclrs/src/worker.rs | 2 +- 4 files changed, 46 insertions(+), 68 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index b06659b80..2a5a958d4 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -28,18 +28,40 @@ struct DynamicSubscriptionExecutable { metadata: Arc, } -// TODO(luca) consider making these enums if we want different callback types -// TODO(luca) make fields private -pub struct NodeDynamicSubscriptionCallback( - pub Box, +pub(crate) struct NodeDynamicSubscriptionCallback( + Box, ); -pub struct NodeAsyncDynamicSubscriptionCallback( - pub Box BoxFuture<'static, ()> + Send + Sync>, + +impl NodeDynamicSubscriptionCallback { + pub(crate) fn new(f: impl Fn(DynamicMessage, MessageInfo) + Send + Sync + 'static) -> Self { + NodeDynamicSubscriptionCallback(Box::new(f)) + } +} + +pub(crate) struct NodeAsyncDynamicSubscriptionCallback( + Box BoxFuture<'static, ()> + Send + Sync>, ); -pub struct WorkerDynamicSubscriptionCallback( - pub Box, + +impl NodeAsyncDynamicSubscriptionCallback { + pub(crate) fn new( + f: impl FnMut(DynamicMessage, MessageInfo) -> BoxFuture<'static, ()> + Send + Sync + 'static, + ) -> Self { + NodeAsyncDynamicSubscriptionCallback(Box::new(f)) + } +} + +pub(crate) struct WorkerDynamicSubscriptionCallback( + Box, ); +impl WorkerDynamicSubscriptionCallback { + pub(crate) fn new( + f: impl FnMut(&mut Payload, DynamicMessage, MessageInfo) + Send + Sync + 'static, + ) -> Self { + WorkerDynamicSubscriptionCallback(Box::new(f)) + } +} + impl Deref for NodeDynamicSubscriptionCallback { type Target = Box; fn deref(&self) -> &Self::Target { @@ -304,61 +326,13 @@ where /// This returns the topic name after remapping, so it is not necessarily the /// topic name which was used when creating the subscription. pub fn topic_name(&self) -> String { - // SAFETY: 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() - } + self.handle.topic_name() } /// 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) - } } #[cfg(test)] diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 29b7a4523..89d3fde74 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -816,7 +816,7 @@ impl NodeState { DynamicSubscriptionState::::create( topic_type, options, - NodeDynamicSubscriptionCallback(Box::new(callback)), + NodeDynamicSubscriptionCallback::new(callback), &self.handle, self.commands.async_worker_commands(), ) @@ -835,7 +835,7 @@ impl NodeState { DynamicSubscriptionState::::create( topic_type, options, - NodeAsyncDynamicSubscriptionCallback(Box::new(callback)), + NodeAsyncDynamicSubscriptionCallback::new(callback), &self.handle, self.commands.async_worker_commands(), ) diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 3414d7b6e..92e9f7746 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -100,14 +100,7 @@ where /// This returns the topic name after remapping, so it is not necessarily the /// topic name which was used when creating the subscription. pub fn topic_name(&self) -> String { - // SAFETY: The subscription handle is valid because its lifecycle is managed by an Arc. - // The unsafe variables get converted to safe types before being returned - unsafe { - let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); - CStr::from_ptr(raw_topic_pointer) - } - .to_string_lossy() - .into_owned() + self.handle.topic_name() } /// Used by [`Node`][crate::Node] to create a new subscription. @@ -333,6 +326,17 @@ impl SubscriptionHandle { self.rcl_subscription.lock().unwrap() } + pub(crate) fn topic_name(&self) -> String { + // SAFETY: The subscription handle is valid because its lifecycle is managed by an Arc. + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.lock()); + CStr::from_ptr(raw_topic_pointer) + } + .to_string_lossy() + .into_owned() + } + /// Fetches a new message. /// /// When there is no new message, this will return a diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index 18a1ff469..e4d6d5a37 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -282,7 +282,7 @@ impl WorkerState { DynamicSubscriptionState::>::create( topic_type, options, - WorkerDynamicSubscriptionCallback(Box::new(callback)), + WorkerDynamicSubscriptionCallback::new(callback), self.node.handle(), &self.commands, ) From 9ab8dc56b372735dae7dd7269b7ee9813a420fb3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 11:49:30 +0800 Subject: [PATCH 24/33] Tests for message field getters / setters Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 39 ++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 0cd6e0822..fdfddf33d 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -522,4 +522,43 @@ mod tests { Err(DynamicMessageError::RequiredPrefixNotSourced { .. }) )); } + + #[test] + fn message_getters_setters() { + let message_type = MessageTypeName { + package_name: "test_msgs".to_owned(), + type_name: "BasicTypes".to_owned(), + }; + let mut message = DynamicMessage::new(message_type).unwrap(); + + { + // Access non existing values + assert!(message.get("invalid_value").is_none()); + } + + { + // Get then set a sample value + let value = message.get_mut("int32_value").unwrap(); + let ValueMut::Simple(value) = value else { + panic!("Unexpected value type, expected Simple value"); + }; + let SimpleValueMut::Int32(value) = value else { + panic!("Unexpected value type, expected Int32"); + }; + assert_eq!(*value, 0); + *value = 42; + } + + { + // Read previously set value + let value = message.get("int32_value").unwrap(); + let Value::Simple(value) = value else { + panic!("Unexpected value type, expected Simple value"); + }; + let SimpleValue::Int32(value) = value else { + panic!("Unexpected value type, expected Int32"); + }; + assert_eq!(*value, 42); + } + } } From 1439045ad9f773bcf9dfac23c0fc1285190cd31e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 12:00:21 +0800 Subject: [PATCH 25/33] Minor cleanups Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 18 +++++++----------- .../dynamic_message/dynamic_subscription.rs | 4 ++-- 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index fdfddf33d..3b6ea0e0f 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -70,7 +70,9 @@ pub struct DynamicMessageMetadata { introspection_type_support_library: Arc, type_support_ptr: *const rosidl_message_type_support_t, structure: MessageStructure, - fini_function: unsafe extern "C" fn(*mut std::os::raw::c_void), + // The message content can be moved out into another message, + // in which case the drop impl is not responsible for calling fini anymore + fini_function: Option, } /// A message whose type is not known at compile-time. @@ -85,9 +87,6 @@ pub struct DynamicMessage { // 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 ========================= @@ -201,8 +200,6 @@ impl DynamicMessagePackage { 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 { message_type, introspection_type_support_library: Arc::clone( @@ -210,7 +207,7 @@ impl DynamicMessagePackage { ), type_support_ptr, structure, - fini_function, + fini_function: message_members.fini_function, }; Ok(metadata) } @@ -314,7 +311,6 @@ impl DynamicMessageMetadata { let dyn_msg = DynamicMessage { metadata: self.clone(), storage, - needs_fini: true, }; Ok(dyn_msg) } @@ -336,9 +332,9 @@ impl Deref for DynamicMessage { impl Drop for DynamicMessage { fn drop(&mut self) { - if self.needs_fini { + if let Some(fini_function) = self.metadata.fini_function { // SAFETY: The fini_function expects to be passed a pointer to the message - unsafe { (self.metadata.fini_function)(self.storage.as_mut_ptr() as _) } + unsafe { (fini_function)(self.storage.as_mut_ptr() as _) } } } } @@ -475,7 +471,7 @@ impl DynamicMessage { 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; + self.metadata.fini_function = None; Ok(dest) } else { Err(self) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 2a5a958d4..5de11a262 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -1,6 +1,6 @@ use std::any::Any; use std::boxed::Box; -use std::ffi::{CStr, CString}; +use std::ffi::CString; use std::ops::{Deref, DerefMut}; use std::sync::{Arc, Mutex}; @@ -102,7 +102,7 @@ impl DerefMut for WorkerDynamicSubscriptionCallback { } } -pub enum DynamicSubscriptionCallback { +pub(crate) enum DynamicSubscriptionCallback { /// A callback with the message and the message info as arguments. Node(NodeAsyncDynamicSubscriptionCallback), /// A callback with the payload, message, and the message info as arguments. From 0462d7f7c6b97e1de8dece130920770de0d7e072 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 13:31:10 +0800 Subject: [PATCH 26/33] Remove commented code Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/message_structure.rs | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/rclrs/src/dynamic_message/message_structure.rs b/rclrs/src/dynamic_message/message_structure.rs index 810c68d6d..c5a67a12a 100644 --- a/rclrs/src/dynamic_message/message_structure.rs +++ b/rclrs/src/dynamic_message/message_structure.rs @@ -1142,13 +1142,3 @@ mod tests { DynamicMessage::new("test_msgs/msg/BoundedSequences".try_into().unwrap()).unwrap(); } } - -// #[test] -// fn test_mut_value_same_as_value() { - -// } - -// #[test] -// fn test_setting_value() { - -// } From 2b993974abbdec571c2229d8f3cfc691fb896567 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 23 Jun 2025 16:44:47 +0800 Subject: [PATCH 27/33] Revert fini change Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 3b6ea0e0f..fdfddf33d 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -70,9 +70,7 @@ pub struct DynamicMessageMetadata { introspection_type_support_library: Arc, type_support_ptr: *const rosidl_message_type_support_t, structure: MessageStructure, - // The message content can be moved out into another message, - // in which case the drop impl is not responsible for calling fini anymore - fini_function: Option, + fini_function: unsafe extern "C" fn(*mut std::os::raw::c_void), } /// A message whose type is not known at compile-time. @@ -87,6 +85,9 @@ pub struct DynamicMessage { // 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 ========================= @@ -200,6 +201,8 @@ impl DynamicMessagePackage { 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 { message_type, introspection_type_support_library: Arc::clone( @@ -207,7 +210,7 @@ impl DynamicMessagePackage { ), type_support_ptr, structure, - fini_function: message_members.fini_function, + fini_function, }; Ok(metadata) } @@ -311,6 +314,7 @@ impl DynamicMessageMetadata { let dyn_msg = DynamicMessage { metadata: self.clone(), storage, + needs_fini: true, }; Ok(dyn_msg) } @@ -332,9 +336,9 @@ impl Deref for DynamicMessage { impl Drop for DynamicMessage { fn drop(&mut self) { - if let Some(fini_function) = self.metadata.fini_function { + if self.needs_fini { // SAFETY: The fini_function expects to be passed a pointer to the message - unsafe { (fini_function)(self.storage.as_mut_ptr() as _) } + unsafe { (self.metadata.fini_function)(self.storage.as_mut_ptr() as _) } } } } @@ -471,7 +475,7 @@ impl DynamicMessage { 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.metadata.fini_function = None; + self.needs_fini = false; Ok(dest) } else { Err(self) From eb5aeeb90847dca6fa023fe1c6f4d6ef92977d30 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 24 Jun 2025 13:53:32 +0800 Subject: [PATCH 28/33] Add note on message builder API Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index fdfddf33d..1648fecd2 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -359,6 +359,10 @@ impl DynamicMessage { /// `std_msgs/msg/String`. /// /// The message instance will contain the default values of the message type. + /// + /// This method might not be the most efficient because of the library loading. If you need to + /// instantiate multiple messages of the same type consider using + /// [`crate::dynamic_message::DynamicMessageMetadata::create`] pub fn new(message_type: MessageTypeName) -> Result { DynamicMessageMetadata::new(message_type)?.create() } From c4e0bed51a839bcef9b6924d0939628de06f36ca Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 24 Jun 2025 15:36:01 +0800 Subject: [PATCH 29/33] Add docs, cleanup tests Signed-off-by: Luca Della Vedova --- .../src/dynamic_message/dynamic_publisher.rs | 12 +- .../dynamic_message/dynamic_subscription.rs | 30 ++++- rclrs/src/lib.rs | 2 + rclrs/src/node.rs | 106 ++++++++++++++++++ rclrs/src/worker.rs | 49 +++++++- 5 files changed, 191 insertions(+), 8 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 5bce6a0e7..741dc1e0a 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -9,6 +9,12 @@ use crate::error::{RclrsError, ToResult}; use crate::rcl_bindings::*; use crate::{NodeHandle, PublisherHandle, PublisherOptions, ENTITY_LIFECYCLE_MUTEX}; +/// Struct for sending dynamic messages. +/// +/// Create a dynamic publisher using [`Node::create_dynamic_publisher`][1]. +/// Refer to [`crate::Publisher`] for details of the behavior. +/// +/// [1]: crate::NodeState::create_dynamic_publisher pub type DynamicPublisher = Arc; /// Struct for sending messages of type `T`. @@ -158,17 +164,17 @@ mod tests { let graph = construct_test_graph(namespace)?; let node_1_empty_publisher = graph.node1.create_dynamic_publisher( - "test_msgs/msg/Empty".try_into().unwrap(), + "test_msgs/msg/Empty".try_into()?, "graph_test_topic_1", )?; let topic1 = node_1_empty_publisher.topic_name(); let node_1_basic_types_publisher = graph.node1.create_dynamic_publisher( - "test_msgs/msg/BasicTypes".try_into().unwrap(), + "test_msgs/msg/BasicTypes".try_into()?, "graph_test_topic_2", )?; let topic2 = node_1_basic_types_publisher.topic_name(); let node_2_default_publisher = graph.node2.create_dynamic_publisher( - "test_msgs/msg/Defaults".try_into().unwrap(), + "test_msgs/msg/Defaults".try_into()?, "graph_test_topic_3", )?; let topic3 = node_2_default_publisher.topic_name(); diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 5de11a262..85d3cd3ed 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -17,8 +17,30 @@ use crate::{ Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; +/// Struct for receiving messages whose type is not know at compile time. +/// +/// Create a dynamic subscription using [`NodeState::create_dynamic_subscription()`][1] +/// or [`NodeState::create_async_dynamic_subscription`][2]. +/// +/// There can be multiple subscriptions for the same topic, in different nodes or the same node. +/// A clone of a `Subscription` will refer to the same subscription instance as the original. +/// The underlying instance is tied to [`DynamicSubscriptionState`] which implements the [`DynamicSubscription`] API. +/// +/// Receiving messages requires the node's executor to [spin][3]. +/// +/// When a subscription is created, it may take some time to get "matched" with a corresponding +/// publisher. +/// +/// [1]: crate::NodeState::create_subscription +/// [2]: crate::NodeState::create_async_subscription +/// [3]: crate::Executor::spin pub type DynamicSubscription = Arc>; +/// A [`DynamicSubscription`] that runs on a [`Worker`]. +/// +/// Create a worker dynamic subscription using [`WorkerState::create_dynamic_subscription`][1]. +/// +/// [1]: crate::WorkerState::create_dynamic_subscription pub type WorkerDynamicSubscription = Arc>>; struct DynamicSubscriptionExecutable { @@ -165,7 +187,7 @@ impl DynamicSubscriptionCallback { } impl DynamicSubscriptionExecutable { - pub fn take(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> { + fn take(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> { let mut dynamic_message = self.metadata.create()?; let rmw_message = dynamic_message.storage.as_mut_ptr(); let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; @@ -357,20 +379,20 @@ mod tests { let graph = construct_test_graph(namespace)?; let node_2_empty_subscription = graph.node2.create_dynamic_subscription::<_>( - "test_msgs/msg/Empty".try_into().unwrap(), + "test_msgs/msg/Empty".try_into()?, "graph_test_topic_1", |_, _| {}, )?; let topic1 = node_2_empty_subscription.topic_name(); let node_2_basic_types_subscription = graph.node2.create_dynamic_subscription::<_>( - "test_msgs/msg/BasicTypes".try_into().unwrap(), + "test_msgs/msg/BasicTypes".try_into()?, "graph_test_topic_2", |_, _| {}, )?; let topic2 = node_2_basic_types_subscription.topic_name(); let node_1_defaults_subscription = graph.node1.create_dynamic_subscription::<_>( - "test_msgs/msg/Defaults".try_into().unwrap(), + "test_msgs/msg/Defaults".try_into()?, "graph_test_topic_3", |_, _| {}, )?; diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 366e499b8..3b0dc1a7a 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -200,6 +200,8 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; +#[cfg(feature = "dyn_msg")] +pub use dynamic_message::*; pub use arguments::*; pub use client::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 89d3fde74..07167acb0 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -403,6 +403,22 @@ impl NodeState { PublisherState::::create(options, Arc::clone(&self.handle)) } + /// Creates a [`DynamicPublisher`], a publisher whose type is only known at runtime. + /// + /// Refer to [`Node::create_publisher`] for the API, the only key difference is that the + /// publisher's message type is passed as a [`crate::MessageTypeName`] parameter. + /// + /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let publisher = node.create_dynamic_publisher( + /// "test_msgs/msg/Empty".try_into().unwrap(), + /// "my_topic" + /// .keep_last(100) + /// ) + /// .unwrap(); #[cfg(feature = "dyn_msg")] pub fn create_dynamic_publisher<'a>( &self, @@ -803,6 +819,42 @@ impl NodeState { ) } + /// Creates a [`DynamicSubscription`] with an ordinary callback. + /// + /// For the behavior and API refer to [`Node::create_subscription`], except two key + /// differences: + /// + /// - The message type is determined at runtime through the `topic_type` function parameter. + /// - Only one type of callback is supported (returning both [`crate::DynamicMessage`] and + /// [`crate::MessageInfo`]). + /// + /// # Message type passing + /// + /// The message type can be passed as a [`crate::MessageTypeName`] struct. The struct also implements `TryFrom<&str>` + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let subscription = node.create_dynamic_subscription( + /// MessageTypeName { + /// package_name: "test_msgs".to_owned(), + /// type_name: "Empty".to_owned(), + /// }, + /// "my_topic" + /// .transient_local(), + /// |_msg: DynamicMessage, _info: MessageInfo| { + /// println!("Received message!"); + /// }, + /// ); + /// + /// let subscription = node.create_dynamic_subscription( + /// "test_msgs/msg/Empty".try_into().unwrap(), + /// "my_topic", + /// |_msg: DynamicMessage, _info: MessageInfo| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` #[cfg(feature = "dyn_msg")] pub fn create_dynamic_subscription<'a, F>( &self, @@ -822,6 +874,60 @@ impl NodeState { ) } + /// Creates a [`DynamicSubscription`] with an async callback. + /// + /// For the behavior and API refer to [`Node::create_async_subscription`], except two key + /// differences: + /// + /// - The message type is determined at runtime through the `topic_type` function parameter. + /// - Only one type of callback is supported (returning both [`crate::DynamicMessage`] and + /// [`crate::MessageInfo`]. + /// + /// # Message type passing + /// + /// The message type can be passed as a [`crate::MessageTypeName`] struct. The struct also implements `TryFrom<&str>` + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::Arc; + /// + /// let count_worker = node.create_worker(0_usize); + /// let data_worker = node.create_worker(String::new()); + /// + /// let service = node.create_async_dynamic_subscription( + /// "example_interfaces/msg/String".try_into()?, + /// "topic", + /// move |msg: DynamicMessage, _info: MessageInfo| { + /// // Clone the workers so they can be captured into the async block + /// let count_worker = Arc::clone(&count_worker); + /// let data_worker = Arc::clone(&data_worker); + /// Box::pin(async move { + /// // Update the message count + /// let current_count = count_worker.run(move |count: &mut usize| { + /// *count += 1; + /// *count + /// }).await.unwrap(); + /// + /// // Change the data in the data_worker and get back the data + /// // that was previously put in there. + /// let previous = data_worker.run(move |data: &mut String| { + /// let value = msg.get("data").unwrap(); + /// let Value::Simple(value) = value else { + /// panic!("Unexpected value type, expected Simple value"); + /// }; + /// let SimpleValue::String(value) = value else { + /// panic!("Unexpected value type, expected String"); + /// }; + /// std::mem::replace(data, value.to_string()) + /// }).await.unwrap(); + /// + /// println!("Current count is {current_count}, data was previously {previous}"); + /// }) + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` #[cfg(feature = "dyn_msg")] pub fn create_async_dynamic_subscription<'a, F>( &self, diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index e4d6d5a37..55c456c13 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -269,6 +269,45 @@ impl WorkerState { ) } + /// Creates a [`WorkerDynamicSubscription`], whose message type is only know at runtime. + /// + /// Refer to ['Worker::create_subscription`] for the API and behavior except two key + /// differences: + /// + /// - The message type is determined at runtime through the `topic_type` function parameter. + /// - Only one type of callback is supported (returning both [`crate::DynamicMessage`] and + /// [`crate::MessageInfo`]). + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// // The worker's payload is data that we want to share with other callbacks. + /// let worker = node.create_worker::>(None); + /// + /// // This variable will be the mutable internal state of the subscription + /// // callback. + /// let mut count = 0_usize; + /// + /// let subscription = worker.create_dynamic_subscription( + /// "example_interfaces/msg/String".try_into()?, + /// "topic", + /// move |data: &mut Option, msg: DynamicMessage, _msg_info: MessageInfo| { + /// count += 1; + /// let value = msg.get("data").unwrap(); + /// let Value::Simple(value) = value else { + /// panic!("Unexpected value type, expected Simple value"); + /// }; + /// let SimpleValue::String(value) = value else { + /// panic!("Unexpected value type, expected String"); + /// }; + /// println!("#{count} | I heard: '{}'", value); + /// + /// *data = Some(value.to_string()); + /// }, + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` #[cfg(feature = "dyn_msg")] pub fn create_dynamic_subscription<'a, F>( &self, @@ -535,6 +574,7 @@ mod tests { struct TestPayload { subscription_count: usize, service_count: usize, + #[cfg(feature = "dyn_msg")] dynamic_subscription_count: usize, } @@ -550,6 +590,7 @@ mod tests { }, ); + #[cfg(feature = "dyn_msg")] let _count_dynamic_sub = worker.create_dynamic_subscription( "test_msgs/msg/Empty".try_into().unwrap(), "test_worker_topic", @@ -569,8 +610,14 @@ mod tests { let promise = worker.listen_until(move |payload| { if payload.service_count > 0 && payload.subscription_count > 0 - && payload.dynamic_subscription_count > 0 { + #[cfg(feature = "dyn_msg")] + if payload.dynamic_subscription_count > 0 { + Some(*payload) + } else { + None + } + #[cfg(not(feature = "dyn_msg"))] Some(*payload) } else { None From ccd38524632cf69c0a59303a03b09a9c0251025c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 24 Jun 2025 15:58:11 +0800 Subject: [PATCH 30/33] Fix take logic, cleanup Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 25 +++++++++++-------- rclrs/src/subscription.rs | 18 ------------- 2 files changed, 14 insertions(+), 29 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index 85d3cd3ed..d62f9b0ce 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -14,7 +14,7 @@ use crate::rcl_bindings::*; use crate::{ MessageInfo, Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, WaitableLifecycle, WorkScope, - Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, + Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, RclrsErrorFilter, }; /// Struct for receiving messages whose type is not know at compile time. @@ -172,17 +172,20 @@ impl DynamicSubscriptionCallback { received: (*any_payload).type_id(), }); }; - match self { - Self::Node(cb) => { - let (msg, msg_info) = executable.take()?; - commands.run_async(cb(msg, msg_info)); + let mut evaluate = || { + match self { + Self::Node(cb) => { + let (msg, msg_info) = executable.take()?; + commands.run_async(cb(msg, msg_info)); + } + Self::Worker(cb) => { + let (msg, msg_info) = executable.take()?; + cb(payload, msg, msg_info); + } } - Self::Worker(cb) => { - let (msg, msg_info) = executable.take()?; - cb(payload, msg, msg_info); - } - } - Ok(()) + Ok(()) + }; + evaluate().take_failed_ok() } } diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 92e9f7746..6c7289b6f 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -270,17 +270,6 @@ impl<'a> DynamicSubscriptionOptions<'a> { } } -/* -impl<'a, T: IntoPrimitiveOptions<'a>> From for DynamicSubscriptionOptions<'a> { - fn from(value: T) -> Self { - let primitive = value.into_primitive_options(); - let mut options = Self::new(primitive.name); - primitive.apply_to(&mut options.qos); - options - } -} -*/ - struct SubscriptionExecutable { handle: Arc, callback: Arc>>, @@ -365,13 +354,6 @@ impl SubscriptionHandle { Ok((T::from_rmw_message(rmw_message), message_info)) } - /* - #[cfg(feature = "dyn_msg")] - fn take_dynamic(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> { - - } - */ - /// This is a version of take() that returns a boxed message. /// /// This can be more efficient for messages containing large arrays. From c9a53db95f9be280b1388f2e624b5330c9c351a6 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 24 Jun 2025 15:58:29 +0800 Subject: [PATCH 31/33] Format Signed-off-by: Luca Della Vedova --- rclrs/src/dynamic_message/dynamic_publisher.rs | 14 ++++++-------- rclrs/src/dynamic_message/dynamic_subscription.rs | 4 ++-- rclrs/src/worker.rs | 4 +--- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 741dc1e0a..2fd3ab523 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -163,20 +163,18 @@ mod tests { let namespace = "/test_dynamic_publishers_graph"; let graph = construct_test_graph(namespace)?; - let node_1_empty_publisher = graph.node1.create_dynamic_publisher( - "test_msgs/msg/Empty".try_into()?, - "graph_test_topic_1", - )?; + let node_1_empty_publisher = graph + .node1 + .create_dynamic_publisher("test_msgs/msg/Empty".try_into()?, "graph_test_topic_1")?; let topic1 = node_1_empty_publisher.topic_name(); let node_1_basic_types_publisher = graph.node1.create_dynamic_publisher( "test_msgs/msg/BasicTypes".try_into()?, "graph_test_topic_2", )?; let topic2 = node_1_basic_types_publisher.topic_name(); - let node_2_default_publisher = graph.node2.create_dynamic_publisher( - "test_msgs/msg/Defaults".try_into()?, - "graph_test_topic_3", - )?; + let node_2_default_publisher = graph + .node2 + .create_dynamic_publisher("test_msgs/msg/Defaults".try_into()?, "graph_test_topic_3")?; let topic3 = node_2_default_publisher.topic_name(); std::thread::sleep(std::time::Duration::from_millis(100)); diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index d62f9b0ce..f276ffdaa 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -13,8 +13,8 @@ use super::{ use crate::rcl_bindings::*; use crate::{ MessageInfo, Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, - SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, WaitableLifecycle, WorkScope, - Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, RclrsErrorFilter, + RclrsErrorFilter, SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, + WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; /// Struct for receiving messages whose type is not know at compile time. diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index 55c456c13..3ecbf81ef 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -608,9 +608,7 @@ mod tests { ); let promise = worker.listen_until(move |payload| { - if payload.service_count > 0 - && payload.subscription_count > 0 - { + if payload.service_count > 0 && payload.subscription_count > 0 { #[cfg(feature = "dyn_msg")] if payload.dynamic_subscription_count > 0 { Some(*payload) From 83cf32c9a1c9cb4b4ab55cbb2529667211ec6f7f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 24 Jun 2025 16:08:29 +0800 Subject: [PATCH 32/33] Clear warnings Signed-off-by: Luca Della Vedova --- .../dynamic_message/dynamic_subscription.rs | 3 ++- rclrs/src/node.rs | 23 +++++++++++-------- rclrs/src/worker.rs | 13 +++++++---- 3 files changed, 23 insertions(+), 16 deletions(-) diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index f276ffdaa..be3ec3c1d 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -242,6 +242,8 @@ where /// /// Holding onto this sender will keep the subscription task alive. Once /// this sender is dropped, the subscription task will end itself. + // TODO(luca) Expose an API to update the callback at runtime + #[allow(unused)] callback: Arc>>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. @@ -267,7 +269,6 @@ where node_handle: &Arc, commands: &Arc, ) -> Result, RclrsError> { - // TODO(luca) a lot of duplication with nomral, refactor // This loads the introspection type support library. let metadata = DynamicMessageMetadata::new(topic_type)?; let SubscriptionOptions { topic, qos } = options.into(); diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 07167acb0..82dbec7ca 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -6,15 +6,19 @@ pub use primitive_options::*; mod graph; #[cfg(feature = "dyn_msg")] -use crate::dynamic_message::{ - DynamicMessage, DynamicPublisher, DynamicPublisherState, DynamicSubscription, - DynamicSubscriptionState, MessageTypeName, NodeAsyncDynamicSubscriptionCallback, - NodeDynamicSubscriptionCallback, +use crate::{ + dynamic_message::{ + DynamicMessage, DynamicPublisher, DynamicPublisherState, DynamicSubscription, + DynamicSubscriptionState, MessageTypeName, NodeAsyncDynamicSubscriptionCallback, + NodeDynamicSubscriptionCallback, + }, + MessageInfo, }; +#[cfg(feature = "dyn_msg")] +use futures::future::BoxFuture; pub use graph::*; -use futures::future::BoxFuture; mod node_graph_task; use node_graph_task::*; @@ -39,11 +43,10 @@ use rosidl_runtime_rs::Message; use crate::{ rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, - IntoNodeSubscriptionCallback, LogParams, Logger, MessageInfo, ParameterBuilder, - ParameterInterface, ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, - PublisherState, RclrsError, Service, ServiceOptions, ServiceState, Subscription, - SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, Worker, WorkerOptions, - WorkerState, ENTITY_LIFECYCLE_MUTEX, + IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, + ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, + Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState, + TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX, }; /// A processing unit that can communicate with other nodes. See the API of diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index 3ecbf81ef..1223175bc 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -1,11 +1,14 @@ #[cfg(feature = "dyn_msg")] -use crate::dynamic_message::{ - DynamicMessage, DynamicSubscriptionState, MessageTypeName, WorkerDynamicSubscription, - WorkerDynamicSubscriptionCallback, +use crate::{ + dynamic_message::{ + DynamicMessage, DynamicSubscriptionState, MessageTypeName, WorkerDynamicSubscription, + WorkerDynamicSubscriptionCallback, + }, + MessageInfo, }; use crate::{ - log_fatal, IntoWorkerServiceCallback, IntoWorkerSubscriptionCallback, MessageInfo, Node, - Promise, RclrsError, ServiceOptions, ServiceState, SubscriptionOptions, SubscriptionState, + log_fatal, IntoWorkerServiceCallback, IntoWorkerSubscriptionCallback, Node, Promise, + RclrsError, ServiceOptions, ServiceState, SubscriptionOptions, SubscriptionState, WorkerCommands, WorkerService, WorkerSubscription, }; use futures::channel::oneshot; From 42cd70f96b8d80c96d060bb9f9fbeee366647cce Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 24 Jun 2025 16:30:13 +0800 Subject: [PATCH 33/33] Cleanup Signed-off-by: Luca Della Vedova --- examples/dynamic_pub_sub/Cargo.toml | 3 ++- examples/dynamic_pub_sub/src/main.rs | 25 +++++++++--------- .../rclrs_example_msgs/msg/VariousTypes.msg | 1 - .../src/dynamic_message/dynamic_publisher.rs | 11 ++------ rclrs/src/node.rs | 12 ++++++--- rclrs/src/subscription.rs | 26 ------------------- rclrs/test_header.h | 5 ---- 7 files changed, 26 insertions(+), 57 deletions(-) delete mode 100644 rclrs/test_header.h diff --git a/examples/dynamic_pub_sub/Cargo.toml b/examples/dynamic_pub_sub/Cargo.toml index 1565cbb7e..d47f25426 100644 --- a/examples/dynamic_pub_sub/Cargo.toml +++ b/examples/dynamic_pub_sub/Cargo.toml @@ -6,5 +6,6 @@ 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"] } +# TODO(luca) change this to a version once dynamic message support is released on crates.io +rclrs = { path = "../../rclrs", features = ["dyn_msg"] } anyhow = {version = "1", features = ["backtrace"]} diff --git a/examples/dynamic_pub_sub/src/main.rs b/examples/dynamic_pub_sub/src/main.rs index 6aae57fdc..939e12a96 100644 --- a/examples/dynamic_pub_sub/src/main.rs +++ b/examples/dynamic_pub_sub/src/main.rs @@ -1,22 +1,23 @@ use anyhow::{Error, Result}; -use std::env; +use rclrs::*; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); - let mut node = rclrs::create_node(&context, "dynamic_subscriber")?; + let node = executor.create_node("dynamic_subscriber")?; - let mut num_messages: usize = 0; - - let _subscription = node.create_dynamic_subscription( + let worker = node.create_worker::(0); + let _subscription = worker.create_dynamic_subscription( + "rclrs_example_msgs/msg/VariousTypes".try_into()?, "topic", - "rclrs_example_msgs/msg/VariousTypes", - rclrs::QOS_PROFILE_DEFAULT, - move |msg| { - num_messages += 1; - println!("I heard: '{:#?}'", msg.structure()); + move |num_messages: &mut usize, msg, _msg_info| { + *num_messages += 1; + println!("#{} | I heard: '{:#?}'", *num_messages, msg.structure()); }, )?; - rclrs::spin(&node).map_err(|err| err.into()) + println!("Waiting for messages..."); + executor.spin(SpinOptions::default()).first_error()?; + Ok(()) } diff --git a/examples/rclrs_example_msgs/msg/VariousTypes.msg b/examples/rclrs_example_msgs/msg/VariousTypes.msg index cebefb6d6..ec990f0f2 100644 --- a/examples/rclrs_example_msgs/msg/VariousTypes.msg +++ b/examples/rclrs_example_msgs/msg/VariousTypes.msg @@ -3,7 +3,6 @@ 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/src/dynamic_message/dynamic_publisher.rs b/rclrs/src/dynamic_message/dynamic_publisher.rs index 2fd3ab523..657b26d4b 100644 --- a/rclrs/src/dynamic_message/dynamic_publisher.rs +++ b/rclrs/src/dynamic_message/dynamic_publisher.rs @@ -17,16 +17,9 @@ use crate::{NodeHandle, PublisherHandle, PublisherOptions, ENTITY_LIFECYCLE_MUTE /// [1]: crate::NodeState::create_dynamic_publisher pub type DynamicPublisher = Arc; -/// Struct for sending messages of type `T`. +/// The inner state of a dynamic publisher. /// -/// 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 +/// Refer to [`crate::PublisherState`] for details of the behavior. pub struct DynamicPublisherState { handle: PublisherHandle, metadata: DynamicMessageMetadata, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 82dbec7ca..5babc3c0c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -408,10 +408,12 @@ impl NodeState { /// Creates a [`DynamicPublisher`], a publisher whose type is only known at runtime. /// - /// Refer to [`Node::create_publisher`] for the API, the only key difference is that the + /// Refer to [`Node::create_publisher`][1] for the API, the only key difference is that the /// publisher's message type is passed as a [`crate::MessageTypeName`] parameter. /// /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// + /// [1]: crate::NodeState::create_publisher /// ``` /// # use rclrs::*; /// # let executor = Context::default().create_basic_executor(); @@ -824,7 +826,7 @@ impl NodeState { /// Creates a [`DynamicSubscription`] with an ordinary callback. /// - /// For the behavior and API refer to [`Node::create_subscription`], except two key + /// For the behavior and API refer to [`Node::create_subscription`][1], except two key /// differences: /// /// - The message type is determined at runtime through the `topic_type` function parameter. @@ -834,6 +836,8 @@ impl NodeState { /// # Message type passing /// /// The message type can be passed as a [`crate::MessageTypeName`] struct. The struct also implements `TryFrom<&str>` + /// + /// [1]: crate::NodeState::create_publisher /// ``` /// # use rclrs::*; /// # let executor = Context::default().create_basic_executor(); @@ -879,7 +883,7 @@ impl NodeState { /// Creates a [`DynamicSubscription`] with an async callback. /// - /// For the behavior and API refer to [`Node::create_async_subscription`], except two key + /// For the behavior and API refer to [`Node::create_async_subscription`][1], except two key /// differences: /// /// - The message type is determined at runtime through the `topic_type` function parameter. @@ -889,6 +893,8 @@ impl NodeState { /// # Message type passing /// /// The message type can be passed as a [`crate::MessageTypeName`] struct. The struct also implements `TryFrom<&str>` + /// + /// [1]: crate::NodeState::create_async_subscription /// ``` /// # use rclrs::*; /// # let executor = Context::default().create_basic_executor(); diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 6c7289b6f..124254766 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -244,32 +244,6 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { } } -/// `SubscriptionOptions` are used by [`Node::create_subscription`][1] to initialize -/// a [`Subscription`]. -/// -/// [1]: crate::NodeState::create_subscription -#[derive(Debug, Clone)] -#[non_exhaustive] -pub struct DynamicSubscriptionOptions<'a> { - /// The topic name for the subscription. - pub topic: &'a str, - /// The topic type for the subscription. - pub topic_type: &'a str, - /// The quality of service settings for the subscription. - pub qos: QoSProfile, -} - -impl<'a> DynamicSubscriptionOptions<'a> { - /// Initialize a new [`SubscriptionOptions`] with default settings. - pub fn new(topic: &'a str, topic_type: &'a str) -> Self { - Self { - topic, - topic_type, - qos: QoSProfile::topics_default(), - } - } -} - struct SubscriptionExecutable { handle: Arc, callback: Arc>>, diff --git a/rclrs/test_header.h b/rclrs/test_header.h deleted file mode 100644 index 392e56386..000000000 --- a/rclrs/test_header.h +++ /dev/null @@ -1,5 +0,0 @@ -#include - -struct Foo { - char16_t i; -};