From 27469969f43b3bf1f398e81f1be1a2afc6129549 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 10 Nov 2022 19:56:03 +0100 Subject: [PATCH 01/32] Make constructor methods for pub/sub/etc. take rcl_node_t mutex (#290) The constructors are not public, so we're free to use internal types as arguments. This signature makes it possible, or at least easier, to write parameter services that are owned by the node: With this change, the Node does not need to be constructed before the parameter services are constructed, which would somewhat contradict the node owning these services. --- rclrs/src/client.rs | 15 ++++++++------- rclrs/src/node.rs | 17 +++++++++++++---- rclrs/src/publisher.rs | 12 +++++++----- rclrs/src/service.rs | 22 ++++++++++++++-------- rclrs/src/subscription.rs | 14 +++++++------- 5 files changed, 49 insertions(+), 31 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index bb3cd4e57..e8cde1e1f 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -9,7 +9,6 @@ use rosidl_runtime_rs::Message; use crate::error::{RclReturnCode, ToResult}; use crate::MessageCow; -use crate::Node; use crate::{rcl_bindings::*, RclrsError}; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -56,8 +55,11 @@ type RequestId = i64; /// Main class responsible for sending requests to a ROS service. /// -/// The only available way to instantiate clients is via [`Node::create_client`], this is to -/// ensure that [`Node`]s can track all the clients that have been created. +/// The only available way to instantiate clients is via [`Node::create_client`][1], this is to +/// ensure that [`Node`][2]s can track all the clients that have been created. +/// +/// [1]: crate::Node::create_client +/// [2]: crate::Node pub struct Client where T: rosidl_runtime_rs::Service, @@ -72,7 +74,7 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new client. - pub(crate) fn new(node: &Node, topic: &str) -> Result + pub(crate) fn new(rcl_node_mtx: Arc>, topic: &str) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_client`], see the struct's documentation for the rationale where @@ -86,7 +88,6 @@ where err, s: topic.into(), })?; - let rcl_node = { &mut *node.rcl_node_mtx.lock().unwrap() }; // SAFETY: No preconditions for this function. let client_options = unsafe { rcl_client_get_default_options() }; @@ -98,7 +99,7 @@ where // afterwards. rcl_client_init( &mut rcl_client, - rcl_node, + &*rcl_node_mtx.lock().unwrap(), type_support, topic_c_string.as_ptr(), &client_options, @@ -108,7 +109,7 @@ where let handle = Arc::new(ClientHandle { rcl_client_mtx: Mutex::new(rcl_client), - rcl_node_mtx: node.rcl_node_mtx.clone(), + rcl_node_mtx, in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index cc0ecc665..4df07ba90 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -184,7 +184,7 @@ impl Node { where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(self, topic)?); + let client = Arc::new(Client::::new(Arc::clone(&self.rcl_node_mtx), topic)?); self.clients .push(Arc::downgrade(&client) as Weak); Ok(client) @@ -243,7 +243,7 @@ impl Node { where T: Message, { - Publisher::::new(self, topic, qos) + Publisher::::new(Arc::clone(&self.rcl_node_mtx), topic, qos) } /// Creates a [`Service`][1]. @@ -259,7 +259,11 @@ impl Node { T: rosidl_runtime_rs::Service, F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { - let service = Arc::new(Service::::new(self, topic, callback)?); + let service = Arc::new(Service::::new( + Arc::clone(&self.rcl_node_mtx), + topic, + callback, + )?); self.services .push(Arc::downgrade(&service) as Weak); Ok(service) @@ -278,7 +282,12 @@ impl Node { where T: Message, { - let subscription = Arc::new(Subscription::::new(self, topic, qos, callback)?); + let subscription = Arc::new(Subscription::::new( + Arc::clone(&self.rcl_node_mtx), + topic, + qos, + callback, + )?); self.subscriptions .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 631c231f9..a77a41aa4 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -9,7 +9,6 @@ use rosidl_runtime_rs::{Message, RmwMessage}; use crate::error::{RclrsError, ToResult}; use crate::qos::QoSProfile; use crate::rcl_bindings::*; -use crate::Node; mod loaned_message; pub use loaned_message::*; @@ -69,7 +68,11 @@ where /// Creates a new `Publisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub fn new(node: &Node, topic: &str, qos: QoSProfile) -> Result + pub fn new( + rcl_node_mtx: Arc>, + topic: &str, + qos: QoSProfile, + ) -> Result where T: Message, { @@ -81,7 +84,6 @@ where 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() }; @@ -94,7 +96,7 @@ where // TODO: type support? rcl_publisher_init( &mut rcl_publisher, - rcl_node, + &*rcl_node_mtx.lock().unwrap(), type_support_ptr, topic_c_string.as_ptr(), &publisher_options, @@ -104,7 +106,7 @@ where Ok(Self { rcl_publisher_mtx: Mutex::new(rcl_publisher), - rcl_node_mtx: Arc::clone(&node.rcl_node_mtx), + rcl_node_mtx, type_support_ptr, message: PhantomData, }) diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ec7f0ad91..8cac799b1 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -6,7 +6,7 @@ use std::sync::{Arc, Mutex, MutexGuard}; use rosidl_runtime_rs::Message; use crate::error::{RclReturnCode, ToResult}; -use crate::{rcl_bindings::*, MessageCow, Node, RclrsError}; +use crate::{rcl_bindings::*, MessageCow, RclrsError}; // 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. @@ -51,8 +51,11 @@ type ServiceCallback = /// Main class responsible for responding to requests sent by ROS clients. /// -/// The only available way to instantiate services is via [`Node::create_service`], this is to -/// ensure that [`Node`]s can track all the services that have been created. +/// The only available way to instantiate services is via [`Node::create_service()`][1], this is to +/// ensure that [`Node`][2]s can track all the services that have been created. +/// +/// [1]: crate::Node::create_service +/// [2]: crate::Node pub struct Service where T: rosidl_runtime_rs::Service, @@ -67,7 +70,11 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new service. - pub(crate) fn new(node: &Node, topic: &str, callback: F) -> Result + pub(crate) fn new( + rcl_node_mtx: Arc>, + topic: &str, + callback: F, + ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_service`], see the struct's documentation for the rationale where @@ -82,7 +89,6 @@ where err, s: topic.into(), })?; - let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap(); // SAFETY: No preconditions for this function. let service_options = unsafe { rcl_service_get_default_options() }; @@ -93,8 +99,8 @@ where // The topic name and the options are copied by this function, so they can be dropped // afterwards. rcl_service_init( - &mut rcl_service as *mut _, - rcl_node as *mut _, + &mut rcl_service, + &*rcl_node_mtx.lock().unwrap(), type_support, topic_c_string.as_ptr(), &service_options as *const _, @@ -104,7 +110,7 @@ where let handle = Arc::new(ServiceHandle { rcl_service_mtx: Mutex::new(rcl_service), - rcl_node_mtx: node.rcl_node_mtx.clone(), + rcl_node_mtx, in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 5a55c5f85..9e551becd 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -8,7 +8,6 @@ use rosidl_runtime_rs::{Message, RmwMessage}; use crate::error::{RclReturnCode, ToResult}; use crate::qos::QoSProfile; -use crate::Node; use crate::{rcl_bindings::*, RclrsError}; mod callback; @@ -63,11 +62,13 @@ pub trait SubscriptionBase: Send + Sync { /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. /// -/// The only available way to instantiate subscriptions is via [`Node::create_subscription`], this -/// is to ensure that [`Node`]s can track all the subscriptions that have been created. +/// The only available way to instantiate subscriptions is via [`Node::create_subscription()`][3], this +/// is to ensure that [`Node`][4]s can track all the subscriptions that have been created. /// /// [1]: crate::spin_once /// [2]: crate::spin +/// [3]: crate::Node::create_subscription +/// [4]: crate::Node pub struct Subscription where T: Message, @@ -84,7 +85,7 @@ where { /// Creates a new subscription. pub(crate) fn new( - node: &Node, + rcl_node_mtx: Arc>, topic: &str, qos: QoSProfile, callback: impl SubscriptionCallback, @@ -102,7 +103,6 @@ where 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() }; @@ -115,7 +115,7 @@ where // TODO: type support? rcl_subscription_init( &mut rcl_subscription, - rcl_node, + &*rcl_node_mtx.lock().unwrap(), type_support, topic_c_string.as_ptr(), &subscription_options, @@ -125,7 +125,7 @@ where let handle = Arc::new(SubscriptionHandle { rcl_subscription_mtx: Mutex::new(rcl_subscription), - rcl_node_mtx: node.rcl_node_mtx.clone(), + rcl_node_mtx, in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); From 087527ef12a353c5b4023b242c31c715f5369a14 Mon Sep 17 00:00:00 2001 From: Sam Privett Date: Sun, 13 Nov 2022 15:30:52 -0800 Subject: [PATCH 02/32] Extend string types (#293) --- rosidl_runtime_rs/src/string.rs | 132 +++++++++++++++++++++++++++----- 1 file changed, 111 insertions(+), 21 deletions(-) diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index 201889a95..44043a60d 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -126,21 +126,6 @@ macro_rules! string_impl { ) -> bool; } - impl Default for $string { - fn default() -> Self { - let mut msg = Self { - data: std::ptr::null_mut(), - size: 0, - capacity: 0, - }; - // SAFETY: Passing in a zeroed string is safe. - if !unsafe { $init(&mut msg as *mut _) } { - panic!("Sinit failed"); - } - msg - } - } - impl Clone for $string { fn clone(&self) -> Self { let mut msg = Self::default(); @@ -158,6 +143,21 @@ macro_rules! string_impl { } } + impl Default for $string { + fn default() -> Self { + let mut msg = Self { + data: std::ptr::null_mut(), + size: 0, + capacity: 0, + }; + // SAFETY: Passing in a zeroed string is safe. + if !unsafe { $init(&mut msg as *mut _) } { + panic!("$init failed"); + } + msg + } + } + // It's not guaranteed that there are no interior null bytes, hence no Deref to CStr. // This does not include the null byte at the end! impl Deref for $string { @@ -200,15 +200,39 @@ macro_rules! string_impl { impl Eq for $string {} - impl Hash for $string { - fn hash(&self, state: &mut H) { - self.deref().hash(state) + impl Extend for $string { + fn extend>(&mut self, iter: I) { + let mut s = self.to_string(); + s.extend(iter); + *self = Self::from(s.as_str()); } } - impl PartialEq for $string { - fn eq(&self, other: &Self) -> bool { - self.deref().eq(other.deref()) + impl<'a> Extend<&'a char> for $string { + fn extend>(&mut self, iter: I) { + self.extend(iter.into_iter().cloned()); + } + } + + impl FromIterator for $string { + fn from_iter>(iter: I) -> Self { + let mut buf = <$string>::default(); + buf.extend(iter); + buf + } + } + + impl<'a> FromIterator<&'a char> for $string { + fn from_iter>(iter: I) -> Self { + let mut buf = <$string>::default(); + buf.extend(iter); + buf + } + } + + impl Hash for $string { + fn hash(&self, state: &mut H) { + self.deref().hash(state) } } @@ -218,6 +242,12 @@ macro_rules! string_impl { } } + impl PartialEq for $string { + fn eq(&self, other: &Self) -> bool { + self.deref().eq(other.deref()) + } + } + impl PartialOrd for $string { fn partial_cmp(&self, other: &Self) -> Option { self.deref().partial_cmp(other.deref()) @@ -503,4 +533,64 @@ mod tests { s.as_str().try_into().unwrap() } } + + #[test] + fn string_from_char_iterator() { + // Base char case + let expected = String::from("abc"); + let actual = "abc".chars().collect::(); + + assert_eq!(expected, actual); + + // Empty case + let expected = String::from(""); + let actual = "".chars().collect::(); + + assert_eq!(expected, actual); + + // Non-ascii char case + let expected = String::from("Grüß Gott! 𝕊"); + let actual = "Grüß Gott! 𝕊".chars().collect::(); + + assert_eq!(expected, actual); + } + + #[test] + fn extend_string_with_char_iterator() { + let expected = WString::from("abcdef"); + let mut actual = WString::from("abc"); + actual.extend("def".chars()); + + assert_eq!(expected, actual); + } + + #[test] + fn wstring_from_char_iterator() { + // Base char case + let expected = WString::from("abc"); + let actual = "abc".chars().collect::(); + + assert_eq!(expected, actual); + + // Empty case + let expected = WString::from(""); + let actual = "".chars().collect::(); + + assert_eq!(expected, actual); + + // Non-ascii char case + let expected = WString::from("Grüß Gott! 𝕊"); + let actual = "Grüß Gott! 𝕊".chars().collect::(); + + assert_eq!(expected, actual); + } + + #[test] + fn extend_wstring_with_char_iterator() { + let expected = WString::from("abcdef"); + let mut actual = WString::from("abc"); + actual.extend("def".chars()); + + assert_eq!(expected, actual); + } } From 37de108a0dc4cb5f3faefdacf7fd2825683bf977 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Sat, 11 Mar 2023 16:20:15 +0100 Subject: [PATCH 03/32] Allow rolling job to fail. Force install of known to work version of (#302) pydocstyle. Schedule CI runs every night. --- .github/workflows/rust.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 69990be42..d20fbebc4 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -5,6 +5,8 @@ on: branches: [ main ] pull_request: branches: [ main ] + schedule: + - cron: '0 0 * * *' env: CARGO_TERM_COLOR: always @@ -36,6 +38,7 @@ jobs: ros_distribution: rolling ros_version: 2 runs-on: ubuntu-latest + continue-on-error: ${{ matrix.ros_distribution == 'rolling' }} container: image: ${{ matrix.docker_image }} steps: @@ -51,6 +54,10 @@ jobs: with: required-ros-distributions: ${{ matrix.ros_distribution }} + - name: Downgrade pydocstyle as a workaround for https://github.com/ament/ament_lint/issues/434 + run: | + sudo pip install pydocstyle==6.1.1 + - name: Setup Rust uses: dtolnay/rust-toolchain@1.63.0 with: From 3ae094494702426c718537fb635a4b45381900b4 Mon Sep 17 00:00:00 2001 From: Sam Privett Date: Sun, 12 Mar 2023 04:02:28 -0700 Subject: [PATCH 04/32] Removed support for the EOL galactic distro (#306) * Removed support for the EOL galactic distro * Bumped setup-ros to v0.6 * Try using the experimental debs * We don't actually need to use experimental debs anymore. --------- Co-authored-by: Sam Privett --- .github/workflows/rust.yml | 7 +------ rclrs/src/dynamic_message.rs | 4 ++-- rclrs/src/subscription/message_info.rs | 10 +++++----- ros2_rust_galactic.repos | 25 ------------------------- 4 files changed, 8 insertions(+), 38 deletions(-) delete mode 100644 ros2_rust_galactic.repos diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index d20fbebc4..febb2c6f9 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -17,7 +17,6 @@ jobs: matrix: ros_distribution: - foxy - - galactic - humble - rolling include: @@ -25,10 +24,6 @@ jobs: - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest ros_distribution: foxy ros_version: 2 - # Galactic Geochelone (May 2021 - November 2022) - - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest - ros_distribution: galactic - ros_version: 2 # Humble Hawksbill (May 2022 - May 2027) - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest ros_distribution: humble @@ -50,7 +45,7 @@ jobs: echo ::set-output name=package_list::$(colcon list --names-only) - name: Setup ROS environment - uses: ros-tooling/setup-ros@v0.3 + uses: ros-tooling/setup-ros@v0.6 with: required-ros-distributions: ${{ matrix.ros_distribution }} diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 1251636c9..3997ad6e7 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -9,9 +9,9 @@ use std::fmt::{self, Display}; use std::path::PathBuf; use std::sync::Arc; -#[cfg(any(ros_distro = "foxy", ros_distro = "galactic"))] +#[cfg(ros_distro = "foxy")] use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; -#[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] +#[cfg(not(ros_distro = "foxy"))] use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers_s as rosidl_message_members_t; use crate::rcl_bindings::*; diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index b1dae04cb..57da4ce7b 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -80,7 +80,7 @@ pub struct MessageInfo { /// received messages. /// Those might have already been taken by other messages that were received in between or lost. /// `psn2 - psn1 - 1 = 0` if and only if the messages were sent by the publisher consecutively. - #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] + #[cfg(not(ros_distro = "foxy"))] pub publication_sequence_number: u64, /// Sequence number of the received message set by the subscription. /// @@ -98,7 +98,7 @@ pub struct MessageInfo { /// /// - `rsn2 > rsn1` (except in the case of a wrap around) /// - `rsn2 = rsn1 + 1` if and only if both messages were received consecutively. - #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] + #[cfg(not(ros_distro = "foxy"))] pub reception_sequence_number: u64, /// An identifier for the publisher that sent the message. pub publisher_gid: PublisherGid, @@ -123,9 +123,9 @@ impl MessageInfo { Self { source_timestamp, received_timestamp, - #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] + #[cfg(not(ros_distro = "foxy"))] publication_sequence_number: rmw_message_info.publication_sequence_number, - #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] + #[cfg(not(ros_distro = "foxy"))] reception_sequence_number: rmw_message_info.reception_sequence_number, publisher_gid, } @@ -136,7 +136,7 @@ impl MessageInfo { mod tests { use super::*; - #[cfg(all(not(ros_distro = "foxy"), not(ros_distro = "galactic")))] + #[cfg(not(ros_distro = "foxy"))] #[test] fn negative_durations() { let rmw_message_info = rmw_message_info_t { diff --git a/ros2_rust_galactic.repos b/ros2_rust_galactic.repos deleted file mode 100644 index d1e2d9792..000000000 --- a/ros2_rust_galactic.repos +++ /dev/null @@ -1,25 +0,0 @@ -repositories: - ros2/common_interfaces: - type: git - url: https://github.com/ros2/common_interfaces.git - version: galactic - ros2/example_interfaces: - type: git - url: https://github.com/ros2/example_interfaces.git - version: galactic - ros2/rcl_interfaces: - type: git - url: https://github.com/ros2/rcl_interfaces.git - version: galactic - ros2/test_interface_files: - type: git - url: https://github.com/ros2/test_interface_files.git - version: galactic - ros2/rosidl_defaults: - type: git - url: https://github.com/ros2/rosidl_defaults.git - version: galactic - ros2/unique_identifier_msgs: - type: git - url: https://github.com/ros2/unique_identifier_msgs.git - version: galactic From 6939a8a5976e603d46fefa0a46c0a50ee679c3d8 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Sun, 12 Mar 2023 12:03:48 +0100 Subject: [PATCH 05/32] Improve efficiency of deserializing strings (#300) --- rosidl_runtime_rs/src/string/serde.rs | 93 +++++++++++++++++++++++++-- 1 file changed, 89 insertions(+), 4 deletions(-) diff --git a/rosidl_runtime_rs/src/string/serde.rs b/rosidl_runtime_rs/src/string/serde.rs index 2df52d58f..be1058a47 100644 --- a/rosidl_runtime_rs/src/string/serde.rs +++ b/rosidl_runtime_rs/src/string/serde.rs @@ -1,13 +1,98 @@ -use serde::{de::Error, Deserialize, Deserializer, Serialize, Serializer}; +use std::fmt; -use super::{BoundedString, BoundedWString, String, WString}; +use serde::{ + de::{Error, SeqAccess, Visitor}, + Deserialize, Deserializer, Serialize, Serializer, +}; + +use super::{ + rosidl_runtime_c__String__assignn, rosidl_runtime_c__U16String__assignn, BoundedString, + BoundedWString, String, WString, +}; + +struct StringVisitor; +struct WStringVisitor; + +impl<'de> Visitor<'de> for StringVisitor { + type Value = String; + + fn expecting(&self, formatter: &mut fmt::Formatter) -> fmt::Result { + formatter.write_str("a string") + } + + fn visit_str(self, v: &str) -> Result + where + E: Error, + { + Ok(String::from(v)) + } + + fn visit_bytes(self, v: &[u8]) -> Result + where + E: Error, + { + let mut msg = String { + data: std::ptr::null_mut(), + size: 0, + capacity: 0, + }; + // SAFETY: This is doing the same thing as rosidl_runtime_c__String__copy. + unsafe { + rosidl_runtime_c__String__assignn(&mut msg, v.as_ptr() as *const _, v.len()); + } + Ok(msg) + } + + // We don't implement visit_bytes_buf, since the data in a string must always be managed by C. +} + +impl<'de> Visitor<'de> for WStringVisitor { + type Value = WString; + + fn expecting(&self, formatter: &mut fmt::Formatter) -> fmt::Result { + formatter.write_str("a string") + } + + fn visit_str(self, v: &str) -> Result + where + E: Error, + { + Ok(WString::from(v)) + } + + fn visit_seq(self, mut seq: A) -> Result + where + A: SeqAccess<'de>, + { + let mut buf = if let Some(size) = seq.size_hint() { + Vec::with_capacity(size) + } else { + Vec::new() + }; + while let Some(el) = seq.next_element::()? { + buf.push(el); + } + let mut msg = WString { + data: std::ptr::null_mut(), + size: 0, + capacity: 0, + }; + // SAFETY: This is doing the same thing as rosidl_runtime_c__U16String__copy. + unsafe { + rosidl_runtime_c__U16String__assignn(&mut msg, buf.as_ptr(), buf.len()); + } + Ok(msg) + } + + // We don't implement visit_bytes_buf, since the data in a string must always be managed by C. +} impl<'de> Deserialize<'de> for String { fn deserialize(deserializer: D) -> Result where D: Deserializer<'de>, { - std::string::String::deserialize(deserializer).map(|s| Self::from(s.as_str())) + deserializer.deserialize_string(StringVisitor) } } @@ -29,7 +114,7 @@ impl<'de> Deserialize<'de> for WString { where D: Deserializer<'de>, { - std::string::String::deserialize(deserializer).map(|s| Self::from(s.as_str())) + deserializer.deserialize_string(WStringVisitor) } } From 1a22b5279ce9aa709389ce4eb9891b74127b5644 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Wed, 29 Mar 2023 19:33:32 +0200 Subject: [PATCH 06/32] Fix hardcoded rmw_gid_t length (#309) --- rclrs/src/rcl_bindings.rs | 2 ++ rclrs/src/rcl_wrapper.h | 2 ++ rclrs/src/subscription/message_info.rs | 4 ++-- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 2a3b5f565..7dc838f5f 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -7,3 +7,5 @@ #![allow(missing_docs)] include!(concat!(env!("OUT_DIR"), "/rcl_bindings_generated.rs")); + +pub const RMW_GID_STORAGE_SIZE: usize = rmw_gid_storage_size_constant; diff --git a/rclrs/src/rcl_wrapper.h b/rclrs/src/rcl_wrapper.h index 7ff9b062c..fe97cb4e5 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -5,3 +5,5 @@ #include #include #include + +const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE; diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index 57da4ce7b..722089592 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -27,7 +27,7 @@ use crate::rcl_bindings::*; #[derive(Clone, Debug, PartialEq, Eq)] pub struct PublisherGid { /// Bytes identifying a publisher in the RMW implementation. - pub data: [u8; 24], + pub data: [u8; RMW_GID_STORAGE_SIZE], /// A string containing the RMW implementation's name. /// /// The `data` member only uniquely identifies the publisher within @@ -145,7 +145,7 @@ mod tests { publication_sequence_number: 0, reception_sequence_number: 0, publisher_gid: rmw_gid_t { - data: [0; 24], + data: [0; RMW_GID_STORAGE_SIZE], implementation_identifier: std::ptr::null(), }, from_intra_process: false, From 3d22cbe2d24a6854ef529aeca99a03bb3f787d2c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 30 Mar 2023 16:22:50 +0200 Subject: [PATCH 07/32] Use ros2-testing debs for rolling (#308) --- .github/workflows/rust.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index febb2c6f9..f413596e8 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -48,6 +48,7 @@ jobs: uses: ros-tooling/setup-ros@v0.6 with: required-ros-distributions: ${{ matrix.ros_distribution }} + use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} - name: Downgrade pydocstyle as a workaround for https://github.com/ament/ament_lint/issues/434 run: | From 047f48387adde48ed55b69506d43c0098ab3caad Mon Sep 17 00:00:00 2001 From: Sam Privett Date: Thu, 6 Apr 2023 07:13:03 -0700 Subject: [PATCH 08/32] Swapped usage of rosidl_cmake over to the new rosidl_pycommon. (#297) * Swapped usage of rosidl_cmake over to the new rosidl_pycommon. As of [rosidl 3.3.0](https://github.com/ros2/rosidl/commit/9348ce9b466335590dc334aab01f4f0dd270713b), the rosidl_cmake Python module was moved to a new rosidl_pycommon package and the Python module in rosidl_cmake was deprecated. * Support builds from older ROS 2 distros. * Fixed build for rolling * Added `test_depend` conditional inclusion as well. * Swap to a more elegant check * PR Feedback --------- Co-authored-by: Sam Privett --- rosidl_generator_rs/CMakeLists.txt | 7 ++++-- rosidl_generator_rs/package.xml | 10 +++++--- .../rosidl_generator_rs/__init__.py | 25 +++++++++---------- 3 files changed, 24 insertions(+), 18 deletions(-) diff --git a/rosidl_generator_rs/CMakeLists.txt b/rosidl_generator_rs/CMakeLists.txt index 9a29958ff..64e5577a4 100644 --- a/rosidl_generator_rs/CMakeLists.txt +++ b/rosidl_generator_rs/CMakeLists.txt @@ -4,12 +4,15 @@ project(rosidl_generator_rs) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(rosidl_cmake REQUIRED) find_package(rosidl_generator_c REQUIRED) find_package(rosidl_typesupport_interface REQUIRED) find_package(rosidl_typesupport_introspection_c REQUIRED) -ament_export_dependencies(rosidl_cmake) +if("$ENV{ROS_DISTRO}" STRLESS_EQUAL "humble") + find_package(rosidl_cmake REQUIRED) + ament_export_dependencies(rosidl_cmake) +endif() + ament_export_dependencies(rmw) ament_export_dependencies(rosidl_generator_c) diff --git a/rosidl_generator_rs/package.xml b/rosidl_generator_rs/package.xml index c12e46cad..18ff07f09 100644 --- a/rosidl_generator_rs/package.xml +++ b/rosidl_generator_rs/package.xml @@ -9,11 +9,14 @@ Esteve Fernandez ament_cmake - + ros_environment + rosidl_runtime_rs ament_cmake - rosidl_cmake + ros_environment + rosidl_cmake + rosidl_pycommon rosidl_runtime_rs rosidl_typesupport_c rosidl_typesupport_interface @@ -26,7 +29,8 @@ ament_cmake_gtest ament_lint_auto ament_lint_common - rosidl_cmake + rosidl_cmake + rosidl_pycommon rosidl_generator_c rosidl_generator_packages diff --git a/rosidl_generator_rs/rosidl_generator_rs/__init__.py b/rosidl_generator_rs/rosidl_generator_rs/__init__.py index 679fe9659..ae61a3e9f 100644 --- a/rosidl_generator_rs/rosidl_generator_rs/__init__.py +++ b/rosidl_generator_rs/rosidl_generator_rs/__init__.py @@ -18,11 +18,10 @@ from pathlib import Path -from rosidl_cmake import convert_camel_case_to_lower_case_underscore -from rosidl_cmake import expand_template -from rosidl_cmake import generate_files -from rosidl_cmake import get_newest_modification_time -from rosidl_cmake import read_generator_arguments +if os.environ['ROS_DISTRO'] <= 'humble': + import rosidl_cmake as rosidl_pycommon +else: + import rosidl_pycommon from rosidl_parser.definition import AbstractGenericString from rosidl_parser.definition import AbstractNestedType @@ -53,7 +52,7 @@ def convert_lower_case_underscore_to_camel_case(word): def generate_rs(generator_arguments_file, typesupport_impls): - args = read_generator_arguments(generator_arguments_file) + args = rosidl_pycommon.read_generator_arguments(generator_arguments_file) package_name = args['package_name'] # expand init modules for each directory @@ -108,7 +107,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): 'constant_value_to_rs': constant_value_to_rs, 'value_to_rs': value_to_rs, 'convert_camel_case_to_lower_case_underscore': - convert_camel_case_to_lower_case_underscore, + rosidl_pycommon.convert_camel_case_to_lower_case_underscore, 'convert_lower_case_underscore_to_camel_case': convert_lower_case_underscore_to_camel_case, 'msg_specs': [], @@ -118,7 +117,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): 'interface_path': idl_rel_path, } - latest_target_timestamp = get_newest_modification_time( + latest_target_timestamp = rosidl_pycommon.get_newest_modification_time( args['target_dependencies']) for message in idl_content.get_elements_of_type(Message): @@ -132,7 +131,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): for generated_filename in generated_filenames: generated_file = os.path.join(args['output_dir'], generated_filename % 'msg') - expand_template( + rosidl_pycommon.expand_template( os.path.join(template_dir, template_file), data.copy(), generated_file, @@ -143,13 +142,13 @@ def generate_rs(generator_arguments_file, typesupport_impls): for generated_filename in generated_filenames: generated_file = os.path.join(args['output_dir'], generated_filename % 'srv') - expand_template( + rosidl_pycommon.expand_template( os.path.join(template_dir, template_file), data.copy(), generated_file, minimum_timestamp=latest_target_timestamp) - expand_template( + rosidl_pycommon.expand_template( os.path.join(template_dir, 'lib.rs.em'), data.copy(), os.path.join(args['output_dir'], 'rust/src/lib.rs'), @@ -160,13 +159,13 @@ def generate_rs(generator_arguments_file, typesupport_impls): 'package_name': args['package_name'], 'package_version': args['package_version'], } - expand_template( + rosidl_pycommon.expand_template( os.path.join(template_dir, 'Cargo.toml.em'), cargo_toml_data, os.path.join(args['output_dir'], 'rust/Cargo.toml'), minimum_timestamp=latest_target_timestamp) - expand_template( + rosidl_pycommon.expand_template( os.path.join(template_dir, 'build.rs.em'), {}, os.path.join(args['output_dir'], 'rust/build.rs'), From 8d5d57e5f426f8e071af27a8855d113189f11807 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Thu, 6 Jul 2023 18:17:07 +0200 Subject: [PATCH 09/32] Remove Foxy from the list of supported ROS 2 distributions (#318) --- .github/workflows/rust.yml | 5 ----- Dockerfile | 2 +- README.md | 6 +++--- docs/building.md | 14 +++++++------- rclrs/src/dynamic_message.rs | 3 --- rclrs/src/lib.rs | 5 ----- rclrs/src/subscription/message_info.rs | 5 ----- ros2_rust_foxy.repos | 25 ------------------------- 8 files changed, 11 insertions(+), 54 deletions(-) delete mode 100644 ros2_rust_foxy.repos diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index f413596e8..662ef293a 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -16,14 +16,9 @@ jobs: strategy: matrix: ros_distribution: - - foxy - humble - rolling include: - # Foxy Fitzroy (June 2020 - May 2023) - - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest - ros_distribution: foxy - ros_version: 2 # Humble Hawksbill (May 2022 - May 2027) - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest ros_distribution: humble diff --git a/Dockerfile b/Dockerfile index 692bdc969..e3929e089 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM ros:foxy as base +FROM ros:humble as base ARG DEBIAN_FRONTEND=noninteractive # Install dependencies diff --git a/README.md b/README.md index 41657dd76..7fb17a8db 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,7 @@ Here are the steps for building the `ros2_rust` examples in a vanilla Ubuntu Foc ```shell # Install Rust, e.g. as described in https://rustup.rs/ -# Install ROS 2 as described in https://docs.ros.org/en/foxy/Installation.html +# Install ROS 2 as described in https://docs.ros.org/en/humble/Installation.html # Assuming you installed the minimal version of ROS 2, you need these additional packages: sudo apt install -y git libclang-dev python3-pip python3-vcstool # libclang-dev is required by bindgen # Install these plugins for cargo and colcon: @@ -43,8 +43,8 @@ pip install git+https://github.com/colcon/colcon-ros-cargo.git mkdir -p workspace/src && cd workspace git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust -vcs import src < src/ros2_rust/ros2_rust_foxy.repos -. /opt/ros/foxy/setup.sh +vcs import src < src/ros2_rust/ros2_rust_humble.repos +. /opt/ros/humble/setup.sh colcon build ``` diff --git a/docs/building.md b/docs/building.md index 4202063d8..c57400b1c 100644 --- a/docs/building.md +++ b/docs/building.md @@ -2,7 +2,7 @@ This is a more detailed guide on how to build ROS 2 packages written in Rust that expands on the minimal steps in the README. -In this guide, the Foxy distribution of ROS 2 is used, but newer distributions can be used by simply replacing 'foxy' with the distribution name everywhere. +In this guide, the Humble distribution of ROS 2 is used, but newer distributions can be used by simply replacing 'humble' with the distribution name everywhere. ## Choosing a workspace directory and cloning `ros2_rust` @@ -24,7 +24,7 @@ git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust ## Environment setup -Building `rclrs` requires a standard [ROS 2 installation](https://docs.ros.org/en/foxy/Installation.html), and a few extensions. +Building `rclrs` requires a standard [ROS 2 installation](https://docs.ros.org/en/humble/Installation.html), and a few extensions. These extensions are: `colcon-cargo`, `colcon-ros-cargo`, `cargo-ament-build`. The first two are `colcon` plugins, and the third is a `cargo` plugin. The `libclang` library is also required for automatically generating FFI bindings with `bindgen`. See the [`bindgen` docs](https://rust-lang.github.io/rust-bindgen/requirements.html) on how to install it. As a side note, on Ubuntu the `clang` package is not required, only the `libclang-dev` package. @@ -41,7 +41,7 @@ The exact steps may differ between platforms, but as an example, here is how you ```shell # Install Rust, e.g. as described in https://rustup.rs/ -# Install ROS 2 as described in https://docs.ros.org/en/foxy/Installation.html +# Install ROS 2 as described in https://docs.ros.org/en/humble/Installation.html # Assuming you installed the minimal version of ROS 2, you need these additional packages: sudo apt install -y git libclang-dev python3-pip python3-vcstool # libclang-dev is required by bindgen # Install these plugins for cargo and colcon: @@ -75,7 +75,7 @@ As you can see, this maps the workspace directory to `/workspace` inside the con ```shell # Make sure to run this in the workspace directory -vcs import src < src/ros2_rust/ros2_rust_foxy.repos +vcs import src < src/ros2_rust/ros2_rust_humble.repos ``` This uses the [`vcs` tool](https://github.com/dirk-thomas/vcstool), which is preinstalled in the Docker image. @@ -87,13 +87,13 @@ The list of needed repositories should very rarely change, so you typically only ### Sourcing the ROS 2 installation Before building, the ROS 2 installation, and ideally _only_ the ROS 2 installation, should be sourced. -Sourcing an installation populates a few environment variables such as `$AMENT_PREFIX_PATH`, so if you are not sure, you can check that the `$AMENT_PREFIX_PATH` environment variable contains the ROS 2 installation, e.g. `/opt/ros/foxy`. +Sourcing an installation populates a few environment variables such as `$AMENT_PREFIX_PATH`, so if you are not sure, you can check that the `$AMENT_PREFIX_PATH` environment variable contains the ROS 2 installation, e.g. `/opt/ros/humble`. In the pre-made Docker image, sourcing is already done for you. Otherwise, if `$AMENT_PREFIX_PATH` is empty, you need to source the ROS 2 installation yourself: ```shell -# You can also do `source /opt/ros/foxy/setup.bash` in bash -. /opt/ros/foxy/setup.sh +# You can also do `source /opt/ros/humble/setup.bash` in bash +. /opt/ros/humble/setup.sh ```` If `$AMENT_PREFIX_PATH` contains undesired paths, exit and reopen your shell. If the problem persists, it's probably because of a sourcing command in your `~/.bashrc` or similar. diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index 3997ad6e7..ba6dce1fc 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -9,9 +9,6 @@ use std::fmt::{self, Display}; use std::path::PathBuf; use std::sync::Arc; -#[cfg(ros_distro = "foxy")] -use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers as rosidl_message_members_t; -#[cfg(not(ros_distro = "foxy"))] use crate::rcl_bindings::rosidl_typesupport_introspection_c__MessageMembers_s as rosidl_message_members_t; use crate::rcl_bindings::*; diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 2cf90e843..4fd69a84e 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -73,11 +73,6 @@ pub fn spin_once(node: &Node, timeout: Option) -> Result<(), RclrsErro /// This function additionally checks that the context is still valid. pub fn spin(node: &Node) -> Result<(), RclrsError> { // The context_is_valid functions exists only to abstract away ROS distro differences - #[cfg(ros_distro = "foxy")] - // SAFETY: No preconditions for this function. - let context_is_valid = - || unsafe { rcl_context_is_valid(&mut *node.rcl_context_mtx.lock().unwrap()) }; - #[cfg(not(ros_distro = "foxy"))] // SAFETY: No preconditions for this function. let context_is_valid = || unsafe { rcl_context_is_valid(&*node.rcl_context_mtx.lock().unwrap()) }; diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index 722089592..1c2ba9936 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -80,7 +80,6 @@ pub struct MessageInfo { /// received messages. /// Those might have already been taken by other messages that were received in between or lost. /// `psn2 - psn1 - 1 = 0` if and only if the messages were sent by the publisher consecutively. - #[cfg(not(ros_distro = "foxy"))] pub publication_sequence_number: u64, /// Sequence number of the received message set by the subscription. /// @@ -98,7 +97,6 @@ pub struct MessageInfo { /// /// - `rsn2 > rsn1` (except in the case of a wrap around) /// - `rsn2 = rsn1 + 1` if and only if both messages were received consecutively. - #[cfg(not(ros_distro = "foxy"))] pub reception_sequence_number: u64, /// An identifier for the publisher that sent the message. pub publisher_gid: PublisherGid, @@ -123,9 +121,7 @@ impl MessageInfo { Self { source_timestamp, received_timestamp, - #[cfg(not(ros_distro = "foxy"))] publication_sequence_number: rmw_message_info.publication_sequence_number, - #[cfg(not(ros_distro = "foxy"))] reception_sequence_number: rmw_message_info.reception_sequence_number, publisher_gid, } @@ -136,7 +132,6 @@ impl MessageInfo { mod tests { use super::*; - #[cfg(not(ros_distro = "foxy"))] #[test] fn negative_durations() { let rmw_message_info = rmw_message_info_t { diff --git a/ros2_rust_foxy.repos b/ros2_rust_foxy.repos deleted file mode 100644 index 3d70dc920..000000000 --- a/ros2_rust_foxy.repos +++ /dev/null @@ -1,25 +0,0 @@ -repositories: - ros2/common_interfaces: - type: git - url: https://github.com/ros2/common_interfaces.git - version: foxy - ros2/example_interfaces: - type: git - url: https://github.com/ros2/example_interfaces.git - version: foxy - ros2/rcl_interfaces: - type: git - url: https://github.com/ros2/rcl_interfaces.git - version: foxy - ros2/test_interface_files: - type: git - url: https://github.com/ros2/test_interface_files.git - version: foxy - ros2/rosidl_defaults: - type: git - url: https://github.com/ros2/rosidl_defaults.git - version: foxy - ros2/unique_identifier_msgs: - type: git - url: https://github.com/ros2/unique_identifier_msgs.git - version: foxy From 50bf305e0d66e671cc22e0264e868730206f1ecf Mon Sep 17 00:00:00 2001 From: Uwe Arzt Date: Thu, 13 Jul 2023 13:04:14 +0200 Subject: [PATCH 10/32] Allow usage of different ROS distributions in Dockerfile (#313) * Allow usage of different ROS distributions in Dockerfile * iron docker is working too --- Dockerfile | 3 ++- docs/building.md | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Dockerfile b/Dockerfile index e3929e089..b0a282995 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,5 @@ -FROM ros:humble as base +ARG ROS_DISTRO=humble +FROM ros:$ROS_DISTRO as base ARG DEBIAN_FRONTEND=noninteractive # Install dependencies diff --git a/docs/building.md b/docs/building.md index c57400b1c..8bcfe9b81 100644 --- a/docs/building.md +++ b/docs/building.md @@ -56,7 +56,8 @@ Build the Docker image with ```shell # Make sure to run this in the workspace directory -docker build -t ros2_rust_dev - < src/ros2_rust/Dockerfile +# ROS_DISTRO can be humble|iron|rolling +docker build -f src/ros2_rust/Dockerfile --build-arg "ROS_DISTRO=humble" -t ros2_rust_dev ``` and then run it with From 1279ef232b36e6959579772c7e2157e1904472ad Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Sat, 15 Jul 2023 10:26:22 +0200 Subject: [PATCH 11/32] Disable clippy warnings for u128 types (#322) * Disable clippy warnings for u128 types * Added note about the rationale for improper_ctypes for clippy * Fix formatting --- rclrs/src/rcl_bindings.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 7dc838f5f..17496b662 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -3,6 +3,10 @@ #![allow(non_upper_case_globals)] #![allow(non_camel_case_types)] #![allow(non_snake_case)] +// Added to avoid clippy complaining about u128 types not being FFI safe +// Caused by https://github.com/ros2/ros2/issues/1374 in iron and newer +// See https://github.com/ros2-rust/ros2_rust/issues/320 +#![allow(improper_ctypes)] #![allow(clippy::all)] #![allow(missing_docs)] From a5e90dae075ceec1c4f7b758c8606f69fc310d07 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Sat, 15 Jul 2023 10:27:45 +0200 Subject: [PATCH 12/32] Update Rust toolchain to 1.71.0 and remove pydocstyle workaround (#321) * Update Rust toolchain to 1.70.0 and remove pydocstyle workaround * Fix clippy warnings * Update to Rust 1.71.0 --- .github/workflows/rust.yml | 6 +----- Dockerfile | 2 +- rclrs/src/dynamic_message.rs | 2 +- rclrs/src/error.rs | 2 +- rclrs/src/node.rs | 2 +- rclrs/src/parameter/value.rs | 4 ++-- rclrs/src/subscription.rs | 2 +- rclrs/src/vendor/rcl_interfaces/msg.rs | 5 +++++ rclrs/vendor_interfaces.py | 2 +- 9 files changed, 14 insertions(+), 13 deletions(-) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 662ef293a..dc0336ace 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -45,12 +45,8 @@ jobs: required-ros-distributions: ${{ matrix.ros_distribution }} use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} - - name: Downgrade pydocstyle as a workaround for https://github.com/ament/ament_lint/issues/434 - run: | - sudo pip install pydocstyle==6.1.1 - - name: Setup Rust - uses: dtolnay/rust-toolchain@1.63.0 + uses: dtolnay/rust-toolchain@1.71.0 with: components: clippy, rustfmt diff --git a/Dockerfile b/Dockerfile index b0a282995..81a3fa61f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -12,7 +12,7 @@ RUN apt-get update && apt-get install -y \ && rm -rf /var/lib/apt/lists/* # Install Rust and the cargo-ament-build plugin -RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.63.0 -y +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.71.0 -y ENV PATH=/root/.cargo/bin:$PATH RUN cargo install cargo-ament-build diff --git a/rclrs/src/dynamic_message.rs b/rclrs/src/dynamic_message.rs index ba6dce1fc..9bbcf7e75 100644 --- a/rclrs/src/dynamic_message.rs +++ b/rclrs/src/dynamic_message.rs @@ -72,7 +72,7 @@ fn get_type_support_library( let ament = ament_rs::Ament::new().map_err(|_| RequiredPrefixNotSourced { package: package_name.to_owned(), })?; - let prefix = PathBuf::from(ament.find_package(&package_name).ok_or( + let prefix = PathBuf::from(ament.find_package(package_name).ok_or( RequiredPrefixNotSourced { package: package_name.to_owned(), }, diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index b84f4d4d3..84bc602b1 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -347,6 +347,6 @@ pub(crate) trait ToResult { impl ToResult for rcl_ret_t { fn ok(&self) -> Result<(), RclrsError> { - to_rclrs_result(*self as i32) + to_rclrs_result(*self) } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 4df07ba90..52fd3b3cf 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -173,7 +173,7 @@ impl Node { &self, getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, ) -> String { - unsafe { call_string_getter_with_handle(&*self.rcl_node_mtx.lock().unwrap(), getter) } + unsafe { call_string_getter_with_handle(&self.rcl_node_mtx.lock().unwrap(), getter) } } /// Creates a [`Client`][1]. diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 04b64322a..b49108a6d 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -163,8 +163,8 @@ mod tests { assert!(!rcl_params.is_null()); assert_eq!(unsafe { (*rcl_params).num_nodes }, 1); let rcl_node_params = unsafe { &(*(*rcl_params).params) }; - assert_eq!((*rcl_node_params).num_params, 1); - let rcl_variant = unsafe { &(*(*rcl_node_params).parameter_values) }; + assert_eq!(rcl_node_params.num_params, 1); + let rcl_variant = unsafe { &(*rcl_node_params.parameter_values) }; let param_value = unsafe { ParameterValue::from_rcl_variant(rcl_variant) }; assert_eq!(param_value, pair.1); unsafe { rcl_yaml_node_struct_fini(rcl_params) }; diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 9e551becd..e684866c2 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -183,7 +183,7 @@ where /// /// This can be more efficient for messages containing large arrays. pub fn take_boxed(&self) -> Result<(Box, MessageInfo), RclrsError> { - let mut rmw_message = Box::new(::RmwMsg::default()); + let mut rmw_message = Box::<::RmwMsg>::default(); let message_info = self.take_inner(&mut *rmw_message)?; // TODO: This will still use the stack in general. Change signature of // from_rmw_message to allow placing the result in a Box directly. diff --git a/rclrs/src/vendor/rcl_interfaces/msg.rs b/rclrs/src/vendor/rcl_interfaces/msg.rs index c3f474b19..39c029ae1 100644 --- a/rclrs/src/vendor/rcl_interfaces/msg.rs +++ b/rclrs/src/vendor/rcl_interfaces/msg.rs @@ -404,6 +404,7 @@ pub mod rmw { pub description: rosidl_runtime_rs::String, pub additional_constraints: rosidl_runtime_rs::String, pub read_only: bool, + pub dynamic_typing: bool, pub floating_point_range: rosidl_runtime_rs::BoundedSequence< crate::vendor::rcl_interfaces::msg::rmw::FloatingPointRange, 1, @@ -1269,6 +1270,7 @@ pub struct ParameterDescriptor { pub description: std::string::String, pub additional_constraints: std::string::String, pub read_only: bool, + pub dynamic_typing: bool, pub floating_point_range: rosidl_runtime_rs::BoundedSequence< crate::vendor::rcl_interfaces::msg::rmw::FloatingPointRange, 1, @@ -1298,6 +1300,7 @@ impl rosidl_runtime_rs::Message for ParameterDescriptor { description: msg.description.as_str().into(), additional_constraints: msg.additional_constraints.as_str().into(), read_only: msg.read_only, + dynamic_typing: msg.dynamic_typing, floating_point_range: msg.floating_point_range, integer_range: msg.integer_range, }), @@ -1307,6 +1310,7 @@ impl rosidl_runtime_rs::Message for ParameterDescriptor { description: msg.description.as_str().into(), additional_constraints: msg.additional_constraints.as_str().into(), read_only: msg.read_only, + dynamic_typing: msg.dynamic_typing, floating_point_range: msg.floating_point_range.clone(), integer_range: msg.integer_range.clone(), }), @@ -1320,6 +1324,7 @@ impl rosidl_runtime_rs::Message for ParameterDescriptor { description: msg.description.to_string(), additional_constraints: msg.additional_constraints.to_string(), read_only: msg.read_only, + dynamic_typing: msg.dynamic_typing, floating_point_range: msg.floating_point_range, integer_range: msg.integer_range, } diff --git a/rclrs/vendor_interfaces.py b/rclrs/vendor_interfaces.py index 59a840d58..37d01e2f5 100644 --- a/rclrs/vendor_interfaces.py +++ b/rclrs/vendor_interfaces.py @@ -58,4 +58,4 @@ def main(): if __name__ == '__main__': - main() \ No newline at end of file + main() From 11b0ff4d96be626380035c83c8d0445e5a8a8ff9 Mon Sep 17 00:00:00 2001 From: Sam Privett Date: Sat, 15 Jul 2023 11:38:35 -0700 Subject: [PATCH 13/32] Add support for Iron Irwini (#319) Co-authored-by: Sam Privett --- .github/workflows/rust.yml | 7 ++++++- ros2_rust_iron.repos | 29 +++++++++++++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 ros2_rust_iron.repos diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index dc0336ace..29fa1bee2 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -17,12 +17,17 @@ jobs: matrix: ros_distribution: - humble + - iron - rolling include: # Humble Hawksbill (May 2022 - May 2027) - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest ros_distribution: humble ros_version: 2 + # Iron Irwini (May 2023 - November 2024) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest + ros_distribution: iron + ros_version: 2 # Rolling Ridley (June 2020 - Present) - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest ros_distribution: rolling @@ -69,7 +74,7 @@ jobs: - name: Build and test id: build - uses: ros-tooling/action-ros-ci@v0.2 + uses: ros-tooling/action-ros-ci@v0.3 with: package-name: ${{ steps.list_packages.outputs.package_list }} target-ros2-distro: ${{ matrix.ros_distribution }} diff --git a/ros2_rust_iron.repos b/ros2_rust_iron.repos new file mode 100644 index 000000000..caa51bdfe --- /dev/null +++ b/ros2_rust_iron.repos @@ -0,0 +1,29 @@ +repositories: + ros2/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: iron + ros2/example_interfaces: + type: git + url: https://github.com/ros2/example_interfaces.git + version: iron + ros2/rcl_interfaces: + type: git + url: https://github.com/ros2/rcl_interfaces.git + version: iron + ros2/test_interface_files: + type: git + url: https://github.com/ros2/test_interface_files.git + version: iron + ros2/rosidl_core: + type: git + url: https://github.com/ros2/rosidl_core.git + version: iron + ros2/rosidl_defaults: + type: git + url: https://github.com/ros2/rosidl_defaults.git + version: iron + ros2/unique_identifier_msgs: + type: git + url: https://github.com/ros2/unique_identifier_msgs.git + version: iron From f9e72631f0a8d2a43085d7619d189e61129540f0 Mon Sep 17 00:00:00 2001 From: Sam Privett Date: Mon, 24 Jul 2023 08:05:42 -0700 Subject: [PATCH 14/32] - Upgraded bindgen to 0.66.1 (#323) - Upgraded libloading to 0.8 - Added instructions to rerun build scripts if dependent files have changed - Enabled bindgen to invalidate the built crate whenever any transitively included header files from the wrapper change - Removed some default true options from our bindgen builder Co-authored-by: Sam Privett --- rclrs/Cargo.toml | 4 ++-- rclrs/build.rs | 12 ++++++++---- rosidl_runtime_rs/build.rs | 3 +++ 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 9ad6d8659..c7451db07 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -19,7 +19,7 @@ ament_rs = { version = "0.2", optional = true } # Needed for clients futures = "0.3" # Needed for dynamic messages -libloading = { version = "0.7", optional = true } +libloading = { version = "0.8", optional = true } # Needed for the Message trait, among others rosidl_runtime_rs = "0.3" @@ -29,7 +29,7 @@ tempfile = "3.3.0" [build-dependencies] # Needed for FFI -bindgen = "0.59.1" +bindgen = "0.66.1" [features] dyn_msg = ["ament_rs", "libloading"] diff --git a/rclrs/build.rs b/rclrs/build.rs index 4f8458db0..fe8cd8b9a 100644 --- a/rclrs/build.rs +++ b/rclrs/build.rs @@ -4,6 +4,7 @@ use std::path::{Path, PathBuf}; const AMENT_PREFIX_PATH: &str = "AMENT_PREFIX_PATH"; const ROS_DISTRO: &str = "ROS_DISTRO"; +const BINDGEN_WRAPPER: &str = "src/rcl_wrapper.h"; fn get_env_var_or_abort(env_var: &'static str) -> String { if let Ok(value) = env::var(env_var) { @@ -21,9 +22,8 @@ fn main() { println!("cargo:rustc-cfg=ros_distro=\"{ros_distro}\""); let mut builder = bindgen::Builder::default() - .header("src/rcl_wrapper.h") + .header(BINDGEN_WRAPPER) .derive_copy(false) - .allowlist_recursively(true) .allowlist_type("rcl_.*") .allowlist_type("rmw_.*") .allowlist_type("rcutils_.*") @@ -37,10 +37,14 @@ fn main() { .allowlist_var("rcutils_.*") .allowlist_var("rosidl_.*") .layout_tests(false) - .size_t_is_usize(true) .default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false, - }); + }) + .parse_callbacks(Box::new(bindgen::CargoCallbacks)); + + // Invalidate the built crate whenever this script or the wrapper changes + println!("cargo:rerun-if-changed=build.rs"); + println!("cargo:rerun-if-changed={BINDGEN_WRAPPER}"); // ############# // # ALGORITHM # diff --git a/rosidl_runtime_rs/build.rs b/rosidl_runtime_rs/build.rs index fbc3c85cf..b895cd24b 100644 --- a/rosidl_runtime_rs/build.rs +++ b/rosidl_runtime_rs/build.rs @@ -20,4 +20,7 @@ fn main() { let library_path = Path::new(ament_prefix_path).join("lib"); println!("cargo:rustc-link-search=native={}", library_path.display()); } + + // Invalidate the built crate whenever this script changes + println!("cargo:rerun-if-changed=build.rs"); } From 7450810daed4855b7c5a68113ed21bf162df6045 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 25 Jul 2023 22:40:29 +0200 Subject: [PATCH 15/32] Return Arc from the create_node function to match other create_X functions (#294) * Fine-grained locks. Made create_subscription, create_service, create_client not take a mutable self * Return an Arc from rclrs::create_node to match other create_X functions * Update spin* to take an Arc * Fix clippy warning --- docs/writing-your-first-rclrs-node.md | 10 ++--- examples/message_demo/src/message_demo.rs | 7 ++-- .../src/minimal_client.rs | 4 +- .../src/minimal_client_async.rs | 4 +- .../src/minimal_service.rs | 4 +- .../minimal_pub_sub/src/minimal_subscriber.rs | 4 +- .../src/zero_copy_subscriber.rs | 4 +- rclrs/src/lib.rs | 19 +++++---- rclrs/src/node.rs | 42 +++++++++++-------- rclrs/src/node/builder.rs | 8 ++-- rclrs_tests/src/graph_tests.rs | 8 ++-- 11 files changed, 62 insertions(+), 52 deletions(-) diff --git a/docs/writing-your-first-rclrs-node.md b/docs/writing-your-first-rclrs-node.md index 4af713bf0..b6b1dadd1 100644 --- a/docs/writing-your-first-rclrs-node.md +++ b/docs/writing-your-first-rclrs-node.md @@ -54,7 +54,7 @@ struct RepublisherNode { impl RepublisherNode { fn new(context: &rclrs::Context) -> Result { - let mut node = rclrs::Node::new(context, "republisher")?; + let node = rclrs::Node::new(context, "republisher")?; let data = None; let _subscription = node.create_subscription( "in_topic", @@ -76,7 +76,7 @@ Next, add a main function to launch it: fn main() -> Result<(), rclrs::RclrsError> { let context = rclrs::Context::new(std::env::args())?; let republisher = RepublisherNode::new(&context)?; - rclrs::spin(&republisher.node) + rclrs::spin(republisher.node) } ``` @@ -121,7 +121,7 @@ struct RepublisherNode { impl RepublisherNode { fn new(context: &rclrs::Context) -> Result { - let mut node = rclrs::Node::new(context, "republisher")?; + let node = rclrs::Node::new(context, "republisher")?; let data = Arc::new(Mutex::new(None)); // (3) let data_cb = Arc::clone(&data); let _subscription = { @@ -190,7 +190,7 @@ fn main() -> Result<(), rclrs::RclrsError> { republisher.republish()?; } }); - rclrs::spin(&republisher.node) + rclrs::spin(republisher.node) } ``` @@ -212,7 +212,7 @@ fn main() -> Result<(), rclrs::RclrsError> { republisher_other_thread.republish()?; } }); - rclrs::spin(&republisher.node) + rclrs::spin(republisher.node) } ``` diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 1f7949216..76d46f67a 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -1,5 +1,6 @@ use std::convert::TryInto; use std::env; +use std::sync::Arc; use anyhow::{Error, Result}; use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; @@ -132,7 +133,7 @@ fn demonstrate_pubsub() -> Result<(), Error> { println!("================== Interoperability demo =================="); // Demonstrate interoperability between idiomatic and RMW-native message types let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "message_demo")?; + let node = rclrs::create_node(&context, "message_demo")?; let idiomatic_publisher = node.create_publisher::( "topic", @@ -159,10 +160,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - rclrs::spin_once(&node, None)?; + rclrs::spin_once(Arc::clone(&node), None)?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - rclrs::spin_once(&node, None)?; + rclrs::spin_once(Arc::clone(&node), None)?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 685dbf6a9..c91cedc91 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -5,7 +5,7 @@ use anyhow::{Error, Result}; fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_client")?; + let node = rclrs::create_node(&context, "minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -28,5 +28,5 @@ fn main() -> Result<(), Error> { std::thread::sleep(std::time::Duration::from_millis(500)); println!("Waiting for response"); - rclrs::spin(&node).map_err(|err| err.into()) + rclrs::spin(node).map_err(|err| err.into()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 38f55f122..26f41e077 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -6,7 +6,7 @@ use anyhow::{Error, Result}; async fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_client")?; + let node = rclrs::create_node(&context, "minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -20,7 +20,7 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); - let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(&node)); + let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(node)); let response = future.await?; println!( diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index 0150aeaa5..b4149c817 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -15,11 +15,11 @@ fn handle_service( fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_service")?; + let node = rclrs::create_node(&context, "minimal_service")?; let _server = node .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - rclrs::spin(&node).map_err(|err| err.into()) + rclrs::spin(node).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index eb34a91bc..ebc5fc194 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -5,7 +5,7 @@ use anyhow::{Error, Result}; fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = rclrs::create_node(&context, "minimal_subscriber")?; let mut num_messages: usize = 0; @@ -19,5 +19,5 @@ fn main() -> Result<(), Error> { }, )?; - rclrs::spin(&node).map_err(|err| err.into()) + rclrs::spin(node).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index b71faec25..9551dba0e 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -5,7 +5,7 @@ use anyhow::{Error, Result}; fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let mut node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = rclrs::create_node(&context, "minimal_subscriber")?; let mut num_messages: usize = 0; @@ -19,5 +19,5 @@ fn main() -> Result<(), Error> { }, )?; - rclrs::spin(&node).map_err(|err| err.into()) + rclrs::spin(node).map_err(|err| err.into()) } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4fd69a84e..cda4a3823 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -23,6 +23,7 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; +use std::sync::Arc; use std::time::Duration; pub use arguments::*; @@ -49,8 +50,8 @@ pub use wait::*; /// This can usually be ignored. /// /// [1]: crate::RclReturnCode -pub fn spin_once(node: &Node, timeout: Option) -> Result<(), RclrsError> { - let wait_set = WaitSet::new_for_node(node)?; +pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { + let wait_set = WaitSet::new_for_node(&node)?; let ready_entities = wait_set.wait(timeout)?; for ready_subscription in ready_entities.subscriptions { @@ -71,14 +72,16 @@ pub fn spin_once(node: &Node, timeout: Option) -> Result<(), RclrsErro /// Convenience function for calling [`spin_once`] in a loop. /// /// This function additionally checks that the context is still valid. -pub fn spin(node: &Node) -> Result<(), RclrsError> { +pub fn spin(node: Arc) -> Result<(), RclrsError> { // The context_is_valid functions exists only to abstract away ROS distro differences // SAFETY: No preconditions for this function. - let context_is_valid = - || unsafe { rcl_context_is_valid(&*node.rcl_context_mtx.lock().unwrap()) }; + let context_is_valid = { + let node = Arc::clone(&node); + move || unsafe { rcl_context_is_valid(&*node.rcl_context_mtx.lock().unwrap()) } + }; while context_is_valid() { - match spin_once(node, None) { + match spin_once(Arc::clone(&node), None) { Ok(_) | Err(RclrsError::RclError { code: RclReturnCode::Timeout, @@ -105,8 +108,8 @@ pub fn spin(node: &Node) -> Result<(), RclrsError> { /// assert!(node.is_ok()); /// # Ok::<(), RclrsError>(()) /// ``` -pub fn create_node(context: &Context, node_name: &str) -> Result { - Node::builder(context, node_name).build() +pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { + Ok(Arc::new(Node::builder(context, node_name).build()?)) } /// Creates a [`NodeBuilder`][1]. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 52fd3b3cf..74eb5ac83 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -67,10 +67,10 @@ unsafe impl Send for rcl_node_t {} pub struct Node { pub(crate) rcl_node_mtx: Arc>, pub(crate) rcl_context_mtx: Arc>, - pub(crate) clients: Vec>, - pub(crate) guard_conditions: Vec>, - pub(crate) services: Vec>, - pub(crate) subscriptions: Vec>, + pub(crate) clients_mtx: Mutex>>, + pub(crate) guard_conditions_mtx: Mutex>>, + pub(crate) services_mtx: Mutex>>, + pub(crate) subscriptions_mtx: Mutex>>, _parameter_map: ParameterOverrideMap, } @@ -180,13 +180,12 @@ impl Node { /// /// [1]: crate::Client // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&mut self, topic: &str) -> Result>, RclrsError> + pub fn create_client(&self, topic: &str) -> Result>, RclrsError> where T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.rcl_node_mtx), topic)?); - self.clients - .push(Arc::downgrade(&client) as Weak); + { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -199,12 +198,12 @@ impl Node { /// /// [1]: crate::GuardCondition /// [2]: crate::spin_once - pub fn create_guard_condition(&mut self) -> Arc { + pub fn create_guard_condition(&self) -> Arc { let guard_condition = Arc::new(GuardCondition::new_with_rcl_context( &mut self.rcl_context_mtx.lock().unwrap(), None, )); - self.guard_conditions + { self.guard_conditions_mtx.lock().unwrap() } .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -226,7 +225,7 @@ impl Node { &mut self.rcl_context_mtx.lock().unwrap(), Some(Box::new(callback) as Box), )); - self.guard_conditions + { self.guard_conditions_mtx.lock().unwrap() } .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -251,7 +250,7 @@ impl Node { /// [1]: crate::Service // TODO: make service's lifetime depend on node's lifetime pub fn create_service( - &mut self, + &self, topic: &str, callback: F, ) -> Result>, RclrsError> @@ -264,7 +263,7 @@ impl Node { topic, callback, )?); - self.services + { self.services_mtx.lock().unwrap() } .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -274,7 +273,7 @@ impl Node { /// [1]: crate::Subscription // TODO: make subscription's lifetime depend on node's lifetime pub fn create_subscription( - &mut self, + &self, topic: &str, qos: QoSProfile, callback: impl SubscriptionCallback, @@ -288,32 +287,39 @@ impl Node { qos, callback, )?); - self.subscriptions + { self.subscriptions_mtx.lock() } + .unwrap() .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 + { self.subscriptions_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_clients(&self) -> Vec> { - self.clients.iter().filter_map(Weak::upgrade).collect() + { self.clients_mtx.lock().unwrap() } + .iter() + .filter_map(Weak::upgrade) + .collect() } pub(crate) fn live_guard_conditions(&self) -> Vec> { - self.guard_conditions + { self.guard_conditions_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_services(&self) -> Vec> { - self.services.iter().filter_map(Weak::upgrade).collect() + { self.services_mtx.lock().unwrap() } + .iter() + .filter_map(Weak::upgrade) + .collect() } /// Returns the ROS domain ID that the node is using. diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 91cd6fbe3..60e3afe27 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -275,10 +275,10 @@ impl NodeBuilder { Ok(Node { rcl_node_mtx, rcl_context_mtx: self.context.clone(), - clients: vec![], - guard_conditions: vec![], - services: vec![], - subscriptions: vec![], + clients_mtx: Mutex::new(vec![]), + guard_conditions_mtx: Mutex::new(vec![]), + services_mtx: Mutex::new(vec![]), + subscriptions_mtx: Mutex::new(vec![]), _parameter_map, }) } diff --git a/rclrs_tests/src/graph_tests.rs b/rclrs_tests/src/graph_tests.rs index e8e2d7a0c..b32447ca1 100644 --- a/rclrs_tests/src/graph_tests.rs +++ b/rclrs_tests/src/graph_tests.rs @@ -85,7 +85,7 @@ fn test_publishers() -> Result<(), RclrsError> { #[test] fn test_subscriptions() -> Result<(), RclrsError> { let namespace = "/test_subscriptions_graph"; - let mut graph = construct_test_graph(namespace)?; + let graph = construct_test_graph(namespace)?; let node_2_empty_subscription = graph.node2.create_subscription::( "graph_test_topic_1", @@ -149,7 +149,7 @@ fn test_subscriptions() -> Result<(), RclrsError> { #[test] fn test_topic_names_and_types() -> Result<(), RclrsError> { - let mut graph = construct_test_graph("test_topics_graph")?; + let graph = construct_test_graph("test_topics_graph")?; let _node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", @@ -191,7 +191,7 @@ fn test_topic_names_and_types() -> Result<(), RclrsError> { #[test] fn test_services() -> Result<(), RclrsError> { let namespace = "/test_services_graph"; - let mut graph = construct_test_graph(namespace)?; + let graph = construct_test_graph(namespace)?; let check_names_and_types = |names_and_types: TopicNamesAndTypes| { let types = names_and_types .get("/test_services_graph/graph_test_topic_4") @@ -225,7 +225,7 @@ fn test_services() -> Result<(), RclrsError> { #[test] fn test_clients() -> Result<(), RclrsError> { let namespace = "/test_clients_graph"; - let mut graph = construct_test_graph(namespace)?; + let graph = construct_test_graph(namespace)?; let _node_2_empty_client = graph .node2 .create_client::("graph_test_topic_4")?; From 8694d4168c142bf8abbb56922492e3d94d2870f9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Mon, 7 Aug 2023 19:25:01 +0200 Subject: [PATCH 16/32] Refactor spin and spin_once to mimic rclcpp's Executor API (#324) * Added rclcpp/rclpy-like executor * Fix comments --- examples/minimal_pub_sub/Cargo.toml | 4 + .../minimal_pub_sub/src/minimal_two_nodes.rs | 77 ++++++++++++++++++ rclrs/src/executor.rs | 81 +++++++++++++++++++ rclrs/src/lib.rs | 45 ++--------- rclrs/src/node.rs | 9 ++- rclrs/src/wait.rs | 11 ++- 6 files changed, 187 insertions(+), 40 deletions(-) create mode 100644 examples/minimal_pub_sub/src/minimal_two_nodes.rs create mode 100644 rclrs/src/executor.rs diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 12771635b..0990d8555 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -13,6 +13,10 @@ path = "src/minimal_subscriber.rs" name = "minimal_publisher" path = "src/minimal_publisher.rs" +[[bin]] +name = "minimal_two_nodes" +path = "src/minimal_two_nodes.rs" + [[bin]] name = "zero_copy_subscriber" path = "src/zero_copy_subscriber.rs" diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs new file mode 100644 index 000000000..ddf685bd7 --- /dev/null +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -0,0 +1,77 @@ +use std::env; +use std::sync::atomic::{AtomicU32, Ordering}; +use std::sync::{Arc, Mutex}; + +use anyhow::{Error, Result}; + +struct MinimalSubscriber { + num_messages: AtomicU32, + node: Arc, + subscription: Mutex>>>, +} + +impl MinimalSubscriber { + pub fn new(name: &str, topic: &str) -> Result, rclrs::RclrsError> { + let context = rclrs::Context::new(env::args())?; + let node = rclrs::create_node(&context, name)?; + let minimal_subscriber = Arc::new(MinimalSubscriber { + num_messages: 0.into(), + node, + subscription: None.into(), + }); + + let minimal_subscriber_aux = Arc::clone(&minimal_subscriber); + let subscription = minimal_subscriber + .node + .create_subscription::( + topic, + rclrs::QOS_PROFILE_DEFAULT, + move |msg: std_msgs::msg::String| { + minimal_subscriber_aux.callback(msg); + }, + )?; + *minimal_subscriber.subscription.lock().unwrap() = Some(subscription); + Ok(minimal_subscriber) + } + + fn callback(&self, msg: std_msgs::msg::String) { + self.num_messages.fetch_add(1, Ordering::SeqCst); + println!("[{}] I heard: '{}'", self.node.name(), msg.data); + println!( + "[{}] (Got {} messages so far)", + self.node.name(), + self.num_messages.load(Ordering::SeqCst) + ); + } +} + +fn main() -> Result<(), Error> { + let publisher_context = rclrs::Context::new(env::args())?; + let publisher_node = rclrs::create_node(&publisher_context, "minimal_publisher")?; + + let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; + let subscriber_node_two = MinimalSubscriber::new("minimal_subscriber_two", "topic")?; + + let publisher = publisher_node + .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + + std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { + let mut message = std_msgs::msg::String::default(); + let mut publish_count: u32 = 1; + loop { + message.data = format!("Hello, world! {}", publish_count); + println!("Publishing: [{}]", message.data); + publisher.publish(&message)?; + publish_count += 1; + std::thread::sleep(std::time::Duration::from_millis(500)); + } + }); + + let executor = rclrs::SingleThreadedExecutor::new(); + + executor.add_node(&publisher_node)?; + executor.add_node(&subscriber_node_one.node)?; + executor.add_node(&subscriber_node_two.node)?; + + executor.spin().map_err(|err| err.into()) +} diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs new file mode 100644 index 000000000..f96dfc942 --- /dev/null +++ b/rclrs/src/executor.rs @@ -0,0 +1,81 @@ +use crate::rcl_bindings::rcl_context_is_valid; +use crate::{Node, RclReturnCode, RclrsError, WaitSet}; +use std::sync::{Arc, Mutex, Weak}; +use std::time::Duration; + +/// Single-threaded executor implementation. +pub struct SingleThreadedExecutor { + nodes_mtx: Mutex>>, +} + +impl Default for SingleThreadedExecutor { + fn default() -> Self { + Self::new() + } +} + +impl SingleThreadedExecutor { + /// Creates a new executor. + pub fn new() -> Self { + SingleThreadedExecutor { + nodes_mtx: Mutex::new(Vec::new()), + } + } + + /// Add a node to the executor. + pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { + { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); + Ok(()) + } + + /// Remove a node from the executor. + pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { + { self.nodes_mtx.lock().unwrap() } + .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); + Ok(()) + } + + /// Polls the nodes for new messages and executes the corresponding callbacks. + /// + /// This function additionally checks that the context is still valid. + pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { + for node in { self.nodes_mtx.lock().unwrap() } + .iter() + .filter_map(Weak::upgrade) + .filter(|node| unsafe { rcl_context_is_valid(&*node.rcl_context_mtx.lock().unwrap()) }) + { + let wait_set = WaitSet::new_for_node(&node)?; + let ready_entities = wait_set.wait(timeout)?; + + for ready_subscription in ready_entities.subscriptions { + ready_subscription.execute()?; + } + + for ready_client in ready_entities.clients { + ready_client.execute()?; + } + + for ready_service in ready_entities.services { + ready_service.execute()?; + } + } + + Ok(()) + } + + /// Convenience function for calling [`SingleThreadedExecutor::spin_once`] in a loop. + pub fn spin(&self) -> Result<(), RclrsError> { + while !{ self.nodes_mtx.lock().unwrap() }.is_empty() { + match self.spin_once(None) { + Ok(_) + | Err(RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + }) => (), + error => return error, + } + } + + Ok(()) + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index cda4a3823..9afec58bd 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -9,6 +9,7 @@ mod arguments; mod client; mod context; mod error; +mod executor; mod node; mod parameter; mod publisher; @@ -30,11 +31,11 @@ pub use arguments::*; pub use client::*; pub use context::*; pub use error::*; +pub use executor::*; pub use node::*; pub use parameter::*; pub use publisher::*; pub use qos::*; -use rcl_bindings::rcl_context_is_valid; pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; @@ -51,46 +52,16 @@ pub use wait::*; /// /// [1]: crate::RclReturnCode pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { - let wait_set = WaitSet::new_for_node(&node)?; - let ready_entities = wait_set.wait(timeout)?; - - for ready_subscription in ready_entities.subscriptions { - ready_subscription.execute()?; - } - - for ready_client in ready_entities.clients { - ready_client.execute()?; - } - - for ready_service in ready_entities.services { - ready_service.execute()?; - } - - Ok(()) + let executor = SingleThreadedExecutor::new(); + executor.add_node(&node)?; + executor.spin_once(timeout) } /// Convenience function for calling [`spin_once`] in a loop. -/// -/// This function additionally checks that the context is still valid. pub fn spin(node: Arc) -> Result<(), RclrsError> { - // The context_is_valid functions exists only to abstract away ROS distro differences - // SAFETY: No preconditions for this function. - let context_is_valid = { - let node = Arc::clone(&node); - move || unsafe { rcl_context_is_valid(&*node.rcl_context_mtx.lock().unwrap()) } - }; - - while context_is_valid() { - match spin_once(Arc::clone(&node), None) { - Ok(_) - | Err(RclrsError::RclError { - code: RclReturnCode::Timeout, - .. - }) => (), - error => return error, - } - } - Ok(()) + let executor = SingleThreadedExecutor::new(); + executor.add_node(&node)?; + executor.spin() } /// Creates a new node in the empty namespace. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 74eb5ac83..459d7e8ec 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -238,11 +238,16 @@ impl Node { &self, topic: &str, qos: QoSProfile, - ) -> Result, RclrsError> + ) -> Result>, RclrsError> where T: Message, { - Publisher::::new(Arc::clone(&self.rcl_node_mtx), topic, qos) + let publisher = Arc::new(Publisher::::new( + Arc::clone(&self.rcl_node_mtx), + topic, + qos, + )?); + Ok(publisher) } /// Creates a [`Service`][1]. diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 61bde77b2..d8c0e3cb1 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -337,7 +337,16 @@ impl WaitSet { // We cannot currently guarantee that the wait sets may not share content, but it is // mentioned in the doc comment for `add_subscription`. // Also, the rcl_wait_set is obviously valid. - unsafe { rcl_wait(&mut self.rcl_wait_set, timeout_ns) }.ok()?; + match unsafe { rcl_wait(&mut self.rcl_wait_set, timeout_ns) }.ok() { + Ok(_) => (), + Err(error) => match error { + RclrsError::RclError { code, msg } => match code { + RclReturnCode::WaitSetEmpty => (), + _ => return Err(RclrsError::RclError { code, msg }), + }, + _ => return Err(error), + }, + } let mut ready_entities = ReadyEntities { subscriptions: Vec::new(), clients: Vec::new(), From 6c87533b6b9d43d924c12d6348d46f8951a64f15 Mon Sep 17 00:00:00 2001 From: Fawdlstty Date: Tue, 22 Aug 2023 19:12:20 +0800 Subject: [PATCH 17/32] add serde big array support (fixed #327) (#328) * add serde big array support --- rosidl_generator_rs/resource/Cargo.toml.em | 3 ++- rosidl_generator_rs/resource/msg.rs.em | 2 ++ rosidl_generator_rs/resource/msg_idiomatic.rs.em | 2 +- rosidl_generator_rs/resource/msg_rmw.rs.em | 2 +- rosidl_generator_rs/resource/srv.rs.em | 2 ++ rosidl_generator_rs/rosidl_generator_rs/__init__.py | 9 +++++++++ 6 files changed, 17 insertions(+), 3 deletions(-) diff --git a/rosidl_generator_rs/resource/Cargo.toml.em b/rosidl_generator_rs/resource/Cargo.toml.em index 110d2c41d..fcf3461d7 100644 --- a/rosidl_generator_rs/resource/Cargo.toml.em +++ b/rosidl_generator_rs/resource/Cargo.toml.em @@ -6,13 +6,14 @@ edition = "2021" [dependencies] rosidl_runtime_rs = "0.3" serde = { version = "1", optional = true, features = ["derive"] } +serde-big-array = { version = "0.5.1", optional = true } @[for dep in dependency_packages]@ @(dep) = "*" @[end for]@ [features] @{ -serde_features = ["dep:serde", "rosidl_runtime_rs/serde"] +serde_features = ["dep:serde", "dep:serde-big-array", "rosidl_runtime_rs/serde"] for dep in dependency_packages: serde_features.append("{}/serde".format(dep)) }@ diff --git a/rosidl_generator_rs/resource/msg.rs.em b/rosidl_generator_rs/resource/msg.rs.em index 8e9995743..3f7d10e5f 100644 --- a/rosidl_generator_rs/resource/msg.rs.em +++ b/rosidl_generator_rs/resource/msg.rs.em @@ -5,6 +5,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=msg_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ @@ -16,6 +17,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=msg_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ diff --git a/rosidl_generator_rs/resource/msg_idiomatic.rs.em b/rosidl_generator_rs/resource/msg_idiomatic.rs.em index b1974a3d8..e6ec28807 100644 --- a/rosidl_generator_rs/resource/msg_idiomatic.rs.em +++ b/rosidl_generator_rs/resource/msg_idiomatic.rs.em @@ -26,7 +26,7 @@ type_name = msg_spec.structure.namespaced_type.name #[derive(Clone, Debug, PartialEq, PartialOrd)] pub struct @(type_name) { @[for member in msg_spec.structure.members]@ - pub @(get_rs_name(member.name)): @(get_idiomatic_rs_type(member.type)), + @(pre_field_serde(member.type))pub @(get_rs_name(member.name)): @(get_idiomatic_rs_type(member.type)), @[end for]@ } diff --git a/rosidl_generator_rs/resource/msg_rmw.rs.em b/rosidl_generator_rs/resource/msg_rmw.rs.em index b85061815..c4420a685 100644 --- a/rosidl_generator_rs/resource/msg_rmw.rs.em +++ b/rosidl_generator_rs/resource/msg_rmw.rs.em @@ -38,7 +38,7 @@ extern "C" { #[derive(Clone, Debug, PartialEq, PartialOrd)] pub struct @(type_name) { @[for member in msg_spec.structure.members]@ - pub @(get_rs_name(member.name)): @(get_rmw_rs_type(member.type)), + @(pre_field_serde(member.type))pub @(get_rs_name(member.name)): @(get_rmw_rs_type(member.type)), @[end for]@ } diff --git a/rosidl_generator_rs/resource/srv.rs.em b/rosidl_generator_rs/resource/srv.rs.em index 31cabc768..369696ff7 100644 --- a/rosidl_generator_rs/resource/srv.rs.em +++ b/rosidl_generator_rs/resource/srv.rs.em @@ -12,6 +12,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=req_res_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ @@ -49,6 +50,7 @@ TEMPLATE( package_name=package_name, interface_path=interface_path, msg_specs=req_res_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ diff --git a/rosidl_generator_rs/rosidl_generator_rs/__init__.py b/rosidl_generator_rs/rosidl_generator_rs/__init__.py index ae61a3e9f..762fd4884 100644 --- a/rosidl_generator_rs/rosidl_generator_rs/__init__.py +++ b/rosidl_generator_rs/rosidl_generator_rs/__init__.py @@ -101,6 +101,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): 'Services template file %s not found' % template_file data = { + 'pre_field_serde': pre_field_serde, 'get_rmw_rs_type': make_get_rmw_rs_type(args['package_name']), 'get_rs_name': get_rs_name, 'get_idiomatic_rs_type': make_get_idiomatic_rs_type(args['package_name']), @@ -274,6 +275,14 @@ def constant_value_to_rs(type_, value): # - BoundedSequence # - UnboundedSequence + +def pre_field_serde(type_): + if isinstance(type_, Array) and type_.size > 32: + return '#[cfg_attr(feature = "serde", serde(with = "serde_big_array::BigArray"))]\n ' + else: + return '' + + def make_get_idiomatic_rs_type(package_name): get_rmw_rs_type = make_get_rmw_rs_type(package_name) def get_idiomatic_rs_type(type_): From ed103113f5169fd0926a980d38a6251a42c08499 Mon Sep 17 00:00:00 2001 From: Fawdlstty Date: Mon, 28 Aug 2023 20:17:12 +0800 Subject: [PATCH 18/32] Fixed an issue where cpu usage remained at 100% (#330) --- rclrs/src/executor.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index f96dfc942..c9469d7a7 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -71,7 +71,7 @@ impl SingleThreadedExecutor { | Err(RclrsError::RclError { code: RclReturnCode::Timeout, .. - }) => (), + }) => std::thread::yield_now(), error => return error, } } From fbcb17a8e086a3be289a5741ee199415b69c3c82 Mon Sep 17 00:00:00 2001 From: Carter Date: Thu, 26 Oct 2023 03:11:59 -0600 Subject: [PATCH 19/32] Add service_is_ready() (#339) * Add check if service is ready * Update changelog * Fix unneeded return --------- Co-authored-by: carter Co-authored-by: carter --- .../src/minimal_client.rs | 4 +++- .../src/minimal_client_async.rs | 4 +++- rclrs/CHANGELOG.rst | 4 ++++ rclrs/src/client.rs | 18 ++++++++++++++++++ 4 files changed, 28 insertions(+), 2 deletions(-) diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index c91cedc91..915541d54 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -13,7 +13,9 @@ fn main() -> Result<(), Error> { println!("Starting client"); - std::thread::sleep(std::time::Duration::from_millis(500)); + while !client.service_is_ready()? { + std::thread::sleep(std::time::Duration::from_millis(10)); + } client.async_send_request_with_callback( &request, diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 26f41e077..0eeb87f4d 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -12,7 +12,9 @@ async fn main() -> Result<(), Error> { println!("Starting client"); - std::thread::sleep(std::time::Duration::from_millis(500)); + while !client.service_is_ready()? { + std::thread::sleep(std::time::Duration::from_millis(10)); + } let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; diff --git a/rclrs/CHANGELOG.rst b/rclrs/CHANGELOG.rst index 7e592f0a2..486d6ab36 100644 --- a/rclrs/CHANGELOG.rst +++ b/rclrs/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package rclrs ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Unreleased +---------------- +* Service clients now support service_is_ready to check if a service server is present ahead of calling (`#399 `_) + 0.3 (2022-07-22) ---------------- * Loaned messages (zero-copy) (`#212 `_) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index e8cde1e1f..a8387604f 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -238,6 +238,24 @@ where .ok()?; Ok((T::Response::from_rmw_message(response_out), request_id_out)) } + + /// Check if a service server is available. + /// + /// Will return true if there is a service server available, false if unavailable. + /// + pub fn service_is_ready(&self) -> Result { + let mut is_ready = false; + let client = &mut *self.handle.rcl_client_mtx.lock().unwrap(); + let node = &mut *self.handle.rcl_node_mtx.lock().unwrap(); + + unsafe { + // SAFETY both node and client are guaranteed to be valid here + // client is guaranteed to have been generated with node + rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready) + } + .ok()?; + Ok(is_ready) + } } impl ClientBase for Client From 7d11dc7f604eb46d0a010d6785efd1e427bc2284 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 27 Oct 2023 20:50:03 +0800 Subject: [PATCH 20/32] Add preliminary support for parameters (#332) * WIP Add minimal TimeSource implementation Signed-off-by: Luca Della Vedova * Minor cleanup and code reorganization Signed-off-by: Luca Della Vedova * Minor cleanups, add debug code Signed-off-by: Luca Della Vedova * Minor cleanup Signed-off-by: Luca Della Vedova * Change safety note to reflect get_now safety Signed-off-by: Luca Della Vedova * Fix comment spelling Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com> * Cleanup and add some docs / tests Signed-off-by: Luca Della Vedova * Avoid duplicated clocks in TimeSource Signed-off-by: Luca Della Vedova * Change _rcl_clock to just Mutex Signed-off-by: Luca Della Vedova * Remove Mutex from node clock Signed-off-by: Luca Della Vedova * Change Mutex to AtomicBool Signed-off-by: Luca Della Vedova * Avoid cloning message Signed-off-by: Luca Della Vedova * Minor cleanup, add dependency Signed-off-by: Luca Della Vedova * Remove hardcoded use_sim_time Signed-off-by: Luca Della Vedova * Cleanup remaining warnings / clippy Signed-off-by: Luca Della Vedova * Fix tests Signed-off-by: Luca Della Vedova * Refactor API to use ClockSource Signed-off-by: Luca Della Vedova * Fix Drop implementation, finish builder and add comments Signed-off-by: Luca Della Vedova * Minor cleanups Signed-off-by: Luca Della Vedova * Fix graph_tests Signed-off-by: Luca Della Vedova * Remove Arc from time source Signed-off-by: Luca Della Vedova * Remove Sync trait, format Signed-off-by: Luca Della Vedova * Add comparison function for times, use reference to clock Signed-off-by: Luca Della Vedova * Implement Add and Sub for Time Signed-off-by: Luca Della Vedova * Make node pointer Weak to avoid memory leaks Signed-off-by: Luca Della Vedova * Cleanups Signed-off-by: Luca Della Vedova * WIP change clock type when use_sim_time parameter is changed Signed-off-by: Luca Della Vedova * Remove need for mutex in node TimeSource Signed-off-by: Luca Della Vedova * Change get_clock() to return cloned Clock object Signed-off-by: Luca Della Vedova * Minor cleanup Signed-off-by: Luca Della Vedova * Refactor spin and spin_once to mimic rclcpp's Executor API (#324) * Added rclcpp/rclpy-like executor * Fix comments * add serde big array support (fixed #327) (#328) * add serde big array support * Fixed an issue where cpu usage remained at 100% (#330) * Make get_parameter pub(crate) Signed-off-by: Luca Della Vedova * Address review feedback Signed-off-by: Luca Della Vedova * Make time_source pub(crate), remove unused APIs Signed-off-by: Luca Della Vedova * WIP parameter interface implementation Signed-off-by: Luca Della Vedova * Migrate to proposed API Signed-off-by: Luca Della Vedova * Add API for mandatory and optional parameters Signed-off-by: Luca Della Vedova * First round of unit tests for parameter API Signed-off-by: Luca Della Vedova * Make complex parameter types Arc Signed-off-by: Luca Della Vedova * First attempt at undeclared parameter API Signed-off-by: Luca Della Vedova * First attempt at automatic parameter undeclaring Signed-off-by: Luca Della Vedova * Change undeclared_parameter API to be more explicit Signed-off-by: Luca Della Vedova * Minor cleanup, use atomic for undeclared params Signed-off-by: Luca Della Vedova * Add docs Signed-off-by: Luca Della Vedova * Add more tests Signed-off-by: Luca Della Vedova * Update setter for optional parameters Signed-off-by: Luca Della Vedova * Add unset api for optional parameters Signed-off-by: Luca Della Vedova * Cleanup and add helper functions for arrays Signed-off-by: Luca Della Vedova * Add type alias Signed-off-by: Luca Della Vedova * Reduce number of generics, add more tests Signed-off-by: Luca Della Vedova * Refactor to include parameter ranges and read_only Signed-off-by: Luca Della Vedova * Refactor ranges Signed-off-by: Luca Della Vedova * Revert API change for range builder Signed-off-by: Luca Della Vedova * Add ParameterValueError variant Signed-off-by: Luca Della Vedova * First implementation of tentative API Signed-off-by: Luca Della Vedova * Rework API for unified declaration syntax Signed-off-by: Luca Della Vedova * Add missing APIs Signed-off-by: Luca Della Vedova * Add error variant for default out of range Signed-off-by: Luca Della Vedova * Refactor and remove duplicated code Signed-off-by: Luca Della Vedova * Revert change to parameter service file Signed-off-by: Luca Della Vedova * Remove time related changes Signed-off-by: Luca Della Vedova * Clarify doc for unset API Signed-off-by: Luca Della Vedova * Fix comments / TODOs Signed-off-by: Luca Della Vedova * Remove Arc wrapper from undeclared params Signed-off-by: Luca Della Vedova * Add short description of deviations from other client libraries Signed-off-by: Luca Della Vedova * Address minor feedback Signed-off-by: Luca Della Vedova * Remove duplicated range Signed-off-by: Luca Della Vedova * Change maybe_from to try_from Signed-off-by: Luca Della Vedova * WIP converting to default value builder argument Signed-off-by: Luca Della Vedova * Add type for test Signed-off-by: Luca Della Vedova * Infer arrays and allow custom discriminators Signed-off-by: Michael X. Grey * Fix CI Signed-off-by: Luca Della Vedova * Fix some documentation Signed-off-by: Michael X. Grey * Fix doc link Signed-off-by: Luca Della Vedova * Minor cleanups and optimizations Signed-off-by: Luca Della Vedova * Fix outdated docstrings Signed-off-by: Luca Della Vedova * Add links in documentation / address clippy warnings Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova Signed-off-by: Michael X. Grey Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com> Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: Fawdlstty Co-authored-by: Michael X. Grey --- rclrs/src/node.rs | 57 +- rclrs/src/node/builder.rs | 21 +- rclrs/src/parameter.rs | 1385 ++++++++++++++++++++++++++++++++++ rclrs/src/parameter/value.rs | 316 +++++++- 4 files changed, 1740 insertions(+), 39 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 459d7e8ec..bf787c956 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,9 +13,9 @@ pub use self::builder::*; pub use self::graph::*; use crate::rcl_bindings::*; use crate::{ - Client, ClientBase, Context, GuardCondition, ParameterOverrideMap, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - ToResult, + Client, ClientBase, Context, GuardCondition, ParameterBuilder, ParameterInterface, + ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, + Subscription, SubscriptionBase, SubscriptionCallback, ToResult, }; impl Drop for rcl_node_t { @@ -71,7 +71,7 @@ pub struct Node { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, - _parameter_map: ParameterOverrideMap, + _parameter: ParameterInterface, } impl Eq for Node {} @@ -328,7 +328,7 @@ impl Node { } /// Returns the ROS domain ID that the node is using. - /// + /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. /// It can be set through the `ROS_DOMAIN_ID` environment variable. /// @@ -360,6 +360,51 @@ impl Node { domain_id } + /// Creates a [`ParameterBuilder`] that can be used to set parameter declaration options and + /// declare a parameter as [`OptionalParameter`](crate::parameter::OptionalParameter), + /// [`MandatoryParameter`](crate::parameter::MandatoryParameter), or + /// [`ReadOnly`](crate::parameter::ReadOnlyParameter). + /// + /// # Example + /// ``` + /// # use rclrs::{Context, ParameterRange, RclrsError}; + /// let context = Context::new([])?; + /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// // Set it to a range of 0-100, with a step of 2 + /// let range = ParameterRange { + /// lower: Some(0), + /// upper: Some(100), + /// step: Some(2), + /// }; + /// let param = node.declare_parameter("int_param") + /// .default(10) + /// .range(range) + /// .mandatory() + /// .unwrap(); + /// assert_eq!(param.get(), 10); + /// param.set(50).unwrap(); + /// assert_eq!(param.get(), 50); + /// // Out of range, will return an error + /// assert!(param.set(200).is_err()); + /// # Ok::<(), RclrsError>(()) + /// ``` + pub fn declare_parameter<'a, T: ParameterVariant + 'a>( + &'a self, + name: impl Into>, + ) -> ParameterBuilder<'a, T> { + self._parameter.declare(name.into()) + } + + /// Enables usage of undeclared parameters for this node. + /// + /// Returns a [`Parameters`] struct that can be used to get and set all parameters. + pub fn use_undeclared_parameters(&self) -> Parameters { + self._parameter.allow_undeclared(); + Parameters { + interface: &self._parameter, + } + } + /// Creates a [`NodeBuilder`][1] with the given name. /// /// Convenience function equivalent to [`NodeBuilder::new()`][2]. @@ -384,7 +429,7 @@ impl Node { // function, which is why it's not merged into Node::call_string_getter(). // This function is unsafe since it's possible to pass in an rcl_node_t with dangling // pointers etc. -unsafe fn call_string_getter_with_handle( +pub(crate) unsafe fn call_string_getter_with_handle( rcl_node: &rcl_node_t, getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, ) -> String { diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 60e3afe27..b6373b7de 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -2,10 +2,7 @@ use std::ffi::CString; use std::sync::{Arc, Mutex}; use crate::rcl_bindings::*; -use crate::{ - node::call_string_getter_with_handle, resolve_parameter_overrides, Context, Node, RclrsError, - ToResult, -}; +use crate::{Context, Node, ParameterInterface, RclrsError, ToResult}; /// A builder for creating a [`Node`][1]. /// @@ -262,16 +259,12 @@ impl NodeBuilder { .ok()?; }; - let _parameter_map = unsafe { - let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name); - resolve_parameter_overrides( - &fqn, - &rcl_node_options.arguments, - &rcl_context.global_arguments, - )? - }; let rcl_node_mtx = Arc::new(Mutex::new(rcl_node)); - + let _parameter = ParameterInterface::new( + &rcl_node_mtx, + &rcl_node_options.arguments, + &rcl_context.global_arguments, + )?; Ok(Node { rcl_node_mtx, rcl_context_mtx: self.context.clone(), @@ -279,7 +272,7 @@ impl NodeBuilder { guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), - _parameter_map, + _parameter, }) } diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 1818d8d0f..d04e36623 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -3,3 +3,1388 @@ mod value; pub(crate) use override_map::*; pub use value::*; + +use crate::rcl_bindings::*; +use crate::{call_string_getter_with_handle, RclrsError}; +use std::collections::{btree_map::Entry, BTreeMap}; +use std::fmt::Debug; +use std::marker::PhantomData; +use std::sync::{ + atomic::{AtomicBool, Ordering}, + Arc, Mutex, RwLock, Weak, +}; + +// This module implements the core logic of parameters in rclrs. +// The implementation is fairly different from the existing ROS 2 client libraries. A detailed +// explanation of the core differences and why they have been implemented is available at: +// https://github.com/ros2-rust/ros2_rust/pull/332 +// Among the most relevant ones: +// +// * Parameter declaration returns an object which will be the main accessor to the parameter, +// providing getters and, except for read only parameters, setters. Object destruction will +// undeclare the parameter. +// * Declaration uses a builder pattern to specify ranges, description, human readable constraints +// instead of an ParameterDescriptor argument. +// * Parameters properties of read only and dynamic are embedded in their type rather than being a +// boolean parameter. +// * There are no runtime exceptions for common cases such as undeclared parameter, already +// declared, or uninitialized. +// * There is no "parameter not set" type, users can instead decide to have a `Mandatory` parameter +// that must always have a value or `Optional` parameter that can be unset. +// * Explicit API for access to undeclared parameters by having a +// `node.use_undeclared_parameters()` API that allows access to all parameters. + +#[derive(Clone, Debug)] +struct ParameterOptionsStorage { + _description: Arc, + _constraints: Arc, + ranges: ParameterRanges, +} + +impl From> for ParameterOptionsStorage { + fn from(opts: ParameterOptions) -> Self { + Self { + _description: opts.description, + _constraints: opts.constraints, + ranges: opts.ranges.into(), + } + } +} + +/// Options that can be attached to a parameter, such as description, ranges. +/// Some of this data will be used to populate the ParameterDescriptor +#[derive(Clone, Debug)] +pub struct ParameterOptions { + description: Arc, + constraints: Arc, + ranges: T::Range, +} + +impl Default for ParameterOptions { + fn default() -> Self { + Self { + description: Arc::from(""), + constraints: Arc::from(""), + ranges: Default::default(), + } + } +} + +impl From> for ParameterRanges { + fn from(params: ParameterRange) -> Self { + Self { + float: Some(params), + ..Default::default() + } + } +} + +impl From> for ParameterRanges { + fn from(params: ParameterRange) -> Self { + Self { + integer: Some(params), + ..Default::default() + } + } +} + +impl From<()> for ParameterRanges { + fn from(_empty: ()) -> Self { + Self::default() + } +} + +/// Contains all the possible type of ranges that can be applied to a value. +/// Usually only one of these ranges will be applied, but all have to be stored since: +/// +/// * A dynamic parameter can change its type at runtime, in which case a different range could be +/// applied. +/// * Introspection through service calls requires all the ranges to be reported to the user. +#[derive(Clone, Debug, Default)] +pub struct ParameterRanges { + float: Option>, + integer: Option>, +} + +impl ParameterRanges { + fn validate(&self) -> Result<(), DeclarationError> { + if let Some(integer) = &self.integer { + integer.validate()?; + } + if let Some(float) = &self.float { + float.validate()?; + } + Ok(()) + } + + fn in_range(&self, value: &ParameterValue) -> bool { + match value { + ParameterValue::Integer(v) => { + if let Some(range) = &self.integer { + if !range.in_range(*v) { + return false; + } + } + } + ParameterValue::Double(v) => { + if let Some(range) = &self.float { + if !range.in_range(*v) { + return false; + } + } + } + _ => {} + } + true + } +} + +/// Describes the range for paramter type T. +#[derive(Clone, Debug, Default)] +pub struct ParameterRange { + /// Lower limit, if set the parameter must be >= l. + pub lower: Option, + /// Upper limit, if set the parameter must be <= u. + pub upper: Option, + /// Step size, if set and `lower` is set the parameter must be within an integer number of + /// steps of size `step` from `lower`, or equal to the upper limit if set. + /// Example: + /// If lower is `Some(0)`, upper is `Some(10)` and step is `Some(3)`, acceptable values are: + /// `[0, 3, 6, 9, 10]`. + pub step: Option, +} + +impl ParameterRange { + fn inside_boundary(&self, value: &T) -> bool { + if self.lower.as_ref().is_some_and(|l| value < l) { + return false; + } + if self.upper.as_ref().is_some_and(|u| value > u) { + return false; + } + true + } + + fn validate(&self) -> Result<(), DeclarationError> { + if self + .lower + .as_ref() + .zip(self.upper.as_ref()) + .is_some_and(|(l, u)| l > u) + { + return Err(DeclarationError::InvalidRange); + } + if self.step.as_ref().is_some_and(|s| s <= &T::default()) { + return Err(DeclarationError::InvalidRange); + } + Ok(()) + } +} + +impl ParameterRange { + fn in_range(&self, value: i64) -> bool { + if !self.inside_boundary(&value) { + return false; + } + if self.upper.is_some_and(|u| u == value) { + return true; + } + if let (Some(l), Some(s)) = (self.lower, self.step) { + if (value - l) % s != 0 { + return false; + } + } + true + } +} + +impl ParameterRange { + // Same comparison function as rclcpp. + fn are_close(v1: f64, v2: f64) -> bool { + const ULP_TOL: f64 = 100.0; + (v1 - v2).abs() <= (f64::EPSILON * (v1 + v2).abs() * ULP_TOL) + } + + fn in_range(&self, value: f64) -> bool { + if self.upper.is_some_and(|u| Self::are_close(u, value)) + || self.lower.is_some_and(|l| Self::are_close(l, value)) + { + return true; + } + if !self.inside_boundary(&value) { + return false; + } + if let (Some(l), Some(s)) = (self.lower, self.step) { + if !Self::are_close(((value - l) / s).round() * s + l, value) { + return false; + } + } + true + } +} + +#[derive(Clone, Debug)] +enum DeclaredValue { + Mandatory(Arc>), + Optional(Arc>>), + ReadOnly(ParameterValue), +} + +/// Builder used to declare a parameter. Obtain this by calling +/// [`crate::Node::declare_parameter`]. +#[must_use] +pub struct ParameterBuilder<'a, T: ParameterVariant> { + name: Arc, + default_value: Option, + ignore_override: bool, + discard_mismatching_prior_value: bool, + discriminator: DiscriminatorFunction<'a, T>, + options: ParameterOptions, + interface: &'a ParameterInterface, +} + +impl<'a, T: ParameterVariant> ParameterBuilder<'a, T> { + /// Sets the default value for the parameter. The parameter value will be + /// initialized to this if no command line override was given for this + /// parameter and if the parameter also had no value prior to being + /// declared. + /// + /// To customize how the initial value of the parameter is chosen, you can + /// provide a custom function with the method [`Self::discriminate()`]. By + /// default, the initial value will be chosen as + /// `default_value < override_value < prior_value` in order of increasing + /// preference. + pub fn default(mut self, value: T) -> Self { + self.default_value = Some(value); + self + } + + /// Ignore any override that was given for this parameter. + /// + /// If you also use [`Self::discriminate()`], the + /// [`AvailableValues::override_value`] field given to the discriminator + /// will be [`None`] even if the user had provided an override. + pub fn ignore_override(mut self) -> Self { + self.ignore_override = true; + self + } + + /// If the parameter was set to a value before being declared with a type + /// that does not match this declaration, discard the prior value instead + /// of emitting a [`DeclarationError::PriorValueTypeMismatch`]. + /// + /// If the type of the prior value does match the declaration, it will + /// still be provided to the discriminator. + pub fn discard_mismatching_prior_value(mut self) -> Self { + self.discard_mismatching_prior_value = true; + self + } + + /// Decide what the initial value for the parameter will be based on the + /// available `default_value`, `override_value`, or `prior_value`. + /// + /// The default discriminator is [`default_initial_value_discriminator()`]. + pub fn discriminate(mut self, f: F) -> Self + where + F: FnOnce(AvailableValues) -> Option + 'a, + { + self.discriminator = Box::new(f); + self + } + + /// Sets the range for the parameter. + pub fn range(mut self, range: T::Range) -> Self { + self.options.ranges = range; + self + } + + /// Sets the parameter's human readable description. + pub fn description(mut self, description: impl Into>) -> Self { + self.options.description = description.into(); + self + } + + /// Sets the parameter's human readable constraints. + /// These are not enforced by the library but are displayed on parameter description requests + /// and can be used by integrators to understand complex constraints. + pub fn constraints(mut self, constraints: impl Into>) -> Self { + self.options.constraints = constraints.into(); + self + } + + /// Declares the parameter as a Mandatory parameter, that must always have a value. + /// + /// ## See also + /// * [`Self::optional()`] + /// * [`Self::read_only()`] + pub fn mandatory(self) -> Result, DeclarationError> { + self.try_into() + } + + /// Declares the parameter as a ReadOnly parameter, that cannot be edited. + /// + /// # See also + /// * [`Self::optional()`] + /// * [`Self::mandatory()`] + pub fn read_only(self) -> Result, DeclarationError> { + self.try_into() + } + + /// Declares the parameter as an Optional parameter, that can be unset. + /// + /// This will never return the [`DeclarationError::NoValueAvailable`] variant. + /// + /// ## See also + /// * [`Self::mandatory()`] + /// * [`Self::read_only()`] + pub fn optional(self) -> Result, DeclarationError> { + self.try_into() + } +} + +impl<'a, T> ParameterBuilder<'a, Arc<[T]>> +where + Arc<[T]>: ParameterVariant, +{ + /// Sets the default for an array-like parameter from an iterable. + pub fn default_from_iter(mut self, default_value: impl IntoIterator) -> Self { + self.default_value = Some(default_value.into_iter().collect()); + self + } +} + +impl<'a> ParameterBuilder<'a, Arc<[Arc]>> { + /// Sets the default for the parameter from a string-like array. + pub fn default_string_array(mut self, default_value: U) -> Self + where + U: IntoIterator, + U::Item: Into>, + { + self.default_value = Some(default_value.into_iter().map(|v| v.into()).collect()); + self + } +} + +/// This struct is given to the discriminator function of the +/// [`ParameterBuilder`] so it knows what values are available to choose from. +pub struct AvailableValues<'a, T> { + /// The value given to the parameter builder as the default value. + pub default_value: Option, + /// The value given as an override value, usually as a command line argument. + pub override_value: Option, + /// A prior value that the parameter was set to before it was declared. + pub prior_value: Option, + /// The valid ranges for the parameter value. + pub ranges: &'a ParameterRanges, +} + +/// The default discriminator that chooses the initial value for a parameter. +/// The implementation here uses a simple preference of +/// ```notrust +/// default_value < override_value < prior_value +/// ``` +/// in ascending order of preference. +/// +/// The `prior_value` will automatically be discarded if it is outside the +/// designated range. The override value will not be discarded if it is out of +/// range because that is more likely to be an error that needs to be escalated. +/// You can replace all of this with custom behavior by providing your own +/// discriminator function to [`ParameterBuilder::discriminate()`]. +pub fn default_initial_value_discriminator( + available: AvailableValues, +) -> Option { + if let Some(prior) = available.prior_value { + if available.ranges.in_range(&prior.clone().into()) { + return Some(prior); + } + } + if available.override_value.is_some() { + return available.override_value; + } + available.default_value +} + +type DiscriminatorFunction<'a, T> = Box) -> Option + 'a>; + +impl TryFrom> for OptionalParameter { + type Error = DeclarationError; + + fn try_from(builder: ParameterBuilder) -> Result { + let ranges = builder.options.ranges.clone().into(); + let initial_value = builder.interface.get_declaration_initial_value::( + &builder.name, + builder.default_value, + builder.ignore_override, + builder.discard_mismatching_prior_value, + builder.discriminator, + &ranges, + )?; + let value = Arc::new(RwLock::new(initial_value.map(|v| v.into()))); + builder.interface.store_parameter( + builder.name.clone(), + T::kind(), + DeclaredValue::Optional(value.clone()), + builder.options.into(), + ); + Ok(OptionalParameter { + name: builder.name, + value, + ranges, + map: Arc::downgrade(&builder.interface._parameter_map), + _marker: Default::default(), + }) + } +} + +/// A parameter that must have a value +/// This struct has ownership of the declared parameter. Additional parameter declaration will fail +/// while this struct exists and the parameter will be undeclared when it is dropped. +pub struct MandatoryParameter { + name: Arc, + value: Arc>, + ranges: ParameterRanges, + map: Weak>, + _marker: PhantomData, +} + +impl Debug for MandatoryParameter { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("MandatoryParameter") + .field("name", &self.name) + .field("value", &self.get()) + .field("range", &self.ranges) + .finish() + } +} + +impl Drop for MandatoryParameter { + fn drop(&mut self) { + // Clear the entry from the parameter map + if let Some(map) = self.map.upgrade() { + let storage = &mut map.lock().unwrap().storage; + storage.remove(&self.name); + } + } +} + +impl<'a, T: ParameterVariant + 'a> TryFrom> for MandatoryParameter { + type Error = DeclarationError; + + fn try_from(builder: ParameterBuilder) -> Result { + let ranges = builder.options.ranges.clone().into(); + let initial_value = builder.interface.get_declaration_initial_value::( + &builder.name, + builder.default_value, + builder.ignore_override, + builder.discard_mismatching_prior_value, + builder.discriminator, + &ranges, + )?; + let Some(initial_value) = initial_value else { + return Err(DeclarationError::NoValueAvailable); + }; + let value = Arc::new(RwLock::new(initial_value.into())); + builder.interface.store_parameter( + builder.name.clone(), + T::kind(), + DeclaredValue::Mandatory(value.clone()), + builder.options.into(), + ); + Ok(MandatoryParameter { + name: builder.name, + value, + ranges, + map: Arc::downgrade(&builder.interface._parameter_map), + _marker: Default::default(), + }) + } +} + +/// A parameter that might not have a value, represented by `Option`. +/// This struct has ownership of the declared parameter. Additional parameter declaration will fail +/// while this struct exists and the parameter will be undeclared when it is dropped. +pub struct OptionalParameter { + name: Arc, + value: Arc>>, + ranges: ParameterRanges, + map: Weak>, + _marker: PhantomData, +} + +impl Debug for OptionalParameter { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("OptionalParameter") + .field("name", &self.name) + .field("value", &self.get()) + .field("range", &self.ranges) + .finish() + } +} + +impl Drop for OptionalParameter { + fn drop(&mut self) { + // Clear the entry from the parameter map + if let Some(map) = self.map.upgrade() { + let storage = &mut map.lock().unwrap().storage; + storage.remove(&self.name); + } + } +} + +/// A parameter that must have a value and cannot be written to +/// This struct has ownership of the declared parameter. Additional parameter declaration will fail +/// while this struct exists and the parameter will be undeclared when it is dropped. +pub struct ReadOnlyParameter { + name: Arc, + value: ParameterValue, + map: Weak>, + _marker: PhantomData, +} + +impl Debug for ReadOnlyParameter { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("ReadOnlyParameter") + .field("name", &self.name) + .field("value", &self.value) + .finish() + } +} + +impl Drop for ReadOnlyParameter { + fn drop(&mut self) { + // Clear the entry from the parameter map + if let Some(map) = self.map.upgrade() { + let storage = &mut map.lock().unwrap().storage; + storage.remove(&self.name); + } + } +} + +impl<'a, T: ParameterVariant + 'a> TryFrom> for ReadOnlyParameter { + type Error = DeclarationError; + + fn try_from(builder: ParameterBuilder) -> Result { + let ranges = builder.options.ranges.clone().into(); + let initial_value = builder.interface.get_declaration_initial_value::( + &builder.name, + builder.default_value, + builder.ignore_override, + builder.discard_mismatching_prior_value, + builder.discriminator, + &ranges, + )?; + let Some(initial_value) = initial_value else { + return Err(DeclarationError::NoValueAvailable); + }; + let value = initial_value.into(); + builder.interface.store_parameter( + builder.name.clone(), + T::kind(), + DeclaredValue::ReadOnly(value.clone()), + builder.options.into(), + ); + Ok(ReadOnlyParameter { + name: builder.name, + value, + map: Arc::downgrade(&builder.interface._parameter_map), + _marker: Default::default(), + }) + } +} + +#[derive(Clone, Debug)] +struct DeclaredStorage { + value: DeclaredValue, + kind: ParameterKind, + options: ParameterOptionsStorage, +} + +#[derive(Debug)] +enum ParameterStorage { + Declared(DeclaredStorage), + Undeclared(ParameterValue), +} + +#[derive(Debug, Default)] +struct ParameterMap { + storage: BTreeMap, ParameterStorage>, +} + +impl MandatoryParameter { + /// Returns a clone of the most recent value of the parameter. + pub fn get(&self) -> T { + self.value.read().unwrap().clone().try_into().ok().unwrap() + } + + /// Sets the parameter value. + /// Returns [`ParameterValueError::OutOfRange`] if the value is out of the parameter's range. + pub fn set>(&self, value: U) -> Result<(), ParameterValueError> { + let value = value.into().into(); + if !self.ranges.in_range(&value) { + return Err(ParameterValueError::OutOfRange); + } + *self.value.write().unwrap() = value; + Ok(()) + } +} + +impl ReadOnlyParameter { + /// Returns a clone of the most recent value of the parameter. + pub fn get(&self) -> T { + self.value.clone().try_into().ok().unwrap() + } +} + +impl OptionalParameter { + /// Returns a clone of the most recent value of the parameter. + pub fn get(&self) -> Option { + self.value + .read() + .unwrap() + .clone() + .map(|p| p.try_into().ok().unwrap()) + } + + /// Assigns a value to the optional parameter, setting it to `Some(value)`. + /// Returns [`ParameterValueError::OutOfRange`] if the value is out of the parameter's range. + pub fn set>(&self, value: U) -> Result<(), ParameterValueError> { + let value = value.into().into(); + if !self.ranges.in_range(&value) { + return Err(ParameterValueError::OutOfRange); + } + *self.value.write().unwrap() = Some(value); + Ok(()) + } + + /// Unsets the optional parameter value to `None`. + pub fn unset(&self) { + *self.value.write().unwrap() = None; + } +} + +/// Allows access to all parameters via get / set functions, using their name as a key. +pub struct Parameters<'a> { + pub(crate) interface: &'a ParameterInterface, +} + +/// Describes errors that can be generated when trying to set a parameter's value. +#[derive(Debug)] +pub enum ParameterValueError { + /// Parameter value was out of the parameter's range. + OutOfRange, + /// Parameter was stored in a static type and an operation on a different type was attempted. + TypeMismatch, + /// A write on a read-only parameter was attempted. + ReadOnly, +} + +/// Error that can be generated when doing operations on parameters. +#[derive(Debug)] +pub enum DeclarationError { + /// Parameter was already declared and a new declaration was attempted. + AlreadyDeclared, + /// Parameter was declared as non optional but no value was available, either through a user + /// specified default, a command-line override, or a previously set value. + NoValueAvailable, + /// The override value that was provided has the wrong type. This error is bypassed + /// when using [`ParameterBuilder::ignore_override()`]. + OverrideValueTypeMismatch, + /// The value that the parameter was already set to has the wrong type. This error + /// is bypassed when using [`ParameterBuilder::discard_mismatching_prior_value`]. + PriorValueTypeMismatch, + /// The initial value that was selected is out of range. + InitialValueOutOfRange, + /// An invalid range was provided to a parameter declaration (i.e. lower bound > higher bound). + InvalidRange, +} + +impl<'a> Parameters<'a> { + /// Tries to read a parameter of the requested type. + /// + /// Returns `Some(T)` if a parameter of the requested type exists, `None` otherwise. + pub fn get(&self, name: &str) -> Option { + let storage = &self.interface._parameter_map.lock().unwrap().storage; + let storage = storage.get(name)?; + match storage { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(p) => p.read().unwrap().clone().try_into().ok(), + DeclaredValue::Optional(p) => { + p.read().unwrap().clone().and_then(|p| p.try_into().ok()) + } + DeclaredValue::ReadOnly(p) => p.clone().try_into().ok(), + }, + ParameterStorage::Undeclared(value) => value.clone().try_into().ok(), + } + } + + /// Tries to set a parameter with the requested value. + /// + /// Returns: + /// * `Ok(())` if setting was successful. + /// * [`Err(DeclarationError::TypeMismatch)`] if the type of the requested value is different + /// from the parameter's type. + pub fn set( + &self, + name: impl Into>, + value: T, + ) -> Result<(), ParameterValueError> { + let mut map = self.interface._parameter_map.lock().unwrap(); + let name: Arc = name.into(); + match map.storage.entry(name) { + Entry::Occupied(mut entry) => { + // If it's declared, we can only set if it's the same variant. + // Undeclared parameters are dynamic by default + match entry.get_mut() { + ParameterStorage::Declared(param) => { + if T::kind() == param.kind { + let value = value.into(); + if !param.options.ranges.in_range(&value) { + return Err(ParameterValueError::OutOfRange); + } + match ¶m.value { + DeclaredValue::Mandatory(p) => *p.write().unwrap() = value, + DeclaredValue::Optional(p) => *p.write().unwrap() = Some(value), + DeclaredValue::ReadOnly(_) => { + return Err(ParameterValueError::ReadOnly); + } + } + } else { + return Err(ParameterValueError::TypeMismatch); + } + } + ParameterStorage::Undeclared(param) => { + *param = value.into(); + } + } + } + Entry::Vacant(entry) => { + entry.insert(ParameterStorage::Undeclared(value.into())); + } + } + + Ok(()) + } +} + +pub(crate) struct ParameterInterface { + _parameter_map: Arc>, + _override_map: ParameterOverrideMap, + allow_undeclared: AtomicBool, + // NOTE(luca-della-vedova) add a ParameterService field to this struct to add support for + // services. +} + +impl ParameterInterface { + pub(crate) fn new( + rcl_node_mtx: &Arc>, + node_arguments: &rcl_arguments_t, + global_arguments: &rcl_arguments_t, + ) -> Result { + let rcl_node = rcl_node_mtx.lock().unwrap(); + let _override_map = unsafe { + let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name); + resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? + }; + + Ok(ParameterInterface { + _parameter_map: Default::default(), + _override_map, + allow_undeclared: Default::default(), + }) + } + + pub(crate) fn declare<'a, T: ParameterVariant + 'a>( + &'a self, + name: Arc, + ) -> ParameterBuilder<'a, T> { + ParameterBuilder { + name, + default_value: None, + ignore_override: false, + discard_mismatching_prior_value: false, + discriminator: Box::new(default_initial_value_discriminator::), + options: Default::default(), + interface: self, + } + } + + fn get_declaration_initial_value<'a, T: ParameterVariant + 'a>( + &self, + name: &str, + default_value: Option, + ignore_override: bool, + discard_mismatching_prior: bool, + discriminator: DiscriminatorFunction, + ranges: &ParameterRanges, + ) -> Result, DeclarationError> { + ranges.validate()?; + let override_value: Option = if ignore_override { + None + } else if let Some(override_value) = self._override_map.get(name).cloned() { + Some( + override_value + .try_into() + .map_err(|_| DeclarationError::OverrideValueTypeMismatch)?, + ) + } else { + None + }; + + let prior_value = + if let Some(prior_value) = self._parameter_map.lock().unwrap().storage.get(name) { + match prior_value { + ParameterStorage::Declared(_) => return Err(DeclarationError::AlreadyDeclared), + ParameterStorage::Undeclared(param) => match param.clone().try_into() { + Ok(prior) => Some(prior), + Err(_) => { + if !discard_mismatching_prior { + return Err(DeclarationError::PriorValueTypeMismatch); + } + None + } + }, + } + } else { + None + }; + + let selection = discriminator(AvailableValues { + default_value, + override_value, + prior_value, + ranges, + }); + if let Some(initial_value) = &selection { + if !ranges.in_range(&initial_value.clone().into()) { + return Err(DeclarationError::InitialValueOutOfRange); + } + } + Ok(selection) + } + + fn store_parameter( + &self, + name: Arc, + kind: ParameterKind, + value: DeclaredValue, + options: ParameterOptionsStorage, + ) { + self._parameter_map.lock().unwrap().storage.insert( + name, + ParameterStorage::Declared(DeclaredStorage { + options, + value, + kind, + }), + ); + } + + pub(crate) fn allow_undeclared(&self) { + self.allow_undeclared.store(true, Ordering::Relaxed); + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::{create_node, Context}; + + #[test] + fn test_parameter_override_errors() { + // Create a new node with a few parameter overrides + let ctx = Context::new([ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ]) + .unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + + // Declaring a parameter with a different type than what was overridden should return an + // error + assert!(matches!( + node.declare_parameter("declared_int") + .default(1.0) + .mandatory(), + Err(DeclarationError::OverrideValueTypeMismatch) + )); + + // The error should not happen if we ignore overrides + assert!(node + .declare_parameter("declared_int") + .default(1.0) + .ignore_override() + .mandatory() + .is_ok()); + + // If the override does not respect the range, we should return an error + let range = ParameterRange { + upper: Some(5), + ..Default::default() + }; + assert!(matches!( + node.declare_parameter("declared_int") + .default(1) + .range(range.clone()) + .mandatory(), + Err(DeclarationError::InitialValueOutOfRange) + )); + + // The override being out of range should not matter if we use + // ignore_override + assert!(node + .declare_parameter("declared_int") + .default(1) + .range(range) + .ignore_override() + .mandatory() + .is_ok()); + } + + #[test] + fn test_parameter_setting_declaring() { + // Create a new node with a few parameter overrides + let ctx = Context::new([ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + String::from("-p"), + String::from("double_array:=[1.0, 2.0]"), + String::from("-p"), + String::from("optional_bool:=true"), + String::from("-p"), + String::from("non_declared_string:='param'"), + ]) + .unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + + let overridden_int = node + .declare_parameter("declared_int") + .default(123) + .mandatory() + .unwrap(); + assert_eq!(overridden_int.get(), 10); + + let new_param = node + .declare_parameter("new_param") + .default(2.0) + .mandatory() + .unwrap(); + assert_eq!(new_param.get(), 2.0); + + // Getting a parameter that was declared should work + assert_eq!( + node.use_undeclared_parameters().get::("new_param"), + Some(2.0) + ); + + // Getting / Setting a parameter with the wrong type should not work + assert!(node + .use_undeclared_parameters() + .get::("new_param") + .is_none()); + assert!(matches!( + node.use_undeclared_parameters().set("new_param", 42), + Err(ParameterValueError::TypeMismatch) + )); + + // Setting a parameter should update both existing parameter objects and be reflected in + // new node.use_undeclared_parameters().get() calls + assert!(node + .use_undeclared_parameters() + .set("new_param", 10.0) + .is_ok()); + assert_eq!( + node.use_undeclared_parameters().get("new_param"), + Some(10.0) + ); + assert_eq!(new_param.get(), 10.0); + new_param.set(5.0).unwrap(); + assert_eq!(new_param.get(), 5.0); + assert_eq!(node.use_undeclared_parameters().get("new_param"), Some(5.0)); + + // Getting a parameter that was not declared should not work + assert_eq!( + node.use_undeclared_parameters() + .get::("non_existing_param"), + None + ); + + // Getting a parameter that was not declared should not work, even if a value was provided + // as a parameter override + assert_eq!( + node.use_undeclared_parameters() + .get::>("non_declared_string"), + None + ); + + // If a param is set when undeclared, the following declared value should have the + // previously set value. + { + node.use_undeclared_parameters() + .set("new_bool", true) + .unwrap(); + let bool_param = node + .declare_parameter("new_bool") + .default(false) + .mandatory() + .unwrap(); + assert!(bool_param.get()); + } + { + node.use_undeclared_parameters() + .set("new_bool", true) + .unwrap(); + let bool_param = node + .declare_parameter("new_bool") + .default(false) + .optional() + .unwrap(); + assert_eq!(bool_param.get(), Some(true)); + } + + let optional_param = node + .declare_parameter("non_existing_bool") + .optional() + .unwrap(); + assert_eq!(optional_param.get(), None); + optional_param.set(true).unwrap(); + assert_eq!(optional_param.get(), Some(true)); + optional_param.unset(); + assert_eq!(optional_param.get(), None); + + let optional_param2 = node + .declare_parameter("non_existing_bool2") + .default(false) + .optional() + .unwrap(); + assert_eq!(optional_param2.get(), Some(false)); + + // This was provided as a parameter override, hence should be set to true + let optional_param3 = node + .declare_parameter("optional_bool") + .default(false) + .optional() + .unwrap(); + assert_eq!(optional_param3.get(), Some(true)); + + // double_array was overriden to [1.0, 2.0] through command line overrides + let array_param = node + .declare_parameter("double_array") + .default_from_iter(vec![10.0, 20.0]) + .mandatory() + .unwrap(); + assert_eq!(array_param.get()[0], 1.0); + assert_eq!(array_param.get()[1], 2.0); + + let array_param = node + .declare_parameter("string_array") + .default_string_array(vec!["Hello", "World"]) + .mandatory() + .unwrap(); + assert_eq!(array_param.get()[0], "Hello".into()); + assert_eq!(array_param.get()[1], "World".into()); + + // If a value is set when undeclared, the following declare_parameter should have the + // previously set value. + node.use_undeclared_parameters() + .set("undeclared_int", 42) + .unwrap(); + let undeclared_int = node + .declare_parameter("undeclared_int") + .default(10) + .mandatory() + .unwrap(); + assert_eq!(undeclared_int.get(), 42); + } + + #[test] + fn test_override_undeclared_set_priority() { + let ctx = Context::new([ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ]) + .unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + // If a parameter was set as an override and as an undeclared parameter, the undeclared + // value should get priority + node.use_undeclared_parameters() + .set("declared_int", 20) + .unwrap(); + let param = node + .declare_parameter("declared_int") + .default(30) + .mandatory() + .unwrap(); + assert_eq!(param.get(), 20); + } + + #[test] + fn test_parameter_scope_redeclaring() { + let ctx = Context::new([ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ]) + .unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + { + // Setting a parameter with an override + let param = node + .declare_parameter("declared_int") + .default(1) + .mandatory() + .unwrap(); + assert_eq!(param.get(), 10); + param.set(2).unwrap(); + assert_eq!(param.get(), 2); + // Redeclaring should fail + assert!(matches!( + node.declare_parameter("declared_int") + .default(1) + .mandatory(), + Err(DeclarationError::AlreadyDeclared) + )); + } + { + // Parameter went out of scope, redeclaring should be OK and return command line + // override + let param = node + .declare_parameter::("declared_int") + .mandatory() + .unwrap(); + assert_eq!(param.get(), 10); + } + // After a declared parameter went out of scope and was cleared, it should still be + // possible to use it as an undeclared parameter, type can now be changed + assert!(node + .use_undeclared_parameters() + .get::("declared_int") + .is_none()); + node.use_undeclared_parameters() + .set("declared_int", 1.0) + .unwrap(); + assert_eq!( + node.use_undeclared_parameters().get::("declared_int"), + Some(1.0) + ); + } + + #[test] + fn test_parameter_ranges() { + let ctx = Context::new([]).unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + // Setting invalid ranges should fail + let range = ParameterRange { + lower: Some(10), + upper: Some(-10), + step: Some(3), + }; + assert!(matches!( + node.declare_parameter("int_param") + .default(5) + .range(range) + .mandatory(), + Err(DeclarationError::InvalidRange) + )); + let range = ParameterRange { + lower: Some(-10), + upper: Some(10), + step: Some(-1), + }; + assert!(matches!( + node.declare_parameter("int_param") + .default(5) + .range(range) + .mandatory(), + Err(DeclarationError::InvalidRange) + )); + // Setting parameters out of range should fail + let range = ParameterRange { + lower: Some(-10), + upper: Some(10), + step: Some(3), + }; + assert!(matches!( + node.declare_parameter("out_of_range_int") + .default(100) + .range(range.clone()) + .mandatory(), + Err(DeclarationError::InitialValueOutOfRange) + )); + assert!(matches!( + node.declare_parameter("wrong_step_int") + .default(-9) + .range(range.clone()) + .mandatory(), + Err(DeclarationError::InitialValueOutOfRange) + )); + let param = node + .declare_parameter("int_param") + .default(-7) + .range(range) + .mandatory() + .unwrap(); + // Out of step but equal to upper, this is OK + assert!(param.set(10).is_ok()); + // Trying to set it as undeclared should have the same result + assert!(matches!( + node.use_undeclared_parameters().set("int_param", 100), + Err(ParameterValueError::OutOfRange) + )); + assert!(matches!( + node.use_undeclared_parameters().set("int_param", -9), + Err(ParameterValueError::OutOfRange) + )); + assert!(node + .use_undeclared_parameters() + .set("int_param", -4) + .is_ok()); + + // Same for a double parameter + let range = ParameterRange { + lower: Some(-10.0), + upper: Some(10.0), + step: Some(3.0), + }; + assert!(matches!( + node.declare_parameter("out_of_range_double") + .default(100.0) + .range(range.clone()) + .optional(), + Err(DeclarationError::InitialValueOutOfRange) + )); + assert!(matches!( + node.declare_parameter("wrong_step_double") + .default(-9.0) + .range(range.clone()) + .read_only(), + Err(DeclarationError::InitialValueOutOfRange) + )); + let param = node + .declare_parameter("double_param") + .default(-7.0) + .range(range.clone()) + .mandatory() + .unwrap(); + // Out of step but equal to upper, this is OK + assert!(param.set(10.0).is_ok()); + // Quite close but out of tolerance, should fail + assert!(matches!( + param.set(-7.001), + Err(ParameterValueError::OutOfRange) + )); + // Close to step within a few EPSILON, should be OK + assert!(param.set(-7.0 - f64::EPSILON * 10.0).is_ok()); + assert!(param.set(-7.0 + f64::EPSILON * 10.0).is_ok()); + // Close to upper within a few EPSILON, should be OK + assert!(param.set(10.0 - f64::EPSILON * 10.0).is_ok()); + assert!(param.set(10.0 + f64::EPSILON * 10.0).is_ok()); + // Close to lower within a few EPSILON, should be OK + assert!(param.set(-10.0 - f64::EPSILON * 10.0).is_ok()); + assert!(param.set(-10.0 + f64::EPSILON * 10.0).is_ok()); + // Trying to set it as undeclared should have the same result + assert!(matches!( + node.use_undeclared_parameters().set("double_param", 100.0), + Err(ParameterValueError::OutOfRange) + )); + assert!(matches!( + node.use_undeclared_parameters().set("double_param", -9.0), + Err(ParameterValueError::OutOfRange) + )); + assert!(node + .use_undeclared_parameters() + .set("double_param", -4.0) + .is_ok()); + } + + #[test] + fn test_readonly_parameters() { + let ctx = Context::new([]).unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + let param = node + .declare_parameter("int_param") + .default(100) + .read_only() + .unwrap(); + // Multiple copies cannot be declared + assert!(matches!( + node.declare_parameter("int_param").default(100).read_only(), + Err(DeclarationError::AlreadyDeclared) + )); + // A reading should work and return the correct value:w + assert_eq!(param.get(), 100); + assert_eq!( + node.use_undeclared_parameters().get::("int_param"), + Some(100) + ); + // Setting should fail + assert!(matches!( + node.use_undeclared_parameters().set("int_param", 10), + Err(ParameterValueError::ReadOnly) + )); + } + + #[test] + fn test_preexisting_value_error() { + let ctx = Context::new([]).unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + node.use_undeclared_parameters() + .set("int_param", 100) + .unwrap(); + + assert!(matches!( + node.declare_parameter("int_param").default(1.0).mandatory(), + Err(DeclarationError::PriorValueTypeMismatch) + )); + + let range = ParameterRange { + lower: Some(-10), + upper: Some(10), + step: Some(3), + }; + assert!(matches!( + node.declare_parameter("int_param") + .default(-1) + .range(range.clone()) + .discriminate(|available| { available.prior_value }) + .mandatory(), + Err(DeclarationError::InitialValueOutOfRange) + )); + + { + // We now ask to discard the mismatching prior value, so we no + // longer get an error. + let param = node + .declare_parameter("int_param") + .default(1.0) + .discard_mismatching_prior_value() + .mandatory() + .unwrap(); + assert_eq!(param.get(), 1.0); + } + { + // The out of range prior value will be discarded by default. + node.use_undeclared_parameters() + .set("int_param", 100) + .unwrap(); + let param = node + .declare_parameter("int_param") + .default(5) + .range(range) + .mandatory() + .unwrap(); + assert_eq!(param.get(), 5); + } + } + + #[test] + fn test_optional_parameter_apis() { + let ctx = Context::new([]).unwrap(); + let node = create_node(&ctx, "param_test_node").unwrap(); + node.declare_parameter::("int_param") + .optional() + .unwrap(); + } +} diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index b49108a6d..1802145f5 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -1,6 +1,8 @@ use std::ffi::CStr; +use std::sync::Arc; use crate::rcl_bindings::*; +use crate::{ParameterRange, ParameterRanges, ParameterValueError}; /// A parameter value. /// @@ -26,27 +28,295 @@ pub enum ParameterValue { /// /// Unquoted strings are also possible, but not recommended, /// because they may be interpreted as another data type. - String(String), + String(Arc), /// An array of u8. /// /// YAML example: Not possible to specify as YAML. - ByteArray(Vec), + ByteArray(Arc<[u8]>), /// An array of booleans. /// /// YAML example: `[true, false, false]`. - BoolArray(Vec), + BoolArray(Arc<[bool]>), /// An array of i64. /// /// YAML example: `[3, 4]`. - IntegerArray(Vec), + IntegerArray(Arc<[i64]>), /// An array of f64. /// /// YAML example: `[5.0, 6e2]`. - DoubleArray(Vec), + DoubleArray(Arc<[f64]>), /// An array of strings. /// /// YAML example: `["abc", ""]`. - StringArray(Vec), + StringArray(Arc<[Arc]>), +} + +/// Describes the parameter's type. Similar to `ParameterValue` but also includes a `Dynamic` +/// variant for dynamic parameters. +#[derive(Clone, Debug, PartialEq)] +pub enum ParameterKind { + /// A boolean parameter. + Bool, + /// An i64 value. + Integer, + /// An f64 value. + Double, + /// A string. + String, + /// An array of u8. + ByteArray, + /// An array of booleans. + BoolArray, + /// An array of i64. + IntegerArray, + /// An array of f64. + DoubleArray, + /// An array of strings. + StringArray, + /// A dynamic parameter that can change its type at runtime. + Dynamic, +} + +impl From for ParameterValue { + fn from(value: bool) -> ParameterValue { + ParameterValue::Bool(value) + } +} + +impl From for ParameterValue { + fn from(value: i64) -> ParameterValue { + ParameterValue::Integer(value) + } +} + +impl From for ParameterValue { + fn from(value: f64) -> ParameterValue { + ParameterValue::Double(value) + } +} + +impl From> for ParameterValue { + fn from(value: Arc) -> ParameterValue { + ParameterValue::String(value) + } +} + +impl From> for ParameterValue { + fn from(value: Arc<[u8]>) -> ParameterValue { + ParameterValue::ByteArray(value) + } +} + +impl From> for ParameterValue { + fn from(value: Arc<[bool]>) -> ParameterValue { + ParameterValue::BoolArray(value) + } +} + +impl From> for ParameterValue { + fn from(value: Arc<[i64]>) -> ParameterValue { + ParameterValue::IntegerArray(value) + } +} + +impl From> for ParameterValue { + fn from(value: Arc<[f64]>) -> ParameterValue { + ParameterValue::DoubleArray(value) + } +} + +impl From]>> for ParameterValue { + fn from(value: Arc<[Arc]>) -> ParameterValue { + ParameterValue::StringArray(value) + } +} + +/// A trait that describes a value that can be converted into a parameter. +pub trait ParameterVariant: Into + Clone + TryFrom { + /// The type used to describe the range of this parameter. + type Range: Into + Default + Clone; + + /// Returns the `ParameterKind` of the implemented type. + fn kind() -> ParameterKind; +} + +impl TryFrom for bool { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::Bool(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for bool { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::Bool + } +} + +impl TryFrom for i64 { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::Integer(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for i64 { + type Range = ParameterRange; + + fn kind() -> ParameterKind { + ParameterKind::Integer + } +} + +impl TryFrom for f64 { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::Double(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for f64 { + type Range = ParameterRange; + + fn kind() -> ParameterKind { + ParameterKind::Double + } +} + +impl TryFrom for Arc { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::String(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for Arc { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::String + } +} + +impl TryFrom for Arc<[u8]> { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::ByteArray(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for Arc<[u8]> { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::ByteArray + } +} + +impl TryFrom for Arc<[bool]> { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::BoolArray(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for Arc<[bool]> { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::BoolArray + } +} + +impl TryFrom for Arc<[i64]> { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::IntegerArray(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for Arc<[i64]> { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::IntegerArray + } +} + +impl TryFrom for Arc<[f64]> { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::DoubleArray(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for Arc<[f64]> { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::DoubleArray + } +} + +impl TryFrom for Arc<[Arc]> { + type Error = ParameterValueError; + + fn try_from(value: ParameterValue) -> Result { + match value { + ParameterValue::StringArray(v) => Ok(v), + _ => Err(ParameterValueError::TypeMismatch), + } + } +} + +impl ParameterVariant for Arc<[Arc]> { + type Range = (); + + fn kind() -> ParameterKind { + ParameterKind::StringArray + } +} + +impl ParameterVariant for ParameterValue { + type Range = ParameterRanges; + + fn kind() -> ParameterKind { + ParameterKind::Dynamic + } } impl ParameterValue { @@ -87,24 +357,24 @@ impl ParameterValue { } else if !var.string_value.is_null() { let cstr = CStr::from_ptr(var.string_value); let s = cstr.to_string_lossy().into_owned(); - ParameterValue::String(s) + ParameterValue::String(s.into()) } else if !var.byte_array_value.is_null() { let rcl_byte_array = &*var.byte_array_value; let slice = std::slice::from_raw_parts(rcl_byte_array.values, rcl_byte_array.size); - ParameterValue::ByteArray(slice.to_vec()) + ParameterValue::ByteArray(slice.into()) } else if !var.bool_array_value.is_null() { let rcl_bool_array = &*var.bool_array_value; let slice = std::slice::from_raw_parts(rcl_bool_array.values, rcl_bool_array.size); - ParameterValue::BoolArray(slice.to_vec()) + ParameterValue::BoolArray(slice.into()) } else if !var.integer_array_value.is_null() { let rcl_integer_array = &*var.integer_array_value; let slice = std::slice::from_raw_parts(rcl_integer_array.values, rcl_integer_array.size); - ParameterValue::IntegerArray(slice.to_vec()) + ParameterValue::IntegerArray(slice.into()) } else if !var.double_array_value.is_null() { let rcl_double_array = &*var.double_array_value; let slice = std::slice::from_raw_parts(rcl_double_array.values, rcl_double_array.size); - ParameterValue::DoubleArray(slice.to_vec()) + ParameterValue::DoubleArray(slice.into()) } else if !var.string_array_value.is_null() { let rcutils_string_array = &*var.string_array_value; let slice = @@ -114,10 +384,10 @@ impl ParameterValue { .map(|&ptr| { debug_assert!(!ptr.is_null()); let cstr = CStr::from_ptr(ptr); - cstr.to_string_lossy().into_owned() + Arc::from(cstr.to_string_lossy()) }) - .collect(); - ParameterValue::StringArray(strings) + .collect::>(); + ParameterValue::StringArray(strings.into()) } else { unreachable!() } @@ -129,6 +399,8 @@ mod tests { use super::*; use crate::{Context, RclrsError, ToResult}; + // TODO(luca) tests for all from / to ParameterVariant functions + #[test] fn test_parameter_value() -> Result<(), RclrsError> { // This test is not a test of the YAML parser or argument parser, only a test that the @@ -137,13 +409,19 @@ mod tests { ("true", ParameterValue::Bool(true)), ("1", ParameterValue::Integer(1)), ("1.0", ParameterValue::Double(1.0)), - ("'1.0'", ParameterValue::String(String::from("1.0"))), - ("[yes, no]", ParameterValue::BoolArray(vec![true, false])), - ("[-3, 2]", ParameterValue::IntegerArray(vec![-3, 2])), - ("[-3.0, 2.0]", ParameterValue::DoubleArray(vec![-3.0, 2.0])), + ("'1.0'", ParameterValue::String(Arc::from("1.0"))), + ( + "[yes, no]", + ParameterValue::BoolArray(Arc::from([true, false])), + ), + ("[-3, 2]", ParameterValue::IntegerArray(Arc::from([-3, 2]))), + ( + "[-3.0, 2.0]", + ParameterValue::DoubleArray(Arc::from([-3.0, 2.0])), + ), ( "['yes']", - ParameterValue::StringArray(vec![String::from("yes")]), + ParameterValue::StringArray(Arc::from([Arc::from("yes")])), ), ]; for pair in input_output_pairs { From c2d8a29d220271cb0aef8b7872f2fcc2c66f27d4 Mon Sep 17 00:00:00 2001 From: Sam Privett Date: Tue, 31 Oct 2023 04:54:27 -0700 Subject: [PATCH 21/32] Allowed the `improper_ctypes_definitions` lint on our generated rcl bindings. (#341) Co-authored-by: Sam Privett --- rclrs/src/rcl_bindings.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 17496b662..245430269 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -7,6 +7,7 @@ // Caused by https://github.com/ros2/ros2/issues/1374 in iron and newer // See https://github.com/ros2-rust/ros2_rust/issues/320 #![allow(improper_ctypes)] +#![allow(improper_ctypes_definitions)] #![allow(clippy::all)] #![allow(missing_docs)] From 429391bf08f48bc591d450ae0a0d702bdf845de3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Nov 2023 00:34:19 +0800 Subject: [PATCH 22/32] Add time source and clock API to nodes (#325) * WIP Add minimal TimeSource implementation Signed-off-by: Luca Della Vedova * Minor cleanup and code reorganization Signed-off-by: Luca Della Vedova * Minor cleanups, add debug code Signed-off-by: Luca Della Vedova * Minor cleanup Signed-off-by: Luca Della Vedova * Change safety note to reflect get_now safety Signed-off-by: Luca Della Vedova * Fix comment spelling Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com> * Cleanup and add some docs / tests Signed-off-by: Luca Della Vedova * Avoid duplicated clocks in TimeSource Signed-off-by: Luca Della Vedova * Change _rcl_clock to just Mutex Signed-off-by: Luca Della Vedova * Remove Mutex from node clock Signed-off-by: Luca Della Vedova * Change Mutex to AtomicBool Signed-off-by: Luca Della Vedova * Avoid cloning message Signed-off-by: Luca Della Vedova * Minor cleanup, add dependency Signed-off-by: Luca Della Vedova * Remove hardcoded use_sim_time Signed-off-by: Luca Della Vedova * Cleanup remaining warnings / clippy Signed-off-by: Luca Della Vedova * Fix tests Signed-off-by: Luca Della Vedova * Refactor API to use ClockSource Signed-off-by: Luca Della Vedova * Fix Drop implementation, finish builder and add comments Signed-off-by: Luca Della Vedova * Minor cleanups Signed-off-by: Luca Della Vedova * Fix graph_tests Signed-off-by: Luca Della Vedova * Remove Arc from time source Signed-off-by: Luca Della Vedova * Remove Sync trait, format Signed-off-by: Luca Della Vedova * Add comparison function for times, use reference to clock Signed-off-by: Luca Della Vedova * Implement Add and Sub for Time Signed-off-by: Luca Della Vedova * Make node pointer Weak to avoid memory leaks Signed-off-by: Luca Della Vedova * Cleanups Signed-off-by: Luca Della Vedova * WIP change clock type when use_sim_time parameter is changed Signed-off-by: Luca Della Vedova * Remove need for mutex in node TimeSource Signed-off-by: Luca Della Vedova * Change get_clock() to return cloned Clock object Signed-off-by: Luca Della Vedova * Minor cleanup Signed-off-by: Luca Della Vedova * Make get_parameter pub(crate) Signed-off-by: Luca Della Vedova * Address review feedback Signed-off-by: Luca Della Vedova * Make time_source pub(crate), remove unused APIs Signed-off-by: Luca Della Vedova * Use MandatoryParameter for use_sim_time Signed-off-by: Luca Della Vedova * Remove error variant from clock builder Signed-off-by: Luca Della Vedova * Add integration test for use_sim_time = true Signed-off-by: Luca Della Vedova * Minor cleanup for attach_node Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com> --- rclrs/Cargo.toml | 2 + rclrs/package.xml | 1 + rclrs/src/clock.rs | 223 +++++++++++++++++++++++++++++++++ rclrs/src/lib.rs | 8 +- rclrs/src/node.rs | 12 +- rclrs/src/node/builder.rs | 34 ++++- rclrs/src/qos.rs | 15 +++ rclrs/src/time.rs | 87 +++++++++++++ rclrs/src/time_source.rs | 165 ++++++++++++++++++++++++ rclrs_tests/src/graph_tests.rs | 5 +- 10 files changed, 542 insertions(+), 10 deletions(-) create mode 100644 rclrs/src/clock.rs create mode 100644 rclrs/src/time.rs create mode 100644 rclrs/src/time_source.rs diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index c7451db07..a0ba674ae 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -20,6 +20,8 @@ ament_rs = { version = "0.2", optional = true } futures = "0.3" # Needed for dynamic messages libloading = { version = "0.8", optional = true } +# Needed for /clock topic subscription when using simulation time +rosgraph_msgs = "*" # Needed for the Message trait, among others rosidl_runtime_rs = "0.3" diff --git a/rclrs/package.xml b/rclrs/package.xml index 70b7d349d..efe2541c7 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -18,6 +18,7 @@ rcl builtin_interfaces rcl_interfaces + rosgraph_msgs ament_cargo diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs new file mode 100644 index 000000000..c89705d9d --- /dev/null +++ b/rclrs/src/clock.rs @@ -0,0 +1,223 @@ +use crate::rcl_bindings::*; +use crate::{error::ToResult, time::Time, to_rclrs_result}; +use std::sync::{Arc, Mutex}; + +/// Enum to describe clock type. Redefined for readability and to eliminate the uninitialized case +/// from the `rcl_clock_type_t` enum in the binding. +#[derive(Clone, Debug, Copy)] +pub enum ClockType { + /// Time with behavior dependent on the `set_ros_time(bool)` function. If called with `true` + /// it will be driven by a manual value override, otherwise it will be System Time + RosTime = 1, + /// Wall time depending on the current system + SystemTime = 2, + /// Steady time, monotonically increasing but not necessarily equal to wall time. + SteadyTime = 3, +} + +impl From for rcl_clock_type_t { + fn from(clock_type: ClockType) -> Self { + match clock_type { + ClockType::RosTime => rcl_clock_type_t::RCL_ROS_TIME, + ClockType::SystemTime => rcl_clock_type_t::RCL_SYSTEM_TIME, + ClockType::SteadyTime => rcl_clock_type_t::RCL_STEADY_TIME, + } + } +} + +/// Struct that implements a Clock and wraps `rcl_clock_t`. +#[derive(Clone, Debug)] +pub struct Clock { + _type: ClockType, + _rcl_clock: Arc>, + // TODO(luca) Implement jump callbacks +} + +/// A clock source that can be used to drive the contained clock. Created when a clock of type +/// `ClockType::RosTime` is constructed +pub struct ClockSource { + _rcl_clock: Arc>, +} + +impl Clock { + /// Creates a new Clock with `ClockType::SystemTime` + pub fn system() -> Self { + Self::make(ClockType::SystemTime) + } + + /// Creates a new Clock with `ClockType::SteadyTime` + pub fn steady() -> Self { + Self::make(ClockType::SteadyTime) + } + + /// Creates a new Clock with `ClockType::RosTime` and a matching `ClockSource` that can be used + /// to update it + pub fn with_source() -> (Self, ClockSource) { + let clock = Self::make(ClockType::RosTime); + let clock_source = ClockSource::new(clock._rcl_clock.clone()); + (clock, clock_source) + } + + /// Creates a new clock of the given `ClockType`. + pub fn new(type_: ClockType) -> (Self, Option) { + let clock = Self::make(type_); + let clock_source = + matches!(type_, ClockType::RosTime).then(|| ClockSource::new(clock._rcl_clock.clone())); + (clock, clock_source) + } + + fn make(type_: ClockType) -> Self { + let mut rcl_clock; + unsafe { + // SAFETY: Getting a default value is always safe. + rcl_clock = Self::init_generic_clock(); + let mut allocator = rcutils_get_default_allocator(); + // Function will return Err(_) only if there isn't enough memory to allocate a clock + // object. + rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator) + .ok() + .unwrap(); + } + Self { + _type: type_, + _rcl_clock: Arc::new(Mutex::new(rcl_clock)), + } + } + + /// Returns the clock's `ClockType`. + pub fn clock_type(&self) -> ClockType { + self._type + } + + /// Returns the current clock's timestamp. + pub fn now(&self) -> Time { + let mut clock = self._rcl_clock.lock().unwrap(); + let mut time_point: i64 = 0; + unsafe { + // SAFETY: No preconditions for this function + rcl_clock_get_now(&mut *clock, &mut time_point); + } + Time { + nsec: time_point, + clock: Arc::downgrade(&self._rcl_clock), + } + } + + /// Helper function to privately initialize a default clock, with the same behavior as + /// `rcl_init_generic_clock`. By defining a private function instead of implementing + /// `Default`, we avoid exposing a public API to create an invalid clock. + // SAFETY: Getting a default value is always safe. + unsafe fn init_generic_clock() -> rcl_clock_t { + let allocator = rcutils_get_default_allocator(); + rcl_clock_t { + type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED, + jump_callbacks: std::ptr::null_mut::(), + num_jump_callbacks: 0, + get_now: None, + data: std::ptr::null_mut::(), + allocator, + } + } +} + +impl Drop for ClockSource { + fn drop(&mut self) { + self.set_ros_time_enable(false); + } +} + +impl PartialEq for ClockSource { + fn eq(&self, other: &Self) -> bool { + Arc::ptr_eq(&self._rcl_clock, &other._rcl_clock) + } +} + +impl ClockSource { + /// Sets the value of the current ROS time. + pub fn set_ros_time_override(&self, nanoseconds: i64) { + let mut clock = self._rcl_clock.lock().unwrap(); + // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed + // by the mutex + unsafe { + // Function will only fail if timer was uninitialized or not RosTime, which should + // not happen + rcl_set_ros_time_override(&mut *clock, nanoseconds) + .ok() + .unwrap(); + } + } + + fn new(clock: Arc>) -> Self { + let source = Self { _rcl_clock: clock }; + source.set_ros_time_enable(true); + source + } + + /// Sets the clock to use ROS Time, if enabled the clock will report the last value set through + /// `Clock::set_ros_time_override(nanoseconds: i64)`. + fn set_ros_time_enable(&self, enable: bool) { + let mut clock = self._rcl_clock.lock().unwrap(); + if enable { + // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed + // by the mutex + unsafe { + // Function will only fail if timer was uninitialized or not RosTime, which should + // not happen + rcl_enable_ros_time_override(&mut *clock).ok().unwrap(); + } + } else { + // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed + // by the mutex + unsafe { + // Function will only fail if timer was uninitialized or not RosTime, which should + // not happen + rcl_disable_ros_time_override(&mut *clock).ok().unwrap(); + } + } + } +} + +impl Drop for rcl_clock_t { + fn drop(&mut self) { + // SAFETY: No preconditions for this function + let rc = unsafe { rcl_clock_fini(&mut *self) }; + if let Err(e) = to_rclrs_result(rc) { + panic!("Unable to release Clock. {:?}", e) + } + } +} + +// 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_clock_t {} + +#[cfg(test)] +mod tests { + use super::*; + + fn assert_send() {} + fn assert_sync() {} + + #[test] + fn clock_is_send_and_sync() { + assert_send::(); + assert_sync::(); + } + + #[test] + fn clock_system_time_now() { + let clock = Clock::system(); + assert!(clock.now().nsec > 0); + } + + #[test] + fn clock_ros_time_with_override() { + let (clock, source) = Clock::with_source(); + // No manual time set, it should default to 0 + assert!(clock.now().nsec == 0); + let set_time = 1234i64; + source.set_ros_time_override(set_time); + // Ros time is set, should return the value that was set + assert_eq!(clock.now().nsec, set_time); + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 9afec58bd..d3a752892 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -7,6 +7,7 @@ mod arguments; mod client; +mod clock; mod context; mod error; mod executor; @@ -16,6 +17,8 @@ mod publisher; mod qos; mod service; mod subscription; +mod time; +mod time_source; mod vendor; mod wait; @@ -29,6 +32,7 @@ use std::time::Duration; pub use arguments::*; pub use client::*; +pub use clock::*; pub use context::*; pub use error::*; pub use executor::*; @@ -39,6 +43,8 @@ pub use qos::*; pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; +pub use time::*; +use time_source::*; pub use wait::*; /// Polls the node for new messages and executes the corresponding callbacks. @@ -80,7 +86,7 @@ pub fn spin(node: Arc) -> Result<(), RclrsError> { /// # Ok::<(), RclrsError>(()) /// ``` pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { - Ok(Arc::new(Node::builder(context, node_name).build()?)) + Node::new(context, node_name) } /// Creates a [`NodeBuilder`][1]. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index bf787c956..b91e16276 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,9 +13,9 @@ pub use self::builder::*; pub use self::graph::*; use crate::rcl_bindings::*; use crate::{ - Client, ClientBase, Context, GuardCondition, ParameterBuilder, ParameterInterface, + Client, ClientBase, Clock, Context, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, - Subscription, SubscriptionBase, SubscriptionCallback, ToResult, + Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, ToResult, }; impl Drop for rcl_node_t { @@ -71,6 +71,7 @@ pub struct Node { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, + _time_source: TimeSource, _parameter: ParameterInterface, } @@ -95,10 +96,15 @@ impl Node { /// /// See [`NodeBuilder::new()`] for documentation. #[allow(clippy::new_ret_no_self)] - pub fn new(context: &Context, node_name: &str) -> Result { + pub fn new(context: &Context, node_name: &str) -> Result, RclrsError> { Self::builder(context, node_name).build() } + /// Returns the clock associated with this node. + pub fn get_clock(&self) -> Clock { + self._time_source.get_clock() + } + /// Returns the name of the node. /// /// This returns the name after remapping, so it is not necessarily the same as the name that diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index b6373b7de..543bd50d2 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -2,7 +2,10 @@ use std::ffi::CString; use std::sync::{Arc, Mutex}; use crate::rcl_bindings::*; -use crate::{Context, Node, ParameterInterface, RclrsError, ToResult}; +use crate::{ + ClockType, Context, Node, ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, + QOS_PROFILE_CLOCK, +}; /// A builder for creating a [`Node`][1]. /// @@ -14,6 +17,8 @@ use crate::{Context, Node, ParameterInterface, RclrsError, ToResult}; /// - `use_global_arguments: true` /// - `arguments: []` /// - `enable_rosout: true` +/// - `clock_type: ClockType::RosTime` +/// - `clock_qos: QOS_PROFILE_CLOCK` /// /// # Example /// ``` @@ -43,6 +48,8 @@ pub struct NodeBuilder { use_global_arguments: bool, arguments: Vec, enable_rosout: bool, + clock_type: ClockType, + clock_qos: QoSProfile, } impl NodeBuilder { @@ -89,6 +96,8 @@ impl NodeBuilder { use_global_arguments: true, arguments: vec![], enable_rosout: true, + clock_type: ClockType::RosTime, + clock_qos: QOS_PROFILE_CLOCK, } } @@ -221,6 +230,18 @@ impl NodeBuilder { self } + /// Sets the node's clock type. + pub fn clock_type(mut self, clock_type: ClockType) -> Self { + self.clock_type = clock_type; + self + } + + /// Sets the QoSProfile for the clock subscription. + pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self { + self.clock_qos = clock_qos; + self + } + /// Builds the node instance. /// /// Node name and namespace validation is performed in this method. @@ -228,7 +249,7 @@ impl NodeBuilder { /// For example usage, see the [`NodeBuilder`][1] docs. /// /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result { + pub fn build(&self) -> Result, RclrsError> { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, @@ -265,15 +286,20 @@ impl NodeBuilder { &rcl_node_options.arguments, &rcl_context.global_arguments, )?; - Ok(Node { + let node = Arc::new(Node { rcl_node_mtx, rcl_context_mtx: self.context.clone(), clients_mtx: Mutex::new(vec![]), guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), + _time_source: TimeSource::builder(self.clock_type) + .clock_qos(self.clock_qos) + .build(), _parameter, - }) + }); + node._time_source.attach_node(&node); + Ok(node) } /// Creates a rcl_node_options_t struct from this builder. diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index b4904262b..0de35dc16 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -296,6 +296,21 @@ pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile { avoid_ros_namespace_conventions: false, }; +/// Equivalent to `ClockQos` from the [`rclcpp` package][1]. +/// Same as sensor data but with a history depth of 1 +/// +/// [1]: https://github.com/ros2/rclcpp/blob/rolling/rclcpp/include/rclcpp/qos.hpp +pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile { + history: QoSHistoryPolicy::KeepLast { depth: 1 }, + reliability: QoSReliabilityPolicy::BestEffort, + durability: QoSDurabilityPolicy::Volatile, + deadline: QoSDuration::SystemDefault, + lifespan: QoSDuration::SystemDefault, + liveliness: QoSLivelinessPolicy::SystemDefault, + liveliness_lease_duration: QoSDuration::SystemDefault, + avoid_ros_namespace_conventions: false, +}; + /// Equivalent to `rmw_qos_profile_parameters` from the [`rmw` package][1]. /// /// [1]: https://github.com/ros2/rmw/blob/master/rmw/include/rmw/qos_profiles.h diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs new file mode 100644 index 000000000..848981aa8 --- /dev/null +++ b/rclrs/src/time.rs @@ -0,0 +1,87 @@ +use crate::rcl_bindings::*; +use std::ops::{Add, Sub}; +use std::sync::{Mutex, Weak}; +use std::time::Duration; + +/// Struct that represents time. +#[derive(Clone, Debug)] +pub struct Time { + /// Timestamp in nanoseconds. + pub nsec: i64, + /// Weak reference to the clock that generated this time + pub clock: Weak>, +} + +impl Time { + /// Compares self to rhs, if they can be compared (originated from the same clock) calls f with + /// the values of the timestamps. + pub fn compare_with(&self, rhs: &Time, f: F) -> Option + where + F: FnOnce(i64, i64) -> U, + { + self.clock + .ptr_eq(&rhs.clock) + .then(|| f(self.nsec, rhs.nsec)) + } +} + +impl Add for Time { + type Output = Self; + + fn add(self, other: Duration) -> Self { + let dur_ns = i64::try_from(other.as_nanos()).unwrap(); + Time { + nsec: self.nsec.checked_add(dur_ns).unwrap(), + clock: self.clock.clone(), + } + } +} + +impl Sub for Time { + type Output = Self; + + fn sub(self, other: Duration) -> Self { + let dur_ns = i64::try_from(other.as_nanos()).unwrap(); + Time { + nsec: self.nsec.checked_sub(dur_ns).unwrap(), + clock: self.clock.clone(), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::Clock; + + #[test] + fn compare_times_from_same_clock() { + let clock = Clock::system(); + let t1 = clock.now(); + std::thread::sleep(std::time::Duration::from_micros(1)); + let t2 = clock.now(); + assert_eq!(t1.compare_with(&t2, |t1, t2| t1 > t2), Some(false)); + assert_eq!(t1.compare_with(&t2, |t1, t2| t2 > t1), Some(true)); + } + + #[test] + fn compare_times_from_different_clocks() { + // Times from different clocks, even if of the same type, can't be compared + let c1 = Clock::system(); + let c2 = Clock::system(); + let t1 = c1.now(); + let t2 = c2.now(); + assert!(t2.compare_with(&t1, |_, _| ()).is_none()); + assert!(t1.compare_with(&t2, |_, _| ()).is_none()); + } + + #[test] + fn add_duration_to_time() { + let (clock, _) = Clock::with_source(); + let t = clock.now(); + let t2 = t.clone() + Duration::from_secs(1); + assert_eq!(t2.nsec - t.nsec, 1_000_000_000i64); + let t3 = t2 - Duration::from_secs(1); + assert_eq!(t3.nsec, t.nsec); + } +} diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs new file mode 100644 index 000000000..d2aac431f --- /dev/null +++ b/rclrs/src/time_source.rs @@ -0,0 +1,165 @@ +use crate::clock::{Clock, ClockSource, ClockType}; +use crate::{MandatoryParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK}; +use rosgraph_msgs::msg::Clock as ClockMsg; +use std::sync::{Arc, Mutex, RwLock, Weak}; + +/// Time source for a node that drives the attached clock. +/// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe +/// to the `/clock` topic and drive the attached clock +pub(crate) struct TimeSource { + _node: Mutex>, + _clock: RwLock, + _clock_source: Arc>>, + _requested_clock_type: ClockType, + _clock_qos: QoSProfile, + _clock_subscription: Mutex>>>, + _last_received_time: Arc>>, + _use_sim_time: Mutex>>, +} + +/// A builder for creating a [`TimeSource`][1]. +/// +/// The builder pattern allows selectively setting some fields, and leaving all others at their default values. +/// This struct instance can be created via [`TimeSource::builder()`][2]. +/// +/// The default values for optional fields are: +/// - `clock_qos: QOS_PROFILE_CLOCK`[3] +/// +/// +/// [1]: crate::TimeSource +/// [2]: crate::TimeSource::builder +/// [3]: crate::QOS_PROFILE_CLOCK +pub(crate) struct TimeSourceBuilder { + clock_qos: QoSProfile, + clock_type: ClockType, +} + +impl TimeSourceBuilder { + /// Creates a builder for a time source that drives the given clock. + pub(crate) fn new(clock_type: ClockType) -> Self { + Self { + clock_qos: QOS_PROFILE_CLOCK, + clock_type, + } + } + + /// Sets the QoS for the `/clock` topic. + pub(crate) fn clock_qos(mut self, clock_qos: QoSProfile) -> Self { + self.clock_qos = clock_qos; + self + } + + /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`. + pub(crate) fn build(self) -> TimeSource { + let clock = match self.clock_type { + ClockType::RosTime | ClockType::SystemTime => Clock::system(), + ClockType::SteadyTime => Clock::steady(), + }; + TimeSource { + _node: Mutex::new(Weak::new()), + _clock: RwLock::new(clock), + _clock_source: Arc::new(Mutex::new(None)), + _requested_clock_type: self.clock_type, + _clock_qos: self.clock_qos, + _clock_subscription: Mutex::new(None), + _last_received_time: Arc::new(Mutex::new(None)), + _use_sim_time: Mutex::new(None), + } + } +} + +impl TimeSource { + /// Creates a new `TimeSourceBuilder` with default parameters. + pub(crate) fn builder(clock_type: ClockType) -> TimeSourceBuilder { + TimeSourceBuilder::new(clock_type) + } + + /// Returns the clock that this TimeSource is controlling. + pub(crate) fn get_clock(&self) -> Clock { + self._clock.read().unwrap().clone() + } + + /// Attaches the given node to to the `TimeSource`, using its interface to read the + /// `use_sim_time` parameter and create the clock subscription. + pub(crate) fn attach_node(&self, node: &Arc) { + // TODO(luca) register a parameter callback that calls set_ros_time(bool) once parameter + // callbacks are implemented. + let param = node + .declare_parameter("use_sim_time") + .default(false) + .mandatory() + .unwrap(); + *self._node.lock().unwrap() = Arc::downgrade(node); + self.set_ros_time_enable(param.get()); + *self._use_sim_time.lock().unwrap() = Some(param); + } + + fn set_ros_time_enable(&self, enable: bool) { + if matches!(self._requested_clock_type, ClockType::RosTime) { + let mut clock = self._clock.write().unwrap(); + if enable && matches!(clock.clock_type(), ClockType::SystemTime) { + let (new_clock, mut clock_source) = Clock::with_source(); + if let Some(last_received_time) = *self._last_received_time.lock().unwrap() { + Self::update_clock(&mut clock_source, last_received_time); + } + *clock = new_clock; + *self._clock_source.lock().unwrap() = Some(clock_source); + *self._clock_subscription.lock().unwrap() = Some(self.create_clock_sub()); + } + if !enable && matches!(clock.clock_type(), ClockType::RosTime) { + *clock = Clock::system(); + *self._clock_source.lock().unwrap() = None; + *self._clock_subscription.lock().unwrap() = None; + } + } + } + + fn update_clock(clock: &mut ClockSource, nanoseconds: i64) { + clock.set_ros_time_override(nanoseconds); + } + + fn create_clock_sub(&self) -> Arc> { + let clock = self._clock_source.clone(); + let last_received_time = self._last_received_time.clone(); + // Safe to unwrap since the function will only fail if invalid arguments are provided + self._node + .lock() + .unwrap() + .upgrade() + .unwrap() + .create_subscription::("/clock", self._clock_qos, move |msg: ClockMsg| { + let nanoseconds: i64 = + (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; + *last_received_time.lock().unwrap() = Some(nanoseconds); + if let Some(clock) = clock.lock().unwrap().as_mut() { + Self::update_clock(clock, nanoseconds); + } + }) + .unwrap() + } +} + +#[cfg(test)] +mod tests { + use crate::{create_node, Context}; + + #[test] + fn time_source_default_clock() { + let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap(); + // Default clock should be above 0 (use_sim_time is default false) + assert!(node.get_clock().now().nsec > 0); + } + + #[test] + fn time_source_sim_time() { + let ctx = Context::new([ + String::from("--ros-args"), + String::from("-p"), + String::from("use_sim_time:=true"), + ]) + .unwrap(); + let node = create_node(&ctx, "test_node").unwrap(); + // Default sim time value should be 0 (no message received) + assert_eq!(node.get_clock().now().nsec, 0); + } +} diff --git a/rclrs_tests/src/graph_tests.rs b/rclrs_tests/src/graph_tests.rs index b32447ca1..11072e09c 100644 --- a/rclrs_tests/src/graph_tests.rs +++ b/rclrs_tests/src/graph_tests.rs @@ -2,11 +2,12 @@ use rclrs::{ Context, Node, NodeBuilder, RclrsError, TopicEndpointInfo, TopicNamesAndTypes, QOS_PROFILE_SYSTEM_DEFAULT, }; +use std::sync::Arc; use test_msgs::{msg, srv}; struct TestGraph { - node1: Node, - node2: Node, + node1: Arc, + node2: Arc, } fn construct_test_graph(namespace: &str) -> Result { From e7065a7fb19d3d852bc9e978ac43cc4914750ca5 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 7 Nov 2023 16:38:55 +0100 Subject: [PATCH 23/32] Version 0.4.0 (#343) Signed-off-by: Esteve Fernandez --- examples/message_demo/Cargo.toml | 8 ++++---- examples/message_demo/package.xml | 2 +- examples/minimal_client_service/Cargo.toml | 6 +++--- examples/minimal_client_service/package.xml | 2 +- examples/minimal_pub_sub/Cargo.toml | 6 +++--- examples/minimal_pub_sub/package.xml | 2 +- rclrs/Cargo.toml | 4 ++-- rclrs/package.xml | 2 +- rclrs_example_msgs/package.xml | 2 +- rclrs_tests/Cargo.toml | 2 +- rclrs_tests/package.xml | 2 +- rosidl_generator_rs/package.xml | 2 +- rosidl_generator_rs/resource/Cargo.toml.em | 2 +- rosidl_runtime_rs/Cargo.toml | 2 +- rosidl_runtime_rs/package.xml | 2 +- 15 files changed, 23 insertions(+), 23 deletions(-) diff --git a/examples/message_demo/Cargo.toml b/examples/message_demo/Cargo.toml index d3e5e210f..94cd483e6 100644 --- a/examples/message_demo/Cargo.toml +++ b/examples/message_demo/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_message_demo" -version = "0.3.1" +version = "0.4.0" authors = ["Nikolai Morin "] edition = "2021" @@ -12,13 +12,13 @@ path = "src/message_demo.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.rclrs_example_msgs] -version = "0.3" +version = "0.4" features = ["serde"] [dependencies.serde_json] diff --git a/examples/message_demo/package.xml b/examples/message_demo/package.xml index 48cd14e4f..96eadd2fe 100644 --- a/examples/message_demo/package.xml +++ b/examples/message_demo/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_message_demo - 0.3.1 + 0.4.0 Package containing an example of message-related functionality in rclrs. Nikolai Morin Apache License 2.0 diff --git a/examples/minimal_client_service/Cargo.toml b/examples/minimal_client_service/Cargo.toml index bd8b43144..e520c0d81 100644 --- a/examples/minimal_client_service/Cargo.toml +++ b/examples/minimal_client_service/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_client_service" -version = "0.3.1" +version = "0.4.0" authors = ["Esteve Fernandez "] edition = "2021" @@ -21,10 +21,10 @@ anyhow = {version = "1", features = ["backtrace"]} tokio = { version = "1", features = ["macros", "rt", "rt-multi-thread", "time"] } [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.example_interfaces] version = "*" diff --git a/examples/minimal_client_service/package.xml b/examples/minimal_client_service/package.xml index 2927b360e..6ceab386c 100644 --- a/examples/minimal_client_service/package.xml +++ b/examples/minimal_client_service/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_client_service - 0.3.1 + 0.4.0 Package containing an example of the client-service mechanism in rclrs. Esteve Fernandez Apache License 2.0 diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 0990d8555..43947fde3 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_pub_sub" -version = "0.3.1" +version = "0.4.0" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" @@ -29,10 +29,10 @@ path = "src/zero_copy_publisher.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.std_msgs] version = "*" diff --git a/examples/minimal_pub_sub/package.xml b/examples/minimal_pub_sub/package.xml index 7ed97620f..2b84438ab 100644 --- a/examples/minimal_pub_sub/package.xml +++ b/examples/minimal_pub_sub/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_pub_sub - 0.3.1 + 0.4.0 Package containing an example of the publish-subscribe mechanism in rclrs. Esteve Fernandez Nikolai Morin diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index a0ba674ae..d57a26cd5 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs" -version = "0.3.1" +version = "0.4.0" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" @@ -23,7 +23,7 @@ libloading = { version = "0.8", optional = true } # Needed for /clock topic subscription when using simulation time rosgraph_msgs = "*" # Needed for the Message trait, among others -rosidl_runtime_rs = "0.3" +rosidl_runtime_rs = "0.4" [dev-dependencies] # Needed for e.g. writing yaml files in tests diff --git a/rclrs/package.xml b/rclrs/package.xml index efe2541c7..86ff4b571 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs - 0.3.1 + 0.4.0 Package containing the Rust client library. Esteve Fernandez Nikolai Morin diff --git a/rclrs_example_msgs/package.xml b/rclrs_example_msgs/package.xml index 52ccbf6ef..5425a9c04 100644 --- a/rclrs_example_msgs/package.xml +++ b/rclrs_example_msgs/package.xml @@ -2,7 +2,7 @@ rclrs_example_msgs - 0.3.1 + 0.4.0 A package containing some example message definitions. Nikolai Morin Apache License 2.0 diff --git a/rclrs_tests/Cargo.toml b/rclrs_tests/Cargo.toml index 51708f4e7..3ba64af1e 100644 --- a/rclrs_tests/Cargo.toml +++ b/rclrs_tests/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs_tests" -version = "0.3.1" +version = "0.4.0" authors = ["Chris Reid "] edition = "2021" diff --git a/rclrs_tests/package.xml b/rclrs_tests/package.xml index 82520d5d1..d616c5dc7 100644 --- a/rclrs_tests/package.xml +++ b/rclrs_tests/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs_tests - 0.3.1 + 0.4.0 Package containing tests for rclrs that make use of msgs and other dependencies Chris Reid Apache License 2.0 diff --git a/rosidl_generator_rs/package.xml b/rosidl_generator_rs/package.xml index 18ff07f09..0cad2299a 100644 --- a/rosidl_generator_rs/package.xml +++ b/rosidl_generator_rs/package.xml @@ -2,7 +2,7 @@ rosidl_generator_rs - 0.3.1 + 0.4.0 Generate the ROS interfaces in Rust. Esteve Fernandez Apache License 2.0 diff --git a/rosidl_generator_rs/resource/Cargo.toml.em b/rosidl_generator_rs/resource/Cargo.toml.em index fcf3461d7..7c8f7fe30 100644 --- a/rosidl_generator_rs/resource/Cargo.toml.em +++ b/rosidl_generator_rs/resource/Cargo.toml.em @@ -4,7 +4,7 @@ version = "@(package_version)" edition = "2021" [dependencies] -rosidl_runtime_rs = "0.3" +rosidl_runtime_rs = "0.4" serde = { version = "1", optional = true, features = ["derive"] } serde-big-array = { version = "0.5.1", optional = true } @[for dep in dependency_packages]@ diff --git a/rosidl_runtime_rs/Cargo.toml b/rosidl_runtime_rs/Cargo.toml index e15ef0ff3..2a363be74 100644 --- a/rosidl_runtime_rs/Cargo.toml +++ b/rosidl_runtime_rs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rosidl_runtime_rs" -version = "0.3.1" +version = "0.4.0" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" diff --git a/rosidl_runtime_rs/package.xml b/rosidl_runtime_rs/package.xml index 71bb5dbad..5e97ba2c4 100644 --- a/rosidl_runtime_rs/package.xml +++ b/rosidl_runtime_rs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rosidl_runtime_rs - 0.3.1 + 0.4.0 Message generation code shared by Rust projects in ROS 2 Jacob Hassold From 5014fcd387a7855495997e8aa51008e0e4aa077c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 7 Nov 2023 17:11:56 +0100 Subject: [PATCH 24/32] Revert "Version 0.4.0 (#343)" (#344) This reverts commit e7065a7fb19d3d852bc9e978ac43cc4914750ca5. --- examples/message_demo/Cargo.toml | 8 ++++---- examples/message_demo/package.xml | 2 +- examples/minimal_client_service/Cargo.toml | 6 +++--- examples/minimal_client_service/package.xml | 2 +- examples/minimal_pub_sub/Cargo.toml | 6 +++--- examples/minimal_pub_sub/package.xml | 2 +- rclrs/Cargo.toml | 4 ++-- rclrs/package.xml | 2 +- rclrs_example_msgs/package.xml | 2 +- rclrs_tests/Cargo.toml | 2 +- rclrs_tests/package.xml | 2 +- rosidl_generator_rs/package.xml | 2 +- rosidl_generator_rs/resource/Cargo.toml.em | 2 +- rosidl_runtime_rs/Cargo.toml | 2 +- rosidl_runtime_rs/package.xml | 2 +- 15 files changed, 23 insertions(+), 23 deletions(-) diff --git a/examples/message_demo/Cargo.toml b/examples/message_demo/Cargo.toml index 94cd483e6..d3e5e210f 100644 --- a/examples/message_demo/Cargo.toml +++ b/examples/message_demo/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_message_demo" -version = "0.4.0" +version = "0.3.1" authors = ["Nikolai Morin "] edition = "2021" @@ -12,13 +12,13 @@ path = "src/message_demo.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.4" +version = "0.3" [dependencies.rosidl_runtime_rs] -version = "0.4" +version = "0.3" [dependencies.rclrs_example_msgs] -version = "0.4" +version = "0.3" features = ["serde"] [dependencies.serde_json] diff --git a/examples/message_demo/package.xml b/examples/message_demo/package.xml index 96eadd2fe..48cd14e4f 100644 --- a/examples/message_demo/package.xml +++ b/examples/message_demo/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_message_demo - 0.4.0 + 0.3.1 Package containing an example of message-related functionality in rclrs. Nikolai Morin Apache License 2.0 diff --git a/examples/minimal_client_service/Cargo.toml b/examples/minimal_client_service/Cargo.toml index e520c0d81..bd8b43144 100644 --- a/examples/minimal_client_service/Cargo.toml +++ b/examples/minimal_client_service/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_client_service" -version = "0.4.0" +version = "0.3.1" authors = ["Esteve Fernandez "] edition = "2021" @@ -21,10 +21,10 @@ anyhow = {version = "1", features = ["backtrace"]} tokio = { version = "1", features = ["macros", "rt", "rt-multi-thread", "time"] } [dependencies.rclrs] -version = "0.4" +version = "0.3" [dependencies.rosidl_runtime_rs] -version = "0.4" +version = "0.3" [dependencies.example_interfaces] version = "*" diff --git a/examples/minimal_client_service/package.xml b/examples/minimal_client_service/package.xml index 6ceab386c..2927b360e 100644 --- a/examples/minimal_client_service/package.xml +++ b/examples/minimal_client_service/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_client_service - 0.4.0 + 0.3.1 Package containing an example of the client-service mechanism in rclrs. Esteve Fernandez Apache License 2.0 diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 43947fde3..0990d8555 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_pub_sub" -version = "0.4.0" +version = "0.3.1" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" @@ -29,10 +29,10 @@ path = "src/zero_copy_publisher.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.4" +version = "0.3" [dependencies.rosidl_runtime_rs] -version = "0.4" +version = "0.3" [dependencies.std_msgs] version = "*" diff --git a/examples/minimal_pub_sub/package.xml b/examples/minimal_pub_sub/package.xml index 2b84438ab..7ed97620f 100644 --- a/examples/minimal_pub_sub/package.xml +++ b/examples/minimal_pub_sub/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_pub_sub - 0.4.0 + 0.3.1 Package containing an example of the publish-subscribe mechanism in rclrs. Esteve Fernandez Nikolai Morin diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index d57a26cd5..a0ba674ae 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs" -version = "0.4.0" +version = "0.3.1" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" @@ -23,7 +23,7 @@ libloading = { version = "0.8", optional = true } # Needed for /clock topic subscription when using simulation time rosgraph_msgs = "*" # Needed for the Message trait, among others -rosidl_runtime_rs = "0.4" +rosidl_runtime_rs = "0.3" [dev-dependencies] # Needed for e.g. writing yaml files in tests diff --git a/rclrs/package.xml b/rclrs/package.xml index 86ff4b571..efe2541c7 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs - 0.4.0 + 0.3.1 Package containing the Rust client library. Esteve Fernandez Nikolai Morin diff --git a/rclrs_example_msgs/package.xml b/rclrs_example_msgs/package.xml index 5425a9c04..52ccbf6ef 100644 --- a/rclrs_example_msgs/package.xml +++ b/rclrs_example_msgs/package.xml @@ -2,7 +2,7 @@ rclrs_example_msgs - 0.4.0 + 0.3.1 A package containing some example message definitions. Nikolai Morin Apache License 2.0 diff --git a/rclrs_tests/Cargo.toml b/rclrs_tests/Cargo.toml index 3ba64af1e..51708f4e7 100644 --- a/rclrs_tests/Cargo.toml +++ b/rclrs_tests/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs_tests" -version = "0.4.0" +version = "0.3.1" authors = ["Chris Reid "] edition = "2021" diff --git a/rclrs_tests/package.xml b/rclrs_tests/package.xml index d616c5dc7..82520d5d1 100644 --- a/rclrs_tests/package.xml +++ b/rclrs_tests/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs_tests - 0.4.0 + 0.3.1 Package containing tests for rclrs that make use of msgs and other dependencies Chris Reid Apache License 2.0 diff --git a/rosidl_generator_rs/package.xml b/rosidl_generator_rs/package.xml index 0cad2299a..18ff07f09 100644 --- a/rosidl_generator_rs/package.xml +++ b/rosidl_generator_rs/package.xml @@ -2,7 +2,7 @@ rosidl_generator_rs - 0.4.0 + 0.3.1 Generate the ROS interfaces in Rust. Esteve Fernandez Apache License 2.0 diff --git a/rosidl_generator_rs/resource/Cargo.toml.em b/rosidl_generator_rs/resource/Cargo.toml.em index 7c8f7fe30..fcf3461d7 100644 --- a/rosidl_generator_rs/resource/Cargo.toml.em +++ b/rosidl_generator_rs/resource/Cargo.toml.em @@ -4,7 +4,7 @@ version = "@(package_version)" edition = "2021" [dependencies] -rosidl_runtime_rs = "0.4" +rosidl_runtime_rs = "0.3" serde = { version = "1", optional = true, features = ["derive"] } serde-big-array = { version = "0.5.1", optional = true } @[for dep in dependency_packages]@ diff --git a/rosidl_runtime_rs/Cargo.toml b/rosidl_runtime_rs/Cargo.toml index 2a363be74..e15ef0ff3 100644 --- a/rosidl_runtime_rs/Cargo.toml +++ b/rosidl_runtime_rs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rosidl_runtime_rs" -version = "0.4.0" +version = "0.3.1" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" diff --git a/rosidl_runtime_rs/package.xml b/rosidl_runtime_rs/package.xml index 5e97ba2c4..71bb5dbad 100644 --- a/rosidl_runtime_rs/package.xml +++ b/rosidl_runtime_rs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rosidl_runtime_rs - 0.4.0 + 0.3.1 Message generation code shared by Rust projects in ROS 2 Jacob Hassold From 2afa06257b2b1f7dfe7225ee01b334c54b6671bc Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 7 Nov 2023 17:41:15 +0100 Subject: [PATCH 25/32] Vendorize rosgraph msgs (#345) * Revert "Version 0.4.0 (#343)" This reverts commit e7065a7fb19d3d852bc9e978ac43cc4914750ca5. * Vendorize rosgraph_msgs Signed-off-by: Esteve Fernandez * Fix module path * Fix format --------- Signed-off-by: Esteve Fernandez --- rclrs/Cargo.toml | 2 - rclrs/src/time_source.rs | 2 +- rclrs/src/vendor/mod.rs | 1 + rclrs/src/vendor/rosgraph_msgs/mod.rs | 3 + rclrs/src/vendor/rosgraph_msgs/msg.rs | 131 ++++++++++++++++++++++++++ rclrs/vendor_interfaces.py | 12 ++- 6 files changed, 144 insertions(+), 7 deletions(-) create mode 100644 rclrs/src/vendor/rosgraph_msgs/mod.rs create mode 100644 rclrs/src/vendor/rosgraph_msgs/msg.rs diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index a0ba674ae..c7451db07 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -20,8 +20,6 @@ ament_rs = { version = "0.2", optional = true } futures = "0.3" # Needed for dynamic messages libloading = { version = "0.8", optional = true } -# Needed for /clock topic subscription when using simulation time -rosgraph_msgs = "*" # Needed for the Message trait, among others rosidl_runtime_rs = "0.3" diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index d2aac431f..102cdbd08 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,6 +1,6 @@ use crate::clock::{Clock, ClockSource, ClockType}; +use crate::vendor::rosgraph_msgs::msg::Clock as ClockMsg; use crate::{MandatoryParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK}; -use rosgraph_msgs::msg::Clock as ClockMsg; use std::sync::{Arc, Mutex, RwLock, Weak}; /// Time source for a node that drives the attached clock. diff --git a/rclrs/src/vendor/mod.rs b/rclrs/src/vendor/mod.rs index f1e33f75a..fe87087b1 100644 --- a/rclrs/src/vendor/mod.rs +++ b/rclrs/src/vendor/mod.rs @@ -4,3 +4,4 @@ pub mod builtin_interfaces; pub mod rcl_interfaces; +pub mod rosgraph_msgs; diff --git a/rclrs/src/vendor/rosgraph_msgs/mod.rs b/rclrs/src/vendor/rosgraph_msgs/mod.rs new file mode 100644 index 000000000..a6365b3b8 --- /dev/null +++ b/rclrs/src/vendor/rosgraph_msgs/mod.rs @@ -0,0 +1,3 @@ +#![allow(non_camel_case_types)] + +pub mod msg; diff --git a/rclrs/src/vendor/rosgraph_msgs/msg.rs b/rclrs/src/vendor/rosgraph_msgs/msg.rs new file mode 100644 index 000000000..1f4af8b70 --- /dev/null +++ b/rclrs/src/vendor/rosgraph_msgs/msg.rs @@ -0,0 +1,131 @@ +pub mod rmw { + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "rosgraph_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__rosgraph_msgs__msg__Clock( + ) -> *const std::os::raw::c_void; + } + + #[link(name = "rosgraph_msgs__rosidl_generator_c")] + extern "C" { + fn rosgraph_msgs__msg__Clock__init(msg: *mut Clock) -> bool; + fn rosgraph_msgs__msg__Clock__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn rosgraph_msgs__msg__Clock__Sequence__fini(seq: *mut rosidl_runtime_rs::Sequence); + fn rosgraph_msgs__msg__Clock__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to rosgraph_msgs__msg__Clock + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct Clock { + pub clock: crate::vendor::builtin_interfaces::msg::rmw::Time, + } + + impl Default for Clock { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !rosgraph_msgs__msg__Clock__init(&mut msg as *mut _) { + panic!("Call to rosgraph_msgs__msg__Clock__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for Clock { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { rosgraph_msgs__msg__Clock__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { rosgraph_msgs__msg__Clock__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { rosgraph_msgs__msg__Clock__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for Clock { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for Clock + where + Self: Sized, + { + const TYPE_NAME: &'static str = "rosgraph_msgs/msg/Clock"; + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__rosgraph_msgs__msg__Clock() + } + } + } +} // mod rmw + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct Clock { + pub clock: crate::vendor::builtin_interfaces::msg::Time, +} + +impl Default for Clock { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::rosgraph_msgs::msg::rmw::Clock::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for Clock { + type RmwMsg = crate::vendor::rosgraph_msgs::msg::rmw::Clock; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + clock: crate::vendor::builtin_interfaces::msg::Time::into_rmw_message( + std::borrow::Cow::Owned(msg.clock), + ) + .into_owned(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + clock: crate::vendor::builtin_interfaces::msg::Time::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.clock), + ) + .into_owned(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + clock: crate::vendor::builtin_interfaces::msg::Time::from_rmw_message(msg.clock), + } + } +} diff --git a/rclrs/vendor_interfaces.py b/rclrs/vendor_interfaces.py index 37d01e2f5..6a0066869 100644 --- a/rclrs/vendor_interfaces.py +++ b/rclrs/vendor_interfaces.py @@ -1,6 +1,7 @@ # This script produces the `vendor` module inside `rclrs` by copying -# the generated code for the `rcl_interfaces` package and its dependency -# `builtin_interfaces` and adjusting the submodule paths in the code. +# the generated code for the `rosgraph_msgs` and `rcl_interfaces` packages and +# its dependency `builtin_interfaces` and adjusting the submodule paths in the +# code. # If these packages, or the `rosidl_generator_rs`, get changed, you can # update the `vendor` module by running this script. # The purpose is to avoid an external dependency on `rcl_interfaces`, which @@ -12,7 +13,7 @@ import subprocess def get_args(): - parser = argparse.ArgumentParser(description='Vendor the rcl_interfaces and builtin_interfaces packages into rclrs') + parser = argparse.ArgumentParser(description='Vendor the rcl_interfaces, builtin_interfaces and rosgraph_msgs packages into rclrs') parser.add_argument('install_base', metavar='install_base', type=Path, help='the install base (must have non-merged layout)') return parser.parse_args() @@ -20,6 +21,7 @@ def get_args(): def adjust(pkg, text): text = text.replace('builtin_interfaces::', 'crate::vendor::builtin_interfaces::') text = text.replace('rcl_interfaces::', 'crate::vendor::rcl_interfaces::') + text = text.replace('rosgraph_msgs::', 'crate::vendor::rosgraph_msgs::') text = text.replace('crate::msg', f'crate::vendor::{pkg}::msg') text = text.replace('crate::srv', f'crate::vendor::{pkg}::srv') return text @@ -34,6 +36,7 @@ def copy_adjusted(pkg, src, dst): pub mod builtin_interfaces; pub mod rcl_interfaces; +pub mod rosgraph_msgs; """.format(Path(__file__).name) def main(): @@ -41,11 +44,12 @@ def main(): assert args.install_base.is_dir(), "Install base does not exist" assert (args.install_base / 'builtin_interfaces').is_dir(), "Install base does not contain builtin_interfaces" assert (args.install_base / 'rcl_interfaces').is_dir(), "Install base does not contain rcl_interfaces" + assert (args.install_base / 'rosgraph_msgs').is_dir(), "Install base does not contain rosgraph_msgs" rclrs_root = Path(__file__).parent vendor_dir = rclrs_root / 'src' / 'vendor' if vendor_dir.exists(): shutil.rmtree(vendor_dir) - for pkg in ['builtin_interfaces', 'rcl_interfaces']: + for pkg in ['builtin_interfaces', 'rcl_interfaces', 'rosgraph_msgs']: src = args.install_base / pkg / 'share' / pkg / 'rust' / 'src' dst = vendor_dir / pkg dst.mkdir(parents=True) From 9fb29f280892afabd7bcc82ceb9eb161f43fcb68 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 7 Nov 2023 18:19:40 +0100 Subject: [PATCH 26/32] Version 0.4.0 (#346) --- examples/message_demo/Cargo.toml | 8 ++++---- examples/message_demo/package.xml | 2 +- examples/minimal_client_service/Cargo.toml | 6 +++--- examples/minimal_client_service/package.xml | 2 +- examples/minimal_pub_sub/Cargo.toml | 6 +++--- examples/minimal_pub_sub/package.xml | 2 +- rclrs/Cargo.toml | 4 ++-- rclrs/package.xml | 2 +- rclrs_example_msgs/package.xml | 2 +- rclrs_tests/Cargo.toml | 2 +- rclrs_tests/package.xml | 2 +- rosidl_generator_rs/package.xml | 2 +- rosidl_generator_rs/resource/Cargo.toml.em | 2 +- rosidl_runtime_rs/Cargo.toml | 2 +- rosidl_runtime_rs/package.xml | 2 +- 15 files changed, 23 insertions(+), 23 deletions(-) diff --git a/examples/message_demo/Cargo.toml b/examples/message_demo/Cargo.toml index d3e5e210f..94cd483e6 100644 --- a/examples/message_demo/Cargo.toml +++ b/examples/message_demo/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_message_demo" -version = "0.3.1" +version = "0.4.0" authors = ["Nikolai Morin "] edition = "2021" @@ -12,13 +12,13 @@ path = "src/message_demo.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.rclrs_example_msgs] -version = "0.3" +version = "0.4" features = ["serde"] [dependencies.serde_json] diff --git a/examples/message_demo/package.xml b/examples/message_demo/package.xml index 48cd14e4f..96eadd2fe 100644 --- a/examples/message_demo/package.xml +++ b/examples/message_demo/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_message_demo - 0.3.1 + 0.4.0 Package containing an example of message-related functionality in rclrs. Nikolai Morin Apache License 2.0 diff --git a/examples/minimal_client_service/Cargo.toml b/examples/minimal_client_service/Cargo.toml index bd8b43144..e520c0d81 100644 --- a/examples/minimal_client_service/Cargo.toml +++ b/examples/minimal_client_service/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_client_service" -version = "0.3.1" +version = "0.4.0" authors = ["Esteve Fernandez "] edition = "2021" @@ -21,10 +21,10 @@ anyhow = {version = "1", features = ["backtrace"]} tokio = { version = "1", features = ["macros", "rt", "rt-multi-thread", "time"] } [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.example_interfaces] version = "*" diff --git a/examples/minimal_client_service/package.xml b/examples/minimal_client_service/package.xml index 2927b360e..6ceab386c 100644 --- a/examples/minimal_client_service/package.xml +++ b/examples/minimal_client_service/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_client_service - 0.3.1 + 0.4.0 Package containing an example of the client-service mechanism in rclrs. Esteve Fernandez Apache License 2.0 diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 0990d8555..43947fde3 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_pub_sub" -version = "0.3.1" +version = "0.4.0" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" @@ -29,10 +29,10 @@ path = "src/zero_copy_publisher.rs" anyhow = {version = "1", features = ["backtrace"]} [dependencies.rclrs] -version = "0.3" +version = "0.4" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4" [dependencies.std_msgs] version = "*" diff --git a/examples/minimal_pub_sub/package.xml b/examples/minimal_pub_sub/package.xml index 7ed97620f..2b84438ab 100644 --- a/examples/minimal_pub_sub/package.xml +++ b/examples/minimal_pub_sub/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_pub_sub - 0.3.1 + 0.4.0 Package containing an example of the publish-subscribe mechanism in rclrs. Esteve Fernandez Nikolai Morin diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index c7451db07..01c6d082a 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs" -version = "0.3.1" +version = "0.4.0" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" @@ -21,7 +21,7 @@ futures = "0.3" # Needed for dynamic messages libloading = { version = "0.8", optional = true } # Needed for the Message trait, among others -rosidl_runtime_rs = "0.3" +rosidl_runtime_rs = "0.4" [dev-dependencies] # Needed for e.g. writing yaml files in tests diff --git a/rclrs/package.xml b/rclrs/package.xml index efe2541c7..86ff4b571 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs - 0.3.1 + 0.4.0 Package containing the Rust client library. Esteve Fernandez Nikolai Morin diff --git a/rclrs_example_msgs/package.xml b/rclrs_example_msgs/package.xml index 52ccbf6ef..5425a9c04 100644 --- a/rclrs_example_msgs/package.xml +++ b/rclrs_example_msgs/package.xml @@ -2,7 +2,7 @@ rclrs_example_msgs - 0.3.1 + 0.4.0 A package containing some example message definitions. Nikolai Morin Apache License 2.0 diff --git a/rclrs_tests/Cargo.toml b/rclrs_tests/Cargo.toml index 51708f4e7..3ba64af1e 100644 --- a/rclrs_tests/Cargo.toml +++ b/rclrs_tests/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs_tests" -version = "0.3.1" +version = "0.4.0" authors = ["Chris Reid "] edition = "2021" diff --git a/rclrs_tests/package.xml b/rclrs_tests/package.xml index 82520d5d1..d616c5dc7 100644 --- a/rclrs_tests/package.xml +++ b/rclrs_tests/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs_tests - 0.3.1 + 0.4.0 Package containing tests for rclrs that make use of msgs and other dependencies Chris Reid Apache License 2.0 diff --git a/rosidl_generator_rs/package.xml b/rosidl_generator_rs/package.xml index 18ff07f09..0cad2299a 100644 --- a/rosidl_generator_rs/package.xml +++ b/rosidl_generator_rs/package.xml @@ -2,7 +2,7 @@ rosidl_generator_rs - 0.3.1 + 0.4.0 Generate the ROS interfaces in Rust. Esteve Fernandez Apache License 2.0 diff --git a/rosidl_generator_rs/resource/Cargo.toml.em b/rosidl_generator_rs/resource/Cargo.toml.em index fcf3461d7..7c8f7fe30 100644 --- a/rosidl_generator_rs/resource/Cargo.toml.em +++ b/rosidl_generator_rs/resource/Cargo.toml.em @@ -4,7 +4,7 @@ version = "@(package_version)" edition = "2021" [dependencies] -rosidl_runtime_rs = "0.3" +rosidl_runtime_rs = "0.4" serde = { version = "1", optional = true, features = ["derive"] } serde-big-array = { version = "0.5.1", optional = true } @[for dep in dependency_packages]@ diff --git a/rosidl_runtime_rs/Cargo.toml b/rosidl_runtime_rs/Cargo.toml index e15ef0ff3..2a363be74 100644 --- a/rosidl_runtime_rs/Cargo.toml +++ b/rosidl_runtime_rs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rosidl_runtime_rs" -version = "0.3.1" +version = "0.4.0" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" diff --git a/rosidl_runtime_rs/package.xml b/rosidl_runtime_rs/package.xml index 71bb5dbad..5e97ba2c4 100644 --- a/rosidl_runtime_rs/package.xml +++ b/rosidl_runtime_rs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rosidl_runtime_rs - 0.3.1 + 0.4.0 Message generation code shared by Rust projects in ROS 2 Jacob Hassold From 533abc695b65b8e32a017c27d82da56c54f87b4d Mon Sep 17 00:00:00 2001 From: Miguel Fernandez-Cortizas <43600658+miferco97@users.noreply.github.com> Date: Wed, 15 Nov 2023 15:09:07 +0100 Subject: [PATCH 27/32] [docs] Add to node field in RepublisherNode (#350) --- docs/writing-your-first-rclrs-node.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/writing-your-first-rclrs-node.md b/docs/writing-your-first-rclrs-node.md index b6b1dadd1..335d50499 100644 --- a/docs/writing-your-first-rclrs-node.md +++ b/docs/writing-your-first-rclrs-node.md @@ -47,7 +47,7 @@ use std::sync::Arc; use std_msgs::msg::String as StringMsg; struct RepublisherNode { - node: rclrs::Node, + node: Arc, _subscription: Arc>, data: Option, } @@ -114,7 +114,7 @@ use std::sync::{Arc, Mutex}; // (1) use std_msgs::msg::String as StringMsg; struct RepublisherNode { - node: rclrs::Node, + node: Arc, _subscription: Arc>, data: Arc>>, // (2) } From 3c5cbfa4b0a06db3015e739268f67a753249d8db Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 24 Nov 2023 16:13:15 +0100 Subject: [PATCH 28/32] Allow rustdoc to run without a ROS distribution (#347) * Allow rustdoc to run without a ROS distribution * Extensions to get working * Fail if generate_docs feature is not enabled and ROS_DISTRO is not set * Fix formatting * Fix formatting * Make clippy slightly less unhappy * Do not run clippy for all features if checking rclrs * Clean up rcl_bindings.rs * Fix comparison * Avoid warnings for rosidl_runtime_rs * Avoid running cargo test with all features for rclrs * Ignore rosidl_runtime_rs for cargo test * rosidl_runtime_rs does not have a dyn_msg feature * Add comment about the generate_docs feature --------- Co-authored-by: carter --- .github/workflows/rust.yml | 16 +++- rclrs/Cargo.toml | 12 +++ rclrs/build.rs | 16 +++- rclrs/src/arguments.rs | 2 +- rclrs/src/rcl_bindings.rs | 138 ++++++++++++++++++++++++++++++++++- rosidl_runtime_rs/Cargo.toml | 13 ++++ rosidl_runtime_rs/build.rs | 37 ++++++---- 7 files changed, 213 insertions(+), 21 deletions(-) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 29fa1bee2..c678075f7 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -87,7 +87,12 @@ jobs: for path in $(colcon list | awk '$3 == "(ament_cargo)" { print $2 }'); do cd $path echo "Running clippy in $path" - cargo clippy --all-targets --all-features -- -D warnings + # Run clippy for all features except generate_docs (needed for docs.rs) + if [ "$(basename $path)" = "rclrs" ]; then + cargo clippy --all-targets -F default,dyn_msg -- -D warnings + else + cargo clippy --all-targets --all-features -- -D warnings + fi cd - done @@ -98,7 +103,14 @@ jobs: for path in $(colcon list | awk '$3 == "(ament_cargo)" && $1 != "examples_rclrs_minimal_pub_sub" && $1 != "examples_rclrs_minimal_client_service" { print $2 }'); do cd $path echo "Running cargo test in $path" - cargo test --all-features + # Run cargo test for all features except generate_docs (needed for docs.rs) + if [ "$(basename $path)" = "rclrs" ]; then + cargo test -F default,dyn_msg + elif [ "$(basename $path)" = "rosidl_runtime_rs" ]; then + cargo test -F default + else + cargo test --all-features + fi cd - done diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 01c6d082a..c13b75d1e 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -16,6 +16,9 @@ path = "src/lib.rs" [dependencies] # Needed for dynamically finding type support libraries ament_rs = { version = "0.2", optional = true } +# Needed for uploading documentation to docs.rs +cfg-if = "1.0.0" + # Needed for clients futures = "0.3" # Needed for dynamic messages @@ -30,6 +33,15 @@ tempfile = "3.3.0" [build-dependencies] # Needed for FFI bindgen = "0.66.1" +# Needed for uploading documentation to docs.rs +cfg-if = "1.0.0" [features] +default = [] dyn_msg = ["ament_rs", "libloading"] +# This feature is solely for the purpose of being able to generate documetation without a ROS installation +# The only intended usage of this feature is for docs.rs builders to work, and is not intended to be used by end users +generate_docs = ["rosidl_runtime_rs/generate_docs"] + +[package.metadata.docs.rs] +features = ["generate_docs"] diff --git a/rclrs/build.rs b/rclrs/build.rs index fe8cd8b9a..9da8afa7f 100644 --- a/rclrs/build.rs +++ b/rclrs/build.rs @@ -18,7 +18,21 @@ fn get_env_var_or_abort(env_var: &'static str) -> String { } fn main() { - let ros_distro = get_env_var_or_abort(ROS_DISTRO); + let ros_distro = if let Ok(value) = env::var(ROS_DISTRO) { + value + } else { + let error_msg = + "ROS_DISTRO environment variable not set - please source ROS 2 installation first."; + cfg_if::cfg_if! { + if #[cfg(feature="generate_docs")] { + println!("{}", error_msg); + return; + } else { + panic!("{}", error_msg); + } + } + }; + println!("cargo:rustc-cfg=ros_distro=\"{ros_distro}\""); let mut builder = bindgen::Builder::default() diff --git a/rclrs/src/arguments.rs b/rclrs/src/arguments.rs index c091bf759..21d410a5b 100644 --- a/rclrs/src/arguments.rs +++ b/rclrs/src/arguments.rs @@ -149,7 +149,7 @@ mod tests { .map(|x| x.to_string()); let non_ros_args: Vec = extract_non_ros_args(input_args).unwrap(); - let expected = vec!["non-ros1", "non-ros2", "non-ros3"]; + let expected = ["non-ros1", "non-ros2", "non-ros3"]; if non_ros_args.len() != expected.len() { return Err(format!( diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 245430269..94491bc91 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -11,6 +11,140 @@ #![allow(clippy::all)] #![allow(missing_docs)] -include!(concat!(env!("OUT_DIR"), "/rcl_bindings_generated.rs")); +cfg_if::cfg_if! { + if #[cfg(feature="generate_docs")] { + #[repr(C)] + #[derive(Debug)] + pub struct rcl_allocator_t; -pub const RMW_GID_STORAGE_SIZE: usize = rmw_gid_storage_size_constant; + #[repr(C)] + #[derive(Debug)] + pub struct rcl_arguments_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_client_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_clock_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_clock_type_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_context_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_guard_condition_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_names_and_types_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_node_options_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_node_params_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_node_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_params_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_publisher_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_ret_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_service_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_subscription_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_topic_endpoint_info_array_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_variant_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcl_wait_set_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rcutils_string_array_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_message_info_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_names_and_types_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_qos_durability_policy_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_qos_history_policy_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_qos_liveliness_policy_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_qos_profile_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_qos_reliability_policy_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_request_id_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_time_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rmw_topic_endpoint_info_array_t; + + #[repr(C)] + #[derive(Debug)] + pub struct rosidl_message_type_support_t; + + pub const RMW_GID_STORAGE_SIZE: usize = 24; + + extern "C" { + pub fn rcl_context_is_valid(context: *const rcl_context_t) -> bool; + } + } else { + include!(concat!(env!("OUT_DIR"), "/rcl_bindings_generated.rs")); + + pub const RMW_GID_STORAGE_SIZE: usize = rmw_gid_storage_size_constant; + } +} diff --git a/rosidl_runtime_rs/Cargo.toml b/rosidl_runtime_rs/Cargo.toml index 2a363be74..e679b87a9 100644 --- a/rosidl_runtime_rs/Cargo.toml +++ b/rosidl_runtime_rs/Cargo.toml @@ -17,8 +17,21 @@ path = "src/lib.rs" # formats such as JSON, YAML, Pickle, etc. serde = { version = "1", optional = true } +[features] +default = [] +# This feature is solely for the purpose of being able to generate documetation without a ROS installation +# The only intended usage of this feature is for docs.rs builders to work, and is not intended to be used by end users +generate_docs = [] + [dev-dependencies] # Needed for writing property tests quickcheck = "1" # Needed for testing serde support serde_json = "1" + +[build-dependencies] +# Needed for uploading documentation to docs.rs +cfg-if = "1.0.0" + +[package.metadata.docs.rs] +features = ["generate_docs"] diff --git a/rosidl_runtime_rs/build.rs b/rosidl_runtime_rs/build.rs index b895cd24b..fb98aeb30 100644 --- a/rosidl_runtime_rs/build.rs +++ b/rosidl_runtime_rs/build.rs @@ -1,24 +1,31 @@ -use std::env; -use std::path::Path; +cfg_if::cfg_if! { + if #[cfg(not(feature="generate_docs"))] { + use std::env; + use std::path::Path; -const AMENT_PREFIX_PATH: &str = "AMENT_PREFIX_PATH"; + const AMENT_PREFIX_PATH: &str = "AMENT_PREFIX_PATH"; -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 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 ament_prefix_path_list = get_env_var_or_abort(AMENT_PREFIX_PATH); - for ament_prefix_path in ament_prefix_path_list.split(':') { - let library_path = Path::new(ament_prefix_path).join("lib"); - println!("cargo:rustc-link-search=native={}", library_path.display()); + #[cfg(not(feature = "generate_docs"))] + { + let ament_prefix_path_list = get_env_var_or_abort(AMENT_PREFIX_PATH); + for ament_prefix_path in ament_prefix_path_list.split(':') { + let library_path = Path::new(ament_prefix_path).join("lib"); + println!("cargo:rustc-link-search=native={}", library_path.display()); + } } // Invalidate the built crate whenever this script changes From 89eae6533da894edf61f18929aeeb38bccc42b0e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 28 Nov 2023 16:48:36 +0100 Subject: [PATCH 29/32] Update GitHub actions (#352) * Update GitHub actions * Downgrade rust-toolchain to the latest stable version (1.74.0) from 1.80.0 * Fix issues with latest clippy * Fix rustdoc check issue * Update Rust toolchain in Dockerfile --- .github/workflows/rust.yml | 6 +++--- Dockerfile | 2 +- rclrs/src/lib.rs | 9 ++++----- rosidl_runtime_rs/src/string.rs | 2 +- rosidl_runtime_rs/src/string/serde.rs | 2 +- 5 files changed, 10 insertions(+), 11 deletions(-) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index c678075f7..02a142d66 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -37,7 +37,7 @@ jobs: container: image: ${{ matrix.docker_image }} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Search packages in this repository id: list_packages @@ -45,13 +45,13 @@ jobs: echo ::set-output name=package_list::$(colcon list --names-only) - name: Setup ROS environment - uses: ros-tooling/setup-ros@v0.6 + uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distribution }} use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} - name: Setup Rust - uses: dtolnay/rust-toolchain@1.71.0 + uses: dtolnay/rust-toolchain@1.74.0 with: components: clippy, rustfmt diff --git a/Dockerfile b/Dockerfile index 81a3fa61f..b148eafe3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -12,7 +12,7 @@ RUN apt-get update && apt-get install -y \ && rm -rf /var/lib/apt/lists/* # Install Rust and the cargo-ament-build plugin -RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.71.0 -y +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.74.0 -y ENV PATH=/root/.cargo/bin:$PATH RUN cargo install cargo-ament-build diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index d3a752892..325cd5b9a 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -89,14 +89,13 @@ pub fn create_node(context: &Context, node_name: &str) -> Result, Rclr Node::new(context, node_name) } -/// Creates a [`NodeBuilder`][1]. +/// Creates a [`NodeBuilder`]. /// -/// Convenience function equivalent to [`NodeBuilder::new()`][2] and [`Node::builder()`][3]. +/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`Node::builder()`][2]. /// Please see that function's documentation. /// -/// [1]: crate::NodeBuilder -/// [2]: crate::NodeBuilder::new -/// [3]: crate::Node::builder +/// [1]: crate::NodeBuilder::new +/// [2]: crate::Node::builder /// /// # Example /// ``` diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index 44043a60d..14025fc5e 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -250,7 +250,7 @@ macro_rules! string_impl { impl PartialOrd for $string { fn partial_cmp(&self, other: &Self) -> Option { - self.deref().partial_cmp(other.deref()) + Some(self.cmp(other)) } } diff --git a/rosidl_runtime_rs/src/string/serde.rs b/rosidl_runtime_rs/src/string/serde.rs index be1058a47..fe68661b5 100644 --- a/rosidl_runtime_rs/src/string/serde.rs +++ b/rosidl_runtime_rs/src/string/serde.rs @@ -125,7 +125,7 @@ impl Serialize for WString { { // Not particularly efficient // SAFETY: See the Display implementation. - let u16_slice = unsafe { std::slice::from_raw_parts(self.data as *mut u16, self.size) }; + let u16_slice = unsafe { std::slice::from_raw_parts(self.data, self.size) }; let s = std::string::String::from_utf16_lossy(u16_slice); serializer.serialize_str(&s) } From f29144bfad5188aa98a75acdec4df166283f2376 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 28 Nov 2023 16:49:19 +0100 Subject: [PATCH 30/32] Version 0.4.1 (#353) --- examples/message_demo/Cargo.toml | 2 +- examples/message_demo/package.xml | 2 +- examples/minimal_client_service/Cargo.toml | 2 +- examples/minimal_client_service/package.xml | 2 +- examples/minimal_pub_sub/Cargo.toml | 2 +- examples/minimal_pub_sub/package.xml | 2 +- rclrs/Cargo.toml | 2 +- rclrs/package.xml | 2 +- rclrs_example_msgs/package.xml | 2 +- rclrs_tests/Cargo.toml | 2 +- rclrs_tests/package.xml | 2 +- rosidl_generator_rs/package.xml | 2 +- rosidl_runtime_rs/Cargo.toml | 2 +- rosidl_runtime_rs/package.xml | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/examples/message_demo/Cargo.toml b/examples/message_demo/Cargo.toml index 94cd483e6..759960805 100644 --- a/examples/message_demo/Cargo.toml +++ b/examples/message_demo/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_message_demo" -version = "0.4.0" +version = "0.4.1" authors = ["Nikolai Morin "] edition = "2021" diff --git a/examples/message_demo/package.xml b/examples/message_demo/package.xml index 96eadd2fe..60519f49d 100644 --- a/examples/message_demo/package.xml +++ b/examples/message_demo/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_message_demo - 0.4.0 + 0.4.1 Package containing an example of message-related functionality in rclrs. Nikolai Morin Apache License 2.0 diff --git a/examples/minimal_client_service/Cargo.toml b/examples/minimal_client_service/Cargo.toml index e520c0d81..618b0028c 100644 --- a/examples/minimal_client_service/Cargo.toml +++ b/examples/minimal_client_service/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_client_service" -version = "0.4.0" +version = "0.4.1" authors = ["Esteve Fernandez "] edition = "2021" diff --git a/examples/minimal_client_service/package.xml b/examples/minimal_client_service/package.xml index 6ceab386c..dd4e41b12 100644 --- a/examples/minimal_client_service/package.xml +++ b/examples/minimal_client_service/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_client_service - 0.4.0 + 0.4.1 Package containing an example of the client-service mechanism in rclrs. Esteve Fernandez Apache License 2.0 diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 43947fde3..52c4f0544 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "examples_rclrs_minimal_pub_sub" -version = "0.4.0" +version = "0.4.1" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" diff --git a/examples/minimal_pub_sub/package.xml b/examples/minimal_pub_sub/package.xml index 2b84438ab..da3e76041 100644 --- a/examples/minimal_pub_sub/package.xml +++ b/examples/minimal_pub_sub/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> examples_rclrs_minimal_pub_sub - 0.4.0 + 0.4.1 Package containing an example of the publish-subscribe mechanism in rclrs. Esteve Fernandez Nikolai Morin diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index c13b75d1e..c84287152 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs" -version = "0.4.0" +version = "0.4.1" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" diff --git a/rclrs/package.xml b/rclrs/package.xml index 86ff4b571..800ccd632 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs - 0.4.0 + 0.4.1 Package containing the Rust client library. Esteve Fernandez Nikolai Morin diff --git a/rclrs_example_msgs/package.xml b/rclrs_example_msgs/package.xml index 5425a9c04..a6677862c 100644 --- a/rclrs_example_msgs/package.xml +++ b/rclrs_example_msgs/package.xml @@ -2,7 +2,7 @@ rclrs_example_msgs - 0.4.0 + 0.4.1 A package containing some example message definitions. Nikolai Morin Apache License 2.0 diff --git a/rclrs_tests/Cargo.toml b/rclrs_tests/Cargo.toml index 3ba64af1e..2f484a352 100644 --- a/rclrs_tests/Cargo.toml +++ b/rclrs_tests/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rclrs_tests" -version = "0.4.0" +version = "0.4.1" authors = ["Chris Reid "] edition = "2021" diff --git a/rclrs_tests/package.xml b/rclrs_tests/package.xml index d616c5dc7..e18023270 100644 --- a/rclrs_tests/package.xml +++ b/rclrs_tests/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rclrs_tests - 0.4.0 + 0.4.1 Package containing tests for rclrs that make use of msgs and other dependencies Chris Reid Apache License 2.0 diff --git a/rosidl_generator_rs/package.xml b/rosidl_generator_rs/package.xml index 0cad2299a..bd6124cf1 100644 --- a/rosidl_generator_rs/package.xml +++ b/rosidl_generator_rs/package.xml @@ -2,7 +2,7 @@ rosidl_generator_rs - 0.4.0 + 0.4.1 Generate the ROS interfaces in Rust. Esteve Fernandez Apache License 2.0 diff --git a/rosidl_runtime_rs/Cargo.toml b/rosidl_runtime_rs/Cargo.toml index e679b87a9..4e4891638 100644 --- a/rosidl_runtime_rs/Cargo.toml +++ b/rosidl_runtime_rs/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rosidl_runtime_rs" -version = "0.4.0" +version = "0.4.1" # This project is not military-sponsored, Jacob's employment contract just requires him to use this email address authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] edition = "2021" diff --git a/rosidl_runtime_rs/package.xml b/rosidl_runtime_rs/package.xml index 5e97ba2c4..44e55f3aa 100644 --- a/rosidl_runtime_rs/package.xml +++ b/rosidl_runtime_rs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rosidl_runtime_rs - 0.4.0 + 0.4.1 Message generation code shared by Rust projects in ROS 2 Jacob Hassold From 908281699949ad79405d1906f78922dbf837aecc Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 5 Dec 2023 19:54:45 +0800 Subject: [PATCH 31/32] Remove leading underscore from private fields (#354) * Remove leading underscore from private fields Signed-off-by: Luca Della Vedova * Revert renaming to suppress unused warning Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova --- rclrs/src/clock.rs | 38 ++++++++++++------------- rclrs/src/node.rs | 12 ++++---- rclrs/src/node/builder.rs | 8 +++--- rclrs/src/parameter.rs | 26 ++++++++--------- rclrs/src/time_source.rs | 60 +++++++++++++++++++-------------------- 5 files changed, 72 insertions(+), 72 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index c89705d9d..018c6f5b7 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -28,15 +28,15 @@ impl From for rcl_clock_type_t { /// Struct that implements a Clock and wraps `rcl_clock_t`. #[derive(Clone, Debug)] pub struct Clock { - _type: ClockType, - _rcl_clock: Arc>, + kind: ClockType, + rcl_clock: Arc>, // TODO(luca) Implement jump callbacks } /// A clock source that can be used to drive the contained clock. Created when a clock of type /// `ClockType::RosTime` is constructed pub struct ClockSource { - _rcl_clock: Arc>, + rcl_clock: Arc>, } impl Clock { @@ -54,19 +54,19 @@ impl Clock { /// to update it pub fn with_source() -> (Self, ClockSource) { let clock = Self::make(ClockType::RosTime); - let clock_source = ClockSource::new(clock._rcl_clock.clone()); + let clock_source = ClockSource::new(clock.rcl_clock.clone()); (clock, clock_source) } /// Creates a new clock of the given `ClockType`. - pub fn new(type_: ClockType) -> (Self, Option) { - let clock = Self::make(type_); + pub fn new(kind: ClockType) -> (Self, Option) { + let clock = Self::make(kind); let clock_source = - matches!(type_, ClockType::RosTime).then(|| ClockSource::new(clock._rcl_clock.clone())); + matches!(kind, ClockType::RosTime).then(|| ClockSource::new(clock.rcl_clock.clone())); (clock, clock_source) } - fn make(type_: ClockType) -> Self { + fn make(kind: ClockType) -> Self { let mut rcl_clock; unsafe { // SAFETY: Getting a default value is always safe. @@ -74,24 +74,24 @@ impl Clock { let mut allocator = rcutils_get_default_allocator(); // Function will return Err(_) only if there isn't enough memory to allocate a clock // object. - rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator) + rcl_clock_init(kind.into(), &mut rcl_clock, &mut allocator) .ok() .unwrap(); } Self { - _type: type_, - _rcl_clock: Arc::new(Mutex::new(rcl_clock)), + kind, + rcl_clock: Arc::new(Mutex::new(rcl_clock)), } } /// Returns the clock's `ClockType`. pub fn clock_type(&self) -> ClockType { - self._type + self.kind } /// Returns the current clock's timestamp. pub fn now(&self) -> Time { - let mut clock = self._rcl_clock.lock().unwrap(); + let mut clock = self.rcl_clock.lock().unwrap(); let mut time_point: i64 = 0; unsafe { // SAFETY: No preconditions for this function @@ -99,7 +99,7 @@ impl Clock { } Time { nsec: time_point, - clock: Arc::downgrade(&self._rcl_clock), + clock: Arc::downgrade(&self.rcl_clock), } } @@ -128,14 +128,14 @@ impl Drop for ClockSource { impl PartialEq for ClockSource { fn eq(&self, other: &Self) -> bool { - Arc::ptr_eq(&self._rcl_clock, &other._rcl_clock) + Arc::ptr_eq(&self.rcl_clock, &other.rcl_clock) } } impl ClockSource { /// Sets the value of the current ROS time. pub fn set_ros_time_override(&self, nanoseconds: i64) { - let mut clock = self._rcl_clock.lock().unwrap(); + let mut clock = self.rcl_clock.lock().unwrap(); // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed // by the mutex unsafe { @@ -147,8 +147,8 @@ impl ClockSource { } } - fn new(clock: Arc>) -> Self { - let source = Self { _rcl_clock: clock }; + fn new(rcl_clock: Arc>) -> Self { + let source = Self { rcl_clock }; source.set_ros_time_enable(true); source } @@ -156,7 +156,7 @@ impl ClockSource { /// Sets the clock to use ROS Time, if enabled the clock will report the last value set through /// `Clock::set_ros_time_override(nanoseconds: i64)`. fn set_ros_time_enable(&self, enable: bool) { - let mut clock = self._rcl_clock.lock().unwrap(); + let mut clock = self.rcl_clock.lock().unwrap(); if enable { // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed // by the mutex diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b91e16276..f1792c1e6 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -71,8 +71,8 @@ pub struct Node { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, - _time_source: TimeSource, - _parameter: ParameterInterface, + time_source: TimeSource, + parameter: ParameterInterface, } impl Eq for Node {} @@ -102,7 +102,7 @@ impl Node { /// Returns the clock associated with this node. pub fn get_clock(&self) -> Clock { - self._time_source.get_clock() + self.time_source.get_clock() } /// Returns the name of the node. @@ -398,16 +398,16 @@ impl Node { &'a self, name: impl Into>, ) -> ParameterBuilder<'a, T> { - self._parameter.declare(name.into()) + self.parameter.declare(name.into()) } /// Enables usage of undeclared parameters for this node. /// /// Returns a [`Parameters`] struct that can be used to get and set all parameters. pub fn use_undeclared_parameters(&self) -> Parameters { - self._parameter.allow_undeclared(); + self.parameter.allow_undeclared(); Parameters { - interface: &self._parameter, + interface: &self.parameter, } } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 543bd50d2..2dd82a79c 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -281,7 +281,7 @@ impl NodeBuilder { }; let rcl_node_mtx = Arc::new(Mutex::new(rcl_node)); - let _parameter = ParameterInterface::new( + let parameter = ParameterInterface::new( &rcl_node_mtx, &rcl_node_options.arguments, &rcl_context.global_arguments, @@ -293,12 +293,12 @@ impl NodeBuilder { guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), - _time_source: TimeSource::builder(self.clock_type) + time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), - _parameter, + parameter, }); - node._time_source.attach_node(&node); + node.time_source.attach_node(&node); Ok(node) } diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index d04e36623..75256d869 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -430,7 +430,7 @@ impl TryFrom> for OptionalParameter name: builder.name, value, ranges, - map: Arc::downgrade(&builder.interface._parameter_map), + map: Arc::downgrade(&builder.interface.parameter_map), _marker: Default::default(), }) } @@ -494,7 +494,7 @@ impl<'a, T: ParameterVariant + 'a> TryFrom> for Mandator name: builder.name, value, ranges, - map: Arc::downgrade(&builder.interface._parameter_map), + map: Arc::downgrade(&builder.interface.parameter_map), _marker: Default::default(), }) } @@ -586,7 +586,7 @@ impl<'a, T: ParameterVariant + 'a> TryFrom> for ReadOnly Ok(ReadOnlyParameter { name: builder.name, value, - map: Arc::downgrade(&builder.interface._parameter_map), + map: Arc::downgrade(&builder.interface.parameter_map), _marker: Default::default(), }) } @@ -703,7 +703,7 @@ impl<'a> Parameters<'a> { /// /// Returns `Some(T)` if a parameter of the requested type exists, `None` otherwise. pub fn get(&self, name: &str) -> Option { - let storage = &self.interface._parameter_map.lock().unwrap().storage; + let storage = &self.interface.parameter_map.lock().unwrap().storage; let storage = storage.get(name)?; match storage { ParameterStorage::Declared(storage) => match &storage.value { @@ -728,7 +728,7 @@ impl<'a> Parameters<'a> { name: impl Into>, value: T, ) -> Result<(), ParameterValueError> { - let mut map = self.interface._parameter_map.lock().unwrap(); + let mut map = self.interface.parameter_map.lock().unwrap(); let name: Arc = name.into(); match map.storage.entry(name) { Entry::Occupied(mut entry) => { @@ -767,8 +767,8 @@ impl<'a> Parameters<'a> { } pub(crate) struct ParameterInterface { - _parameter_map: Arc>, - _override_map: ParameterOverrideMap, + parameter_map: Arc>, + override_map: ParameterOverrideMap, allow_undeclared: AtomicBool, // NOTE(luca-della-vedova) add a ParameterService field to this struct to add support for // services. @@ -781,14 +781,14 @@ impl ParameterInterface { global_arguments: &rcl_arguments_t, ) -> Result { let rcl_node = rcl_node_mtx.lock().unwrap(); - let _override_map = unsafe { + let override_map = unsafe { let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name); resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? }; Ok(ParameterInterface { - _parameter_map: Default::default(), - _override_map, + parameter_map: Default::default(), + override_map, allow_undeclared: Default::default(), }) } @@ -820,7 +820,7 @@ impl ParameterInterface { ranges.validate()?; let override_value: Option = if ignore_override { None - } else if let Some(override_value) = self._override_map.get(name).cloned() { + } else if let Some(override_value) = self.override_map.get(name).cloned() { Some( override_value .try_into() @@ -831,7 +831,7 @@ impl ParameterInterface { }; let prior_value = - if let Some(prior_value) = self._parameter_map.lock().unwrap().storage.get(name) { + if let Some(prior_value) = self.parameter_map.lock().unwrap().storage.get(name) { match prior_value { ParameterStorage::Declared(_) => return Err(DeclarationError::AlreadyDeclared), ParameterStorage::Undeclared(param) => match param.clone().try_into() { @@ -869,7 +869,7 @@ impl ParameterInterface { value: DeclaredValue, options: ParameterOptionsStorage, ) { - self._parameter_map.lock().unwrap().storage.insert( + self.parameter_map.lock().unwrap().storage.insert( name, ParameterStorage::Declared(DeclaredStorage { options, diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 102cdbd08..19274fa9e 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -7,14 +7,14 @@ use std::sync::{Arc, Mutex, RwLock, Weak}; /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe /// to the `/clock` topic and drive the attached clock pub(crate) struct TimeSource { - _node: Mutex>, - _clock: RwLock, - _clock_source: Arc>>, - _requested_clock_type: ClockType, - _clock_qos: QoSProfile, - _clock_subscription: Mutex>>>, - _last_received_time: Arc>>, - _use_sim_time: Mutex>>, + node: Mutex>, + clock: RwLock, + clock_source: Arc>>, + requested_clock_type: ClockType, + clock_qos: QoSProfile, + clock_subscription: Mutex>>>, + last_received_time: Arc>>, + use_sim_time: Mutex>>, } /// A builder for creating a [`TimeSource`][1]. @@ -56,14 +56,14 @@ impl TimeSourceBuilder { ClockType::SteadyTime => Clock::steady(), }; TimeSource { - _node: Mutex::new(Weak::new()), - _clock: RwLock::new(clock), - _clock_source: Arc::new(Mutex::new(None)), - _requested_clock_type: self.clock_type, - _clock_qos: self.clock_qos, - _clock_subscription: Mutex::new(None), - _last_received_time: Arc::new(Mutex::new(None)), - _use_sim_time: Mutex::new(None), + node: Mutex::new(Weak::new()), + clock: RwLock::new(clock), + clock_source: Arc::new(Mutex::new(None)), + requested_clock_type: self.clock_type, + clock_qos: self.clock_qos, + clock_subscription: Mutex::new(None), + last_received_time: Arc::new(Mutex::new(None)), + use_sim_time: Mutex::new(None), } } } @@ -76,7 +76,7 @@ impl TimeSource { /// Returns the clock that this TimeSource is controlling. pub(crate) fn get_clock(&self) -> Clock { - self._clock.read().unwrap().clone() + self.clock.read().unwrap().clone() } /// Attaches the given node to to the `TimeSource`, using its interface to read the @@ -89,27 +89,27 @@ impl TimeSource { .default(false) .mandatory() .unwrap(); - *self._node.lock().unwrap() = Arc::downgrade(node); + *self.node.lock().unwrap() = Arc::downgrade(node); self.set_ros_time_enable(param.get()); - *self._use_sim_time.lock().unwrap() = Some(param); + *self.use_sim_time.lock().unwrap() = Some(param); } fn set_ros_time_enable(&self, enable: bool) { - if matches!(self._requested_clock_type, ClockType::RosTime) { - let mut clock = self._clock.write().unwrap(); + if matches!(self.requested_clock_type, ClockType::RosTime) { + let mut clock = self.clock.write().unwrap(); if enable && matches!(clock.clock_type(), ClockType::SystemTime) { let (new_clock, mut clock_source) = Clock::with_source(); - if let Some(last_received_time) = *self._last_received_time.lock().unwrap() { + if let Some(last_received_time) = *self.last_received_time.lock().unwrap() { Self::update_clock(&mut clock_source, last_received_time); } *clock = new_clock; - *self._clock_source.lock().unwrap() = Some(clock_source); - *self._clock_subscription.lock().unwrap() = Some(self.create_clock_sub()); + *self.clock_source.lock().unwrap() = Some(clock_source); + *self.clock_subscription.lock().unwrap() = Some(self.create_clock_sub()); } if !enable && matches!(clock.clock_type(), ClockType::RosTime) { *clock = Clock::system(); - *self._clock_source.lock().unwrap() = None; - *self._clock_subscription.lock().unwrap() = None; + *self.clock_source.lock().unwrap() = None; + *self.clock_subscription.lock().unwrap() = None; } } } @@ -119,15 +119,15 @@ impl TimeSource { } fn create_clock_sub(&self) -> Arc> { - let clock = self._clock_source.clone(); - let last_received_time = self._last_received_time.clone(); + let clock = self.clock_source.clone(); + let last_received_time = self.last_received_time.clone(); // Safe to unwrap since the function will only fail if invalid arguments are provided - self._node + self.node .lock() .unwrap() .upgrade() .unwrap() - .create_subscription::("/clock", self._clock_qos, move |msg: ClockMsg| { + .create_subscription::("/clock", self.clock_qos, move |msg: ClockMsg| { let nanoseconds: i64 = (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; *last_received_time.lock().unwrap() = Some(nanoseconds); From 812b91a98cd1c1862b556f160d03b6bd15bd8d47 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Sat, 30 Dec 2023 20:53:10 +0100 Subject: [PATCH 32/32] Add Arc::clone to the Readme tutorial to fix compile error (#356) --- docs/writing-your-first-rclrs-node.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/writing-your-first-rclrs-node.md b/docs/writing-your-first-rclrs-node.md index 335d50499..6f252675c 100644 --- a/docs/writing-your-first-rclrs-node.md +++ b/docs/writing-your-first-rclrs-node.md @@ -212,7 +212,7 @@ fn main() -> Result<(), rclrs::RclrsError> { republisher_other_thread.republish()?; } }); - rclrs::spin(republisher.node) + rclrs::spin(Arc::clone(&republisher.node)) } ```