diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 8ea67974b..f489ea3f0 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -34,6 +34,8 @@ rosidl_runtime_rs = "0.4" tempfile = "3.3.0" # Needed for publisher and subscriber tests test_msgs = {version = "*"} +# Needed for parameter service tests +tokio = { version = "*", features = ["rt", "time", "macros"] } [build-dependencies] # Needed for FFI diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 92adc83b1..011d5d2f3 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -18,6 +18,7 @@ use crate::{ /// - `use_global_arguments: true` /// - `arguments: []` /// - `enable_rosout: true` +/// - `start_parameter_services: true` /// - `clock_type: ClockType::RosTime` /// - `clock_qos: QOS_PROFILE_CLOCK` /// @@ -49,6 +50,7 @@ pub struct NodeBuilder { use_global_arguments: bool, arguments: Vec, enable_rosout: bool, + start_parameter_services: bool, clock_type: ClockType, clock_qos: QoSProfile, } @@ -97,6 +99,7 @@ impl NodeBuilder { use_global_arguments: true, arguments: vec![], enable_rosout: true, + start_parameter_services: true, clock_type: ClockType::RosTime, clock_qos: QOS_PROFILE_CLOCK, } @@ -231,6 +234,15 @@ impl NodeBuilder { self } + /// Enables or disables parameter services. + /// + /// Parameter services can be used to allow external nodes to list, get and set + /// parameters for this node. + pub fn start_parameter_services(mut self, start: bool) -> Self { + self.start_parameter_services = start; + self + } + /// Sets the node's clock type. pub fn clock_type(mut self, clock_type: ClockType) -> Self { self.clock_type = clock_type; @@ -308,6 +320,9 @@ impl NodeBuilder { parameter, }); node.time_source.attach_node(&node); + if self.start_parameter_services { + node.parameter.create_services(&node)?; + } Ok(node) } diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index e19d13e13..c8079b32a 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -519,16 +519,16 @@ mod tests { assert!(subscription_infos.is_empty()); - // Test that the graph has no services + // Test that the graph only has 6 services (parameter services) let names_and_topics = node .get_service_names_and_types_by_node(node_name, "") .unwrap(); - assert_eq!(names_and_topics.len(), 0); + assert_eq!(names_and_topics.len(), 6); let names_and_topics = node.get_service_names_and_types().unwrap(); - assert_eq!(names_and_topics.len(), 0); + assert_eq!(names_and_topics.len(), 6); // Test that the graph has no clients let names_and_topics = node diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index d1a334f6b..c8a710eeb 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -1,18 +1,21 @@ mod override_map; +mod range; +mod service; mod value; pub(crate) use override_map::*; +pub use range::*; +use service::*; pub use value::*; -use crate::{call_string_getter_with_rcl_node, rcl_bindings::*, RclrsError}; +use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; + +use crate::{call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError}; use std::{ collections::{btree_map::Entry, BTreeMap}, fmt::Debug, marker::PhantomData, - sync::{ - atomic::{AtomicBool, Ordering}, - Arc, Mutex, RwLock, Weak, - }, + sync::{Arc, Mutex, RwLock, Weak}, }; // This module implements the core logic of parameters in rclrs. @@ -37,16 +40,16 @@ use std::{ #[derive(Clone, Debug)] struct ParameterOptionsStorage { - _description: Arc, - _constraints: Arc, + description: Arc, + constraints: Arc, ranges: ParameterRanges, } impl From> for ParameterOptionsStorage { fn from(opts: ParameterOptions) -> Self { Self { - _description: opts.description, - _constraints: opts.constraints, + description: opts.description, + constraints: opts.constraints, ranges: opts.ranges.into(), } } @@ -71,159 +74,6 @@ impl Default for ParameterOptions { } } -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>), @@ -606,9 +456,104 @@ enum ParameterStorage { Undeclared(ParameterValue), } +impl ParameterStorage { + pub(crate) fn to_parameter_type(&self) -> u8 { + match self { + ParameterStorage::Declared(s) => match s.kind { + ParameterKind::Bool => ParameterType::PARAMETER_BOOL, + ParameterKind::Integer => ParameterType::PARAMETER_INTEGER, + ParameterKind::Double => ParameterType::PARAMETER_DOUBLE, + ParameterKind::String => ParameterType::PARAMETER_STRING, + ParameterKind::ByteArray => ParameterType::PARAMETER_BYTE_ARRAY, + ParameterKind::BoolArray => ParameterType::PARAMETER_BOOL_ARRAY, + ParameterKind::IntegerArray => ParameterType::PARAMETER_INTEGER_ARRAY, + ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, + ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, + ParameterKind::Dynamic => match &s.value { + // Unwraps here are safe because None will only be returned if the RwLock is + // poisoned, but it is only written in internal set(value) calls that have no + // way to panic. + DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), + DeclaredValue::Optional(v) => v + .read() + .unwrap() + .as_ref() + .map(|v| v.rcl_parameter_type()) + .unwrap_or(ParameterType::PARAMETER_NOT_SET), + DeclaredValue::ReadOnly(v) => v.rcl_parameter_type(), + }, + }, + ParameterStorage::Undeclared(value) => value.rcl_parameter_type(), + } + } +} + #[derive(Debug, Default)] -struct ParameterMap { +pub(crate) struct ParameterMap { storage: BTreeMap, ParameterStorage>, + allow_undeclared: bool, +} + +impl ParameterMap { + /// Validates the requested parameter setting and returns an error if the requested value is + /// not valid. + fn validate_parameter_setting( + &self, + name: &str, + value: RmwParameterValue, + ) -> Result { + let Ok(value): Result = value.try_into() else { + return Err("Invalid parameter type"); + }; + match self.storage.get(name) { + Some(entry) => { + if let ParameterStorage::Declared(storage) = entry { + if std::mem::discriminant(&storage.kind) + == std::mem::discriminant(&value.kind()) + || matches!(storage.kind, ParameterKind::Dynamic) + { + if !storage.options.ranges.in_range(&value) { + return Err("Parameter value is out of range"); + } + if matches!(&storage.value, DeclaredValue::ReadOnly(_)) { + return Err("Parameter is read only"); + } + } else { + return Err( + "Parameter set to different type and dynamic typing is disabled", + ); + } + } + } + None => { + if !self.allow_undeclared { + return Err( + "Parameter was not declared and undeclared parameters are not allowed", + ); + } + } + } + Ok(value) + } + + /// Stores the requested parameter in the map. + fn store_parameter(&mut self, name: Arc, value: ParameterValue) { + match self.storage.entry(name) { + Entry::Occupied(mut entry) => match entry.get_mut() { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(p) => *p.write().unwrap() = value, + DeclaredValue::Optional(p) => *p.write().unwrap() = Some(value), + DeclaredValue::ReadOnly(_) => unreachable!(), + }, + ParameterStorage::Undeclared(param) => { + *param = value; + } + }, + Entry::Vacant(entry) => { + entry.insert(ParameterStorage::Undeclared(value)); + } + } + } } impl MandatoryParameter { @@ -724,6 +669,9 @@ impl<'a> Parameters<'a> { /// * `Ok(())` if setting was successful. /// * [`Err(DeclarationError::TypeMismatch)`] if the type of the requested value is different /// from the parameter's type. + /// * [`Err(DeclarationError::OutOfRange)`] if the requested value is out of the parameter's + /// range. + /// * [`Err(DeclarationError::ReadOnly)`] if the parameter is read only. pub fn set( &self, name: impl Into>, @@ -762,7 +710,6 @@ impl<'a> Parameters<'a> { entry.insert(ParameterStorage::Undeclared(value.into())); } } - Ok(()) } } @@ -770,9 +717,7 @@ impl<'a> Parameters<'a> { 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. + services: Mutex>, } impl ParameterInterface { @@ -789,7 +734,7 @@ impl ParameterInterface { Ok(ParameterInterface { parameter_map: Default::default(), override_map, - allow_undeclared: Default::default(), + services: Mutex::new(None), }) } @@ -808,6 +753,12 @@ impl ParameterInterface { } } + pub(crate) fn create_services(&self, node: &Node) -> Result<(), RclrsError> { + *self.services.lock().unwrap() = + Some(ParameterService::new(node, self.parameter_map.clone())?); + Ok(()) + } + fn get_declaration_initial_value<'a, T: ParameterVariant + 'a>( &self, name: &str, @@ -880,7 +831,7 @@ impl ParameterInterface { } pub(crate) fn allow_undeclared(&self) { - self.allow_undeclared.store(true, Ordering::Relaxed); + self.parameter_map.lock().unwrap().allow_undeclared = true; } } diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs new file mode 100644 index 000000000..96f66d6e3 --- /dev/null +++ b/rclrs/src/parameter/range.rs @@ -0,0 +1,205 @@ +use crate::{ + vendor::rcl_interfaces::msg::rmw::{FloatingPointRange, IntegerRange}, + DeclarationError, ParameterValue, ParameterVariant, +}; +use rosidl_runtime_rs::{seq, BoundedSequence}; + +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 { + pub(crate) fn to_descriptor_ranges( + &self, + ) -> ( + BoundedSequence, + BoundedSequence, + ) { + let int_range = self + .integer + .as_ref() + .map(|range| { + // Converting step to a positive value is safe because declaring a parameter with a + // negative step is not allowed. + // TODO(luca) explore changing step into a positive value in the generic definition to + // make negative steps a compile error. + if range.is_default() { + Default::default() + } else { + seq![1 # IntegerRange { + from_value: range.lower.unwrap_or(i64::MIN), + to_value: range.upper.unwrap_or(i64::MAX), + step: range.step.unwrap_or(0).try_into().unwrap(), + }] + } + }) + .unwrap_or_default(); + let float_range = self + .float + .as_ref() + .map(|range| { + if range.is_default() { + Default::default() + } else { + seq![1 # FloatingPointRange { + from_value: range.lower.unwrap_or(f64::NEG_INFINITY), + to_value: range.upper.unwrap_or(f64::INFINITY), + step: range.step.unwrap_or(0.0), + }] + } + }) + .unwrap_or_default(); + (int_range, float_range) + } + + pub(crate) fn validate(&self) -> Result<(), DeclarationError> { + if let Some(integer) = &self.integer { + integer.validate()?; + } + if let Some(float) = &self.float { + float.validate()?; + } + Ok(()) + } + + pub(crate) 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 is_default(&self) -> bool { + self.lower.is_none() && self.upper.is_none() && self.step.is_none() + } + + 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 + } +} diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index bcbe24beb..a52e3e555 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -1,89 +1,946 @@ -use std::sync::{Arc, Weak, Mutex}; +use std::{ + collections::BTreeSet, + sync::{Arc, Mutex}, +}; -use crate::vendor::rcl_interfaces::srv::rmw::*; -use crate::vendor::rcl_interfaces::msg::rmw::*; -use rosidl_runtime_rs::{Sequence, seq}; +use crate::vendor::rcl_interfaces::{msg::rmw::*, srv::rmw::*}; +use rosidl_runtime_rs::Sequence; -use crate::{rmw_request_id_t, Node, RclrsError, Service, ServiceBase}; -use crate::rcl_bindings::rcl_node_t; +use super::ParameterMap; +use crate::{ + parameter::{DeclaredValue, ParameterKind, ParameterStorage}, + rmw_request_id_t, Node, RclrsError, Service, +}; +// The variables only exist to keep a strong reference to the services and are technically unused. +// What is used is the Weak that is stored in the node, and is upgraded when spinning. pub struct ParameterService { + #[allow(dead_code)] describe_parameters_service: Arc>, + #[allow(dead_code)] get_parameter_types_service: Arc>, + #[allow(dead_code)] get_parameters_service: Arc>, + #[allow(dead_code)] list_parameters_service: Arc>, + #[allow(dead_code)] set_parameters_service: Arc>, + #[allow(dead_code)] 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![] +fn describe_parameters( + req: DescribeParameters_Request, + map: &ParameterMap, +) -> DescribeParameters_Response { + let descriptors = req + .names + .into_iter() + .map(|name| { + let name = name.to_cstr().to_str().ok()?; + let Some(storage) = map.storage.get(name) else { + return Some(ParameterDescriptor { + name: name.into(), + ..Default::default() + }); + }; + let mut descriptor = match storage { + ParameterStorage::Declared(storage) => { + let (integer_range, floating_point_range) = + storage.options.ranges.to_descriptor_ranges(); + ParameterDescriptor { + name: name.into(), + type_: Default::default(), + description: storage.options.description.clone().into(), + additional_constraints: storage.options.constraints.clone().into(), + dynamic_typing: matches!(storage.kind, ParameterKind::Dynamic), + read_only: matches!(storage.value, DeclaredValue::ReadOnly(_)), + floating_point_range, + integer_range, + } + } + ParameterStorage::Undeclared(_) => ParameterDescriptor { + name: name.into(), + dynamic_typing: true, + ..Default::default() + }, + }; + descriptor.type_ = storage.to_parameter_type(); + Some(descriptor) + }) + .collect::>() + .unwrap_or_default(); + // TODO(luca) error logging if the service call failed + DescribeParameters_Response { descriptors } +} + +fn get_parameter_types( + req: GetParameterTypes_Request, + map: &ParameterMap, +) -> GetParameterTypes_Response { + let types = req + .names + .into_iter() + .map(|name| { + let name = name.to_cstr().to_str().ok()?; + map.storage + .get(name) + .map(|s| s.to_parameter_type()) + .or(Some(ParameterType::PARAMETER_NOT_SET)) + }) + .collect::>() + .unwrap_or_default(); + GetParameterTypes_Response { types } +} + +fn get_parameters(req: GetParameters_Request, map: &ParameterMap) -> GetParameters_Response { + let values = req + .names + .into_iter() + .map(|name| { + let name = name.to_cstr().to_str().ok()?; + let Some(storage) = map.storage.get(name) else { + return Some(ParameterValue::default()); + }; + match storage { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(v) => Some(v.read().unwrap().clone().into()), + DeclaredValue::Optional(v) => Some( + v.read() + .unwrap() + .clone() + .map(|v| v.into()) + .unwrap_or_default(), + ), + DeclaredValue::ReadOnly(v) => Some(v.clone().into()), + }, + ParameterStorage::Undeclared(value) => Some(value.clone().into()), + } + }) + .collect::>() + .unwrap_or_default(); + GetParameters_Response { values } +} + +fn list_parameters(req: ListParameters_Request, map: &ParameterMap) -> ListParameters_Response { + let check_parameter_name_depth = |substring: &[i8]| { + if req.depth == ListParameters_Request::DEPTH_RECURSIVE { + return true; + } + u64::try_from(substring.iter().filter(|c| **c == ('.' as i8)).count()).unwrap() < req.depth + }; + let names: Sequence<_> = map + .storage + .keys() + .filter_map(|name| { + let name: rosidl_runtime_rs::String = name.clone().into(); + if req.prefixes.len() == 0 && check_parameter_name_depth(&name[..]) { + return Some(name); + } + req.prefixes + .iter() + .any(|prefix| { + if name == *prefix { + return true; + } + let mut prefix = prefix.clone(); + prefix.extend(".".chars()); + if name.len() > prefix.len() + && name.starts_with(&prefix) + && check_parameter_name_depth(&name[prefix.len()..]) + { + return true; + } + false + }) + .then_some(name) + }) + .collect(); + let prefixes: BTreeSet = names + .iter() + .filter_map(|name| { + let name = name.to_string(); + if let Some(pos) = name.rfind('.') { + return Some(name[0..pos].into()); + } + None + }) + .collect(); + ListParameters_Response { + result: ListParametersResult { + names, + prefixes: prefixes.into_iter().collect(), + }, + } +} + +fn set_parameters(req: SetParameters_Request, map: &mut ParameterMap) -> SetParameters_Response { + let results = req + .parameters + .into_iter() + .map(|param| { + let Ok(name) = param.name.to_cstr().to_str() else { + return SetParametersResult { + successful: false, + reason: "Failed parsing into UTF-8".into(), + }; + }; + match map.validate_parameter_setting(name, param.value) { + Ok(value) => { + map.store_parameter(name.into(), value); + SetParametersResult { + successful: true, + reason: Default::default(), + } } + Err(e) => SetParametersResult { + successful: false, + reason: e.into(), + }, + } + }) + .collect(); + SetParameters_Response { results } +} + +fn set_parameters_atomically( + req: SetParametersAtomically_Request, + map: &mut ParameterMap, +) -> SetParametersAtomically_Response { + let results = req + .parameters + .into_iter() + .map(|param| { + let Ok(name) = param.name.to_cstr().to_str() else { + return Err("Failed parsing into UTF-8".into()); + }; + let value = map.validate_parameter_setting(name, param.value)?; + Ok((name.into(), value)) + }) + .collect::, _>>(); + // Check if there was any error and update parameters accordingly + let result = match results { + Ok(results) => { + for (name, value) in results.into_iter() { + map.store_parameter(name, value); + } + SetParametersResult { + successful: true, + reason: Default::default(), + } + } + Err(reason) => SetParametersResult { + successful: false, + reason, + }, + }; + SetParametersAtomically_Response { result } +} + +impl ParameterService { + pub(crate) fn new( + node: &Node, + parameter_map: Arc>, + ) -> Result { + let fqn = node.fully_qualified_name(); + // TODO(luca) make sure it is OK to have an Arc instead of a Weak here and cleanup on + // destruction is made for the parameter map. + let map = parameter_map.clone(); + let describe_parameters_service = node.create_service( + &(fqn.clone() + "/describe_parameters"), + move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + let map = map.lock().unwrap(); + describe_parameters(req, &map) }, )?; - 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 map = parameter_map.clone(); + let get_parameter_types_service = node.create_service( + &(fqn.clone() + "/get_parameter_types"), + move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + let map = map.lock().unwrap(); + get_parameter_types(req, &map) }, )?; - 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 map = parameter_map.clone(); + let get_parameters_service = node.create_service( + &(fqn.clone() + "/get_parameters"), + move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { + let map = map.lock().unwrap(); + get_parameters(req, &map) }, )?; - 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 map = parameter_map.clone(); + let list_parameters_service = node.create_service( + &(fqn.clone() + "/list_parameters"), + move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { + let map = map.lock().unwrap(); + list_parameters(req, &map) }, )?; - 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 map = parameter_map.clone(); + let set_parameters_service = node.create_service( + &(fqn.clone() + "/set_parameters"), + move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { + let mut map = map.lock().unwrap(); + set_parameters(req, &mut map) }, )?; - 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() - } + let set_parameters_atomically_service = node.create_service( + &(fqn.clone() + "/set_parameters_atomically"), + move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + let mut map = parameter_map.lock().unwrap(); + set_parameters_atomically(req, &mut map) }, )?; 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), + describe_parameters_service, + get_parameter_types_service, + get_parameters_service, + list_parameters_service, + set_parameters_service, + set_parameters_atomically_service, + }) + } +} + +#[cfg(test)] +mod tests { + use crate::{ + vendor::rcl_interfaces::{ + msg::rmw::{ + Parameter as RmwParameter, ParameterType, ParameterValue as RmwParameterValue, + }, + srv::rmw::*, + }, + Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, + ReadOnlyParameter, + }; + use rosidl_runtime_rs::{seq, Sequence}; + use std::sync::{Arc, RwLock}; + + struct TestNode { + node: Arc, + bool_param: MandatoryParameter, + _ns_param: MandatoryParameter, + _read_only_param: ReadOnlyParameter, + dynamic_param: MandatoryParameter, + } + + async fn try_until_timeout(f: F) -> Result<(), ()> + where + F: FnOnce() -> bool + Copy, + { + let mut retry_count = 0; + while !f() { + if retry_count > 50 { + return Err(()); + } + tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; + retry_count += 1; + } + Ok(()) + } + + fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { + let node = NodeBuilder::new(context, "node") + .namespace(ns) + .build() + .unwrap(); + let range = ParameterRange { + lower: Some(0), + upper: Some(100), + step: None, + }; + let bool_param = node + .declare_parameter("bool") + .default(true) + .description("A boolean value") + .mandatory() + .unwrap(); + let _ns_param = node + .declare_parameter("ns1.ns2.ns3.int") + .default(42) + .range(range) + .constraints("Only the answer") + .mandatory() + .unwrap(); + let _read_only_param = node + .declare_parameter("read_only") + .default(1.23) + .read_only() + .unwrap(); + let dynamic_param = node + .declare_parameter("dynamic") + .default(ParameterValue::String("hello".into())) + .mandatory() + .unwrap(); + + let client = NodeBuilder::new(context, "client") + .namespace(ns) + .build() + .unwrap(); + + ( + TestNode { + node, + bool_param, + _ns_param, + _read_only_param, + dynamic_param, + }, + client, + ) + } + + #[test] + fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { + let context = Context::new([]).unwrap(); + let (node, _client) = construct_test_nodes(&context, "names_types"); + + std::thread::sleep(std::time::Duration::from_millis(100)); + + let names_and_types = node.node.get_service_names_and_types()?; + let types = names_and_types + .get("/names_types/node/describe_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters_atomically") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string())); + let types = names_and_types + .get("/names_types/node/list_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameter_types") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); + Ok(()) + } + + #[tokio::test] + async fn test_list_parameters_service() -> Result<(), RclrsError> { + let context = Context::new([]).unwrap(); + let (node, client) = construct_test_nodes(&context, "list"); + let list_client = client.create_client::("/list/node/list_parameters")?; + + try_until_timeout(|| list_client.service_is_ready().unwrap()) + .await + .unwrap(); + + let done = Arc::new(RwLock::new(false)); + + let inner_done = done.clone(); + let rclrs_spin = tokio::task::spawn(async move { + try_until_timeout(|| { + crate::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + *inner_done.read().unwrap() + }) + .await + .unwrap(); + }); + + let res = tokio::task::spawn(async move { + // List all parameters + let request = ListParameters_Request { + prefixes: seq![], + depth: 0, + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + list_client + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + // use_sim_time + all the manually defined ones + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 5); + // Parameter names are returned in alphabetical order + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "dynamic"); + assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(names[3].to_string(), "read_only"); + assert_eq!(names[4].to_string(), "use_sim_time"); + // Only one prefix + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Limit depth, namespaced parameter is not returned + let request = ListParameters_Request { + prefixes: seq![], + depth: 1, + }; + let call_done = client_finished.clone(); + *call_done.write().unwrap() = false; + list_client + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 4); + assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); + assert_eq!(response.result.prefixes.len(), 0); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Filter by prefix, just return the requested one with the right prefix + let request = ListParameters_Request { + prefixes: seq!["ns1.ns2".into()], + depth: 0, + }; + let call_done = client_finished.clone(); + *call_done.write().unwrap() = false; + list_client + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 1); + assert_eq!(names[0].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // If prefix is equal to names, parameters should be returned + let request = ListParameters_Request { + prefixes: seq!["use_sim_time".into(), "bool".into()], + depth: 0, + }; + let call_done = client_finished.clone(); + *call_done.write().unwrap() = false; + list_client + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + dbg!(&names); + assert_eq!(names.len(), 2); + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "use_sim_time"); + assert_eq!(response.result.prefixes.len(), 0); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + *done.write().unwrap() = true; + }); + + res.await.unwrap(); + rclrs_spin.await.unwrap(); + + Ok(()) + } + + #[tokio::test] + async fn test_get_set_parameters_service() -> Result<(), RclrsError> { + let context = Context::new([]).unwrap(); + let (node, client) = construct_test_nodes(&context, "get_set"); + let get_client = client.create_client::("/get_set/node/get_parameters")?; + let set_client = client.create_client::("/get_set/node/set_parameters")?; + let set_atomically_client = client + .create_client::("/get_set/node/set_parameters_atomically")?; + + try_until_timeout(|| { + get_client.service_is_ready().unwrap() + && set_client.service_is_ready().unwrap() + && set_atomically_client.service_is_ready().unwrap() }) + .await + .unwrap(); + + let done = Arc::new(RwLock::new(false)); + + let inner_node = node.node.clone(); + let inner_done = done.clone(); + let rclrs_spin = tokio::task::spawn(async move { + try_until_timeout(|| { + crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); + crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + *inner_done.read().unwrap() + }) + .await + .unwrap(); + }); + + let res = tokio::task::spawn(async move { + // Get an existing parameter + let request = GetParameters_Request { + names: seq!["bool".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_client + .async_send_request_with_callback( + &request, + move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 1); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Getting both existing and non existing parameters, missing one should return + // PARAMETER_NOT_SET + let request = GetParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_client + .async_send_request_with_callback( + &request, + move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 2); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + assert_eq!(response.values[1].type_, ParameterType::PARAMETER_NOT_SET); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Set a mix of existing, non existing, dynamic and out of range parameters + let bool_parameter = RmwParameter { + name: "bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: false, + ..Default::default() + }, + }; + let bool_parameter_mismatched = RmwParameter { + name: "bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: 42, + ..Default::default() + }, + }; + let read_only_parameter = RmwParameter { + name: "read_only".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE, + double_value: 3.45, + ..Default::default() + }, + }; + let dynamic_parameter = RmwParameter { + name: "dynamic".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: true, + ..Default::default() + }, + }; + let out_of_range_parameter = RmwParameter { + name: "ns1.ns2.ns3.int".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: 1000, + ..Default::default() + }, + }; + let invalid_parameter_type = RmwParameter { + name: "dynamic".into(), + value: RmwParameterValue { + type_: 200, + integer_value: 1000, + ..Default::default() + }, + }; + let undeclared_bool = RmwParameter { + name: "undeclared_bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: true, + ..Default::default() + }, + }; + let request = SetParameters_Request { + parameters: seq![ + bool_parameter.clone(), + read_only_parameter.clone(), + bool_parameter_mismatched, + dynamic_parameter, + out_of_range_parameter, + invalid_parameter_type, + undeclared_bool.clone() + ], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + // Parameter is assigned a default of true at declaration time + assert!(node.bool_param.get()); + set_client + .async_send_request_with_callback( + &request, + move |response: SetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.results.len(), 7); + // Setting a bool value set for a bool parameter + assert!(response.results[0].successful); + // Value was set to false, node parameter get should reflect this + assert!(!node.bool_param.get()); + // Setting a parameter to the wrong type + assert!(!response.results[1].successful); + // Setting a read only parameter + assert!(!response.results[2].successful); + // Setting a dynamic parameter to a new type + assert!(response.results[3].successful); + assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); + // Setting a value out of range + assert!(!response.results[4].successful); + // Setting an invalid type + assert!(!response.results[5].successful); + // Setting an undeclared parameter, without allowing undeclared parameters + assert!(!response.results[6].successful); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Set the node to use undeclared parameters and try to set one + node.node.use_undeclared_parameters(); + let request = SetParameters_Request { + parameters: seq![undeclared_bool], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + set_client + .async_send_request_with_callback( + &request, + move |response: SetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.results.len(), 1); + // Setting the undeclared parameter is now allowed + assert!(response.results[0].successful); + assert_eq!( + node.node.use_undeclared_parameters().get("undeclared_bool"), + Some(ParameterValue::Bool(true)) + ); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // With set_parameters_atomically, if one fails all should fail + let request = SetParametersAtomically_Request { + parameters: seq![bool_parameter, read_only_parameter], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + set_atomically_client + .async_send_request_with_callback( + &request, + move |response: SetParametersAtomically_Response| { + *call_done.write().unwrap() = true; + assert!(!response.result.successful); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + *done.write().unwrap() = true; + }); + + res.await.unwrap(); + rclrs_spin.await.unwrap(); + + Ok(()) } - 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, - ] + #[tokio::test] + async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { + let context = Context::new([]).unwrap(); + let (node, client) = construct_test_nodes(&context, "describe"); + let describe_client = + client.create_client::("/describe/node/describe_parameters")?; + let get_types_client = + client.create_client::("/describe/node/get_parameter_types")?; + + try_until_timeout(|| { + describe_client.service_is_ready().unwrap() + && get_types_client.service_is_ready().unwrap() + }) + .await + .unwrap(); + + let done = Arc::new(RwLock::new(false)); + + let inner_done = done.clone(); + let inner_node = node.node.clone(); + let rclrs_spin = tokio::task::spawn(async move { + try_until_timeout(|| { + crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); + crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + *inner_done.read().unwrap() + }) + .await + .unwrap(); + }); + + let res = tokio::task::spawn(async move { + // Describe all parameters + let request = DescribeParameters_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into() + ], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + describe_client + .async_send_request_with_callback( + &request, + move |response: DescribeParameters_Response| { + *call_done.write().unwrap() = true; + let desc = response.descriptors; + assert_eq!(desc.len(), 4); + // Descriptors are returned in the requested order + assert_eq!(desc[0].name.to_string(), "bool"); + assert_eq!(desc[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(desc[0].description.to_string(), "A boolean value"); + assert!(!desc[0].read_only); + assert!(!desc[0].dynamic_typing); + assert_eq!(desc[1].name.to_string(), "ns1.ns2.ns3.int"); + assert_eq!(desc[1].type_, ParameterType::PARAMETER_INTEGER); + assert_eq!(desc[1].integer_range.len(), 1); + assert_eq!(desc[1].integer_range[0].from_value, 0); + assert_eq!(desc[1].integer_range[0].to_value, 100); + assert_eq!(desc[1].integer_range[0].step, 0); + assert!(!desc[1].read_only); + assert!(!desc[1].dynamic_typing); + assert_eq!( + desc[1].additional_constraints.to_string(), + "Only the answer" + ); + assert_eq!(desc[2].name.to_string(), "read_only"); + assert_eq!(desc[2].type_, ParameterType::PARAMETER_DOUBLE); + assert!(desc[2].read_only); + assert!(!desc[2].dynamic_typing); + assert_eq!(desc[3].name.to_string(), "dynamic"); + assert_eq!(desc[3].type_, ParameterType::PARAMETER_STRING); + assert!(desc[3].dynamic_typing); + assert!(!desc[3].read_only); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // If a describe parameters request is sent with a non existing parameter, an empty + // response should be returned + let request = DescribeParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + describe_client + .async_send_request_with_callback( + &request, + move |response: DescribeParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.descriptors[0].name.to_string(), "bool"); + assert_eq!(response.descriptors[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(response.descriptors.len(), 2); + assert_eq!(response.descriptors[1].name.to_string(), "non_existing"); + assert_eq!( + response.descriptors[1].type_, + ParameterType::PARAMETER_NOT_SET + ); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Get all parameter types, including a non existing one that will be NOT_SET + let request = GetParameterTypes_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into(), + "non_existing".into() + ], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_types_client + .async_send_request_with_callback( + &request, + move |response: GetParameterTypes_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.types.len(), 5); + // Types are returned in the requested order + assert_eq!(response.types[0], ParameterType::PARAMETER_BOOL); + assert_eq!(response.types[1], ParameterType::PARAMETER_INTEGER); + assert_eq!(response.types[2], ParameterType::PARAMETER_DOUBLE); + assert_eq!(response.types[3], ParameterType::PARAMETER_STRING); + assert_eq!(response.types[4], ParameterType::PARAMETER_NOT_SET); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + *done.write().unwrap() = true; + }); + + res.await.unwrap(); + rclrs_spin.await.unwrap(); + + Ok(()) } } diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 08a39d4f5..9d431c03e 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -1,6 +1,11 @@ use std::{ffi::CStr, sync::Arc}; -use crate::{rcl_bindings::*, ParameterRange, ParameterRanges, ParameterValueError}; +use crate::{ + parameter::{ParameterRange, ParameterRanges}, + rcl_bindings::*, + vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}, + ParameterValueError, +}; /// A parameter value. /// @@ -317,6 +322,103 @@ impl ParameterVariant for ParameterValue { } } +impl From for RmwParameterValue { + fn from(value: ParameterValue) -> Self { + match value { + ParameterValue::Bool(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: v, + ..Default::default() + }, + ParameterValue::Integer(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: v, + ..Default::default() + }, + ParameterValue::Double(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE, + double_value: v, + ..Default::default() + }, + ParameterValue::String(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_STRING, + string_value: v.into(), + ..Default::default() + }, + ParameterValue::ByteArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_BYTE_ARRAY, + byte_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::BoolArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL_ARRAY, + bool_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::IntegerArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::DoubleArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::StringArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_STRING_ARRAY, + string_array_value: v.iter().map(|v| v.clone().into()).collect(), + ..Default::default() + }, + } + } +} + +/// An error that occured when trying to convert a parameter from an +/// `rcl_interfaces::msg::ParameterValue` +pub enum RmwParameterConversionError { + /// The parameter type was not valid. + InvalidParameterType, +} + +impl TryFrom for ParameterValue { + type Error = RmwParameterConversionError; + + fn try_from(param: RmwParameterValue) -> Result { + // TODO(luca) how to deal with PARAMETER_NOT_SET? Should we allow service calls to unset + // parameters? + match param.type_ { + ParameterType::PARAMETER_BOOL => Ok(ParameterValue::Bool(param.bool_value)), + ParameterType::PARAMETER_INTEGER => Ok(ParameterValue::Integer(param.integer_value)), + ParameterType::PARAMETER_DOUBLE => Ok(ParameterValue::Double(param.double_value)), + ParameterType::PARAMETER_STRING => Ok(ParameterValue::String( + param.string_value.to_string().into(), + )), + ParameterType::PARAMETER_BYTE_ARRAY => { + Ok(ParameterValue::ByteArray((*param.byte_array_value).into())) + } + ParameterType::PARAMETER_BOOL_ARRAY => { + Ok(ParameterValue::BoolArray((*param.bool_array_value).into())) + } + ParameterType::PARAMETER_INTEGER_ARRAY => Ok(ParameterValue::IntegerArray( + (*param.integer_array_value).into(), + )), + ParameterType::PARAMETER_DOUBLE_ARRAY => Ok(ParameterValue::DoubleArray( + (*param.double_array_value).into(), + )), + ParameterType::PARAMETER_STRING_ARRAY => Ok(ParameterValue::StringArray( + param + .string_array_value + .iter() + .map(|s| s.to_string().into()) + .collect::>() + .into(), + )), + _ => Err(RmwParameterConversionError::InvalidParameterType), + } + } +} + impl ParameterValue { // Panics if the rcl_variant_t does not have exactly one field set. // @@ -390,6 +492,35 @@ impl ParameterValue { unreachable!() } } + + pub(crate) fn rcl_parameter_type(&self) -> u8 { + match self { + ParameterValue::Bool(_) => ParameterType::PARAMETER_BOOL, + ParameterValue::Integer(_) => ParameterType::PARAMETER_INTEGER, + ParameterValue::Double(_) => ParameterType::PARAMETER_DOUBLE, + ParameterValue::String(_) => ParameterType::PARAMETER_STRING, + ParameterValue::ByteArray(_) => ParameterType::PARAMETER_BYTE_ARRAY, + ParameterValue::BoolArray(_) => ParameterType::PARAMETER_BOOL_ARRAY, + ParameterValue::IntegerArray(_) => ParameterType::PARAMETER_INTEGER_ARRAY, + ParameterValue::DoubleArray(_) => ParameterType::PARAMETER_DOUBLE_ARRAY, + ParameterValue::StringArray(_) => ParameterType::PARAMETER_STRING_ARRAY, + } + } + + /// Returns the `ParameterKind` for the parameter. + pub(crate) fn kind(&self) -> ParameterKind { + match self { + ParameterValue::Bool(_) => ParameterKind::Bool, + ParameterValue::Integer(_) => ParameterKind::Integer, + ParameterValue::Double(_) => ParameterKind::Double, + ParameterValue::String(_) => ParameterKind::String, + ParameterValue::ByteArray(_) => ParameterKind::ByteArray, + ParameterValue::BoolArray(_) => ParameterKind::BoolArray, + ParameterValue::IntegerArray(_) => ParameterKind::IntegerArray, + ParameterValue::DoubleArray(_) => ParameterKind::DoubleArray, + ParameterValue::StringArray(_) => ParameterKind::StringArray, + } + } } #[cfg(test)] diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 143adf09c..a6b600800 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,7 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - MandatoryParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK, + Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -16,7 +16,9 @@ pub(crate) struct TimeSource { clock_qos: QoSProfile, clock_subscription: Mutex>>>, last_received_time: Arc>>, - use_sim_time: Mutex>>, + // TODO(luca) Make this parameter editable when we have parameter callbacks implemented and can + // safely change clock type at runtime + use_sim_time: Mutex>>, } /// A builder for creating a [`TimeSource`][1]. @@ -84,12 +86,12 @@ impl TimeSource { /// 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. + // TODO(luca) Make this parameter editable and 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() + .read_only() .unwrap(); *self.node.lock().unwrap() = Arc::downgrade(node); self.set_ros_time_enable(param.get()); diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index 74286b034..9f0251cda 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -1,4 +1,5 @@ use std::{ + borrow::Borrow, cmp::Ordering, ffi::CStr, fmt::{self, Debug, Display}, @@ -302,13 +303,17 @@ string_impl!( rosidl_runtime_c__U16String__Sequence__copy ); -impl From<&str> for String { - fn from(s: &str) -> Self { +impl From for String +where + T: Borrow, +{ + fn from(s: T) -> Self { let mut msg = Self { data: std::ptr::null_mut(), size: 0, capacity: 0, }; + let s = s.borrow(); // SAFETY: It's okay to pass a non-zero-terminated string here since assignn uses the // specified length and will append the 0 byte to the dest string itself. if !unsafe {