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)