diff --git a/rclrs/package.xml b/rclrs/package.xml index 01548c59d..70b7d349d 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -16,6 +16,8 @@ libclang-dev rosidl_runtime_rs rcl + builtin_interfaces + rcl_interfaces ament_cargo diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4d43ffe2b..2cf90e843 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -15,6 +15,7 @@ mod publisher; mod qos; mod service; mod subscription; +mod vendor; mod wait; mod rcl_bindings; diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs new file mode 100644 index 000000000..bcbe24beb --- /dev/null +++ b/rclrs/src/parameter/service.rs @@ -0,0 +1,89 @@ +use std::sync::{Arc, Weak, Mutex}; + +use crate::vendor::rcl_interfaces::srv::rmw::*; +use crate::vendor::rcl_interfaces::msg::rmw::*; +use rosidl_runtime_rs::{Sequence, seq}; + +use crate::{rmw_request_id_t, Node, RclrsError, Service, ServiceBase}; +use crate::rcl_bindings::rcl_node_t; + +pub struct ParameterService { + describe_parameters_service: Arc>, + get_parameter_types_service: Arc>, + get_parameters_service: Arc>, + list_parameters_service: Arc>, + set_parameters_service: Arc>, + set_parameters_atomically_service: Arc>, +} + +impl ParameterService { + pub fn new(rcl_node_mtx: Arc>) -> Result { + let describe_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + "describe_parameters", + |req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + DescribeParameters_Response { + descriptors: seq![] + } + }, + )?; + let get_parameter_types_service = Service::new(Arc::clone(&rcl_node_mtx), + "get_parameter_types", + |req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + GetParameterTypes_Response { + types: seq![] + } + }, + )?; + let get_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + "get_parameters", + |req_id: &rmw_request_id_t, req: GetParameters_Request| { + GetParameters_Response { + values: seq![] + } + }, + )?; + let list_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + "list_parameters", + |req_id: &rmw_request_id_t, req: ListParameters_Request| { + ListParameters_Response { + result: ListParametersResult::default() + } + }, + )?; + let set_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + "set_parameters", + |req_id: &rmw_request_id_t, req: SetParameters_Request| { + SetParameters_Response { + results: seq![] + } + }, + )?; + let set_parameters_atomically_service = Service::new(Arc::clone(&rcl_node_mtx), + "set_parameters_atomically", + |req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + SetParametersAtomically_Response { + result: SetParametersResult::default() + } + }, + )?; + Ok(Self { + describe_parameters_service: Arc::new(describe_parameters_service), + get_parameter_types_service: Arc::new(get_parameter_types_service), + get_parameters_service: Arc::new(get_parameters_service), + list_parameters_service: Arc::new(list_parameters_service), + set_parameters_service: Arc::new(set_parameters_service), + set_parameters_atomically_service: Arc::new(set_parameters_atomically_service), + }) + } + + pub fn services(&self) -> Vec> { + vec![ + Arc::downgrade(&self.describe_parameters_service) as Weak, + Arc::downgrade(&self.get_parameter_types_service) as Weak, + Arc::downgrade(&self.get_parameters_service) as Weak, + Arc::downgrade(&self.list_parameters_service) as Weak, + Arc::downgrade(&self.set_parameters_service) as Weak, + Arc::downgrade(&self.set_parameters_atomically_service) as Weak, + ] + } +} diff --git a/rclrs/src/vendor/builtin_interfaces/mod.rs b/rclrs/src/vendor/builtin_interfaces/mod.rs new file mode 100644 index 000000000..a6365b3b8 --- /dev/null +++ b/rclrs/src/vendor/builtin_interfaces/mod.rs @@ -0,0 +1,3 @@ +#![allow(non_camel_case_types)] + +pub mod msg; diff --git a/rclrs/src/vendor/builtin_interfaces/msg.rs b/rclrs/src/vendor/builtin_interfaces/msg.rs new file mode 100644 index 000000000..371803171 --- /dev/null +++ b/rclrs/src/vendor/builtin_interfaces/msg.rs @@ -0,0 +1,258 @@ +pub mod rmw { + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "builtin_interfaces__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Duration( + ) -> *const std::os::raw::c_void; + } + + #[link(name = "builtin_interfaces__rosidl_generator_c")] + extern "C" { + fn builtin_interfaces__msg__Duration__init(msg: *mut Duration) -> bool; + fn builtin_interfaces__msg__Duration__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn builtin_interfaces__msg__Duration__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn builtin_interfaces__msg__Duration__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to builtin_interfaces__msg__Duration + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct Duration { + pub sec: i32, + pub nanosec: u32, + } + + impl Default for Duration { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !builtin_interfaces__msg__Duration__init(&mut msg as *mut _) { + panic!("Call to builtin_interfaces__msg__Duration__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for Duration { + 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 { builtin_interfaces__msg__Duration__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 { builtin_interfaces__msg__Duration__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 { builtin_interfaces__msg__Duration__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for Duration { + 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 Duration + where + Self: Sized, + { + const TYPE_NAME: &'static str = "builtin_interfaces/msg/Duration"; + 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__builtin_interfaces__msg__Duration() + } + } + } + + #[link(name = "builtin_interfaces__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Time( + ) -> *const std::os::raw::c_void; + } + + #[link(name = "builtin_interfaces__rosidl_generator_c")] + extern "C" { + fn builtin_interfaces__msg__Time__init(msg: *mut Time) -> bool; + fn builtin_interfaces__msg__Time__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence