diff --git a/r2r/examples/parameters.rs b/r2r/examples/parameters.rs index d40487fef..f4aab460a 100644 --- a/r2r/examples/parameters.rs +++ b/r2r/examples/parameters.rs @@ -47,7 +47,7 @@ fn main() -> Result<(), Box> { loop { println!("node parameters"); params.lock().unwrap().iter().for_each(|(k, v)| { - println!("{} - {:?}", k, v); + println!("{} - {:?}", k, v.value); }); let _elapsed = timer.tick().await.expect("could not tick"); } diff --git a/r2r/examples/parameters_derive.rs b/r2r/examples/parameters_derive.rs index 299b510a0..30c965bce 100644 --- a/r2r/examples/parameters_derive.rs +++ b/r2r/examples/parameters_derive.rs @@ -23,6 +23,17 @@ use std::sync::{Arc, Mutex}; // par1: 5.1 // par2: 0 // +// ros2 param describe /demo/my_node par1 nested.nested2.par5 +// Prints: +// Parameter name: par1 +// Type: double +// Description: Parameter description +// Constraints: +// Parameter name: nested.nested2.par5 +// Type: integer +// Description: Small parameter +// Constraints: + // Error handling: // cargo run --example parameters_derive -- --ros-args -p nested.par4:=xxx @@ -31,7 +42,9 @@ use std::sync::{Arc, Mutex}; #[derive(RosParams, Default, Debug)] struct Params { + /// Parameter description par1: f64, + /// Dummy parameter [m/s] par2: i32, nested: NestedParams, } @@ -45,6 +58,7 @@ struct NestedParams { #[derive(RosParams, Default, Debug)] struct NestedParams2 { + /// Small parameter par5: i8, } diff --git a/r2r/src/lib.rs b/r2r/src/lib.rs index e2e1211a6..f9e128ab6 100644 --- a/r2r/src/lib.rs +++ b/r2r/src/lib.rs @@ -112,7 +112,7 @@ mod context; pub use context::Context; mod parameters; -pub use parameters::{ParameterValue, RosParams}; +pub use parameters::{Parameter, ParameterValue, RosParams}; mod clocks; pub use clocks::{Clock, ClockType}; diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs index 0f889ec9d..5f3e8f861 100644 --- a/r2r/src/nodes.rs +++ b/r2r/src/nodes.rs @@ -35,8 +35,8 @@ use crate::subscribers::*; /// be called continously. pub struct Node { context: Context, - /// ROS parameter values. - pub params: Arc>>, + /// ROS parameters. + pub params: Arc>>, node_handle: Box, // the node owns the subscribers subscribers: Vec>, @@ -146,7 +146,7 @@ impl Node { let s = unsafe { CStr::from_ptr(*s) }; let key = s.to_str().unwrap_or(""); let val = ParameterValue::from_rcl(v); - params.insert(key.to_owned(), val); + params.insert(key.to_owned(), Parameter::new(val)); } } @@ -243,7 +243,7 @@ impl Node { // register all parameters ps.lock() .unwrap() - .register_parameters("", &mut self.params.lock().unwrap())?; + .register_parameters("", None, &mut self.params.lock().unwrap())?; } let mut handlers: Vec + Send>>> = Vec::new(); let (mut event_tx, event_rx) = mpsc::channel::<(String, ParameterValue)>(10); @@ -266,19 +266,31 @@ impl Node { .lock() .unwrap() .get(&p.name) - .map(|v| v != &val) + .map(|v| v.value != val) .unwrap_or(true); // changed=true if new let r = if let Some(ps) = ¶ms_struct_clone { + // Update parameter structure let result = ps.lock().unwrap().set_parameter(&p.name, &val); if result.is_ok() { - params.lock().unwrap().insert(p.name.clone(), val.clone()); + // Also update Node::params + params + .lock() + .unwrap() + .entry(p.name.clone()) + .and_modify(|p| p.value = val.clone()); } rcl_interfaces::msg::SetParametersResult { successful: result.is_ok(), reason: result.err().map_or("".into(), |e| e.to_string()), } } else { - params.lock().unwrap().insert(p.name.clone(), val.clone()); + // No parameter structure - update only Node::params + params + .lock() + .unwrap() + .entry(p.name.clone()) + .and_modify(|p| p.value = val.clone()) + .or_insert(Parameter::new(val.clone())); rcl_interfaces::msg::SetParametersResult { successful: true, reason: "".into(), @@ -316,17 +328,17 @@ impl Node { .names .iter() .map(|n| { + // First try to get the parameter from the param structure if let Some(ps) = ¶ms_struct_clone { - ps.lock() - .unwrap() - .get_parameter(&n) - .unwrap_or(ParameterValue::NotSet) - } else { - match params.get(n) { - Some(v) => v.clone(), - None => ParameterValue::NotSet, + if let Ok(value) = ps.lock().unwrap().get_parameter(&n) { + return value; } } + // Otherwise get it from node HashMap + match params.get(n) { + Some(v) => v.value.clone(), + None => ParameterValue::NotSet, + } }) .map(|v| v.into_parameter_value_msg()) .collect::>(); @@ -354,13 +366,55 @@ impl Node { handlers.push(Box::pin(list_params_future)); + // rcl_interfaces/srv/DescribeParameters + use rcl_interfaces::srv::DescribeParameters; + let desc_params_request_stream = self.create_service::( + &format!("{node_name}/describe_parameters"), + )?; + + let params = self.params.clone(); + let desc_params_future = desc_params_request_stream.for_each( + move |req: ServiceRequest| { + Self::handle_desc_parameters(req, ¶ms) + }, + ); + + handlers.push(Box::pin(desc_params_future)); + + // rcl_interfaces/srv/GetParameterTypes + use rcl_interfaces::srv::GetParameterTypes; + let get_param_types_request_stream = self.create_service::( + &format!("{node_name}/get_parameter_types"), + )?; + + let params = self.params.clone(); + let get_param_types_future = get_param_types_request_stream.for_each( + move |req: ServiceRequest| { + let params = params.lock().unwrap(); + let types = req + .message + .names + .iter() + .map(|name| match params.get(name) { + Some(param) => param.value.into_parameter_type(), + None => rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET as u8, + }) + .collect(); + req.respond(GetParameterTypes::Response { types }) + .expect("could not send reply to get parameter types request"); + future::ready(()) + }, + ); + + handlers.push(Box::pin(get_param_types_future)); + // we don't care about the result, the futures will not complete anyway. Ok((join_all(handlers).map(|_| ()), event_rx)) } fn handle_list_parameters( req: ServiceRequest, - params: &Arc>>, + params: &Arc>>, ) -> future::Ready<()> { use rcl_interfaces::srv::ListParameters; @@ -401,6 +455,29 @@ impl Node { future::ready(()) } + fn handle_desc_parameters( + req: ServiceRequest, + params: &Arc>>, + ) -> future::Ready<()> { + use rcl_interfaces::msg::ParameterDescriptor; + use rcl_interfaces::srv::DescribeParameters; + let mut descriptors = Vec::::new(); + let params = params.lock().unwrap(); + for name in &req.message.names { + let default = Parameter::empty(); + let param = params.get(name).unwrap_or(&default); + descriptors.push(ParameterDescriptor { + name: name.clone(), + type_: param.value.into_parameter_type(), + description: param.description.to_string(), + ..Default::default() + }); + } + req.respond(DescribeParameters::Response { descriptors }) + .expect("could not send reply to describe parameters request"); + future::ready(()) + } + /// Subscribe to a ROS topic. /// /// This function returns a `Stream` of ros messages. diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index f452c5e48..d454e2825 100644 --- a/r2r/src/parameters.rs +++ b/r2r/src/parameters.rs @@ -143,6 +143,44 @@ impl ParameterValue { } ret } + + pub(crate) fn into_parameter_type(&self) -> u8 { + match self { + ParameterValue::NotSet => 0, // uint8 PARAMETER_NOT_SET=0 + ParameterValue::Bool(_) => 1, // uint8 PARAMETER_BOOL=1 + ParameterValue::Integer(_) => 2, // uint8 PARAMETER_INTEGER=2 + ParameterValue::Double(_) => 3, // uint8 PARAMETER_DOUBLE=3 + ParameterValue::String(_) => 4, // uint8 PARAMETER_STRING=4 + ParameterValue::ByteArray(_) => 5, // uint8 PARAMETER_BYTE_ARRAY=5 + ParameterValue::BoolArray(_) => 6, // uint8 PARAMETER_BOOL_ARRAY=6 + ParameterValue::IntegerArray(_) => 7, // uint8 PARAMETER_INTEGER_ARRAY=7 + ParameterValue::DoubleArray(_) => 8, // uint8 PARAMETER_DOUBLE_ARRAY=8 + ParameterValue::StringArray(_) => 9, // int PARAMETER_STRING_ARRAY=9 + } + } +} + +/// ROS parameter. +pub struct Parameter { + pub value: ParameterValue, + pub description: &'static str, + // TODO: Add other fields like min, max, step. Use field + // attributes for defining them. +} + +impl Parameter { + pub fn new(value: ParameterValue) -> Self { + Self { + value, + description: "", + } + } + pub fn empty() -> Self { + Self { + value: ParameterValue::NotSet, + description: "", + } + } } /// Trait for use it with @@ -152,7 +190,7 @@ impl ParameterValue { /// `parameters_derive.rs` example. pub trait RosParams { fn register_parameters( - &mut self, prefix: &str, params: &mut HashMap, + &mut self, prefix: &str, param: Option, params: &mut HashMap, ) -> Result<()>; fn get_parameter(&mut self, param_name: &str) -> Result; fn set_parameter(&mut self, param_name: &str, param_val: &ParameterValue) -> Result<()>; @@ -163,16 +201,18 @@ macro_rules! impl_ros_params { ($type:path, $param_value_type:path, $to_param_conv:path, $from_param_conv:path) => { impl RosParams for $type { fn register_parameters( - &mut self, prefix: &str, params: &mut HashMap, + &mut self, prefix: &str, param: Option, + params: &mut HashMap, ) -> Result<()> { - if let Some(param_val) = params.get(prefix) { + if let Some(cli_param) = params.get(prefix) { // Apply parameter value if set from command line or launch file - self.set_parameter("", param_val) + self.set_parameter("", &cli_param.value) .map_err(|e| e.update_param_name(prefix))?; - } else { - // Insert missing parameter with its default value - params.insert(prefix.to_owned(), $param_value_type($to_param_conv(self)?)); } + // Insert (or replace) the parameter with filled-in description etc. + let mut param = param.unwrap(); + param.value = $param_value_type($to_param_conv(self)?); + params.insert(prefix.to_owned(), param); Ok(()) } diff --git a/r2r_macros/src/lib.rs b/r2r_macros/src/lib.rs index 56b76e4de..2e511d1ff 100644 --- a/r2r_macros/src/lib.rs +++ b/r2r_macros/src/lib.rs @@ -5,7 +5,6 @@ use syn::{parse_macro_input, Data, DeriveInput, Fields}; extern crate proc_macro; -// TODO: Should this be called R2RParams? Or R2rParams? /// Derives RosParams trait for a structure to use it with /// `r2r::Node::make_derived_parameter_handler()`. #[proc_macro_derive(RosParams)] @@ -27,7 +26,8 @@ pub fn derive_r2r_params(input: proc_macro::TokenStream) -> proc_macro::TokenStr fn register_parameters( &mut self, prefix: &str, - params: &mut ::std::collections::hash_map::HashMap, + desc: ::std::option::Option<::r2r::Parameter>, + params: &mut ::std::collections::hash_map::HashMap, ) -> ::r2r::Result<()> { let prefix = if prefix.is_empty() { String::from("") @@ -80,9 +80,14 @@ fn get_register_calls(data: &Data) -> TokenStream { let field_matches = fields.named.iter().map(|f| { let name = &f.ident; let format_str = format!("{{prefix}}{}", name.as_ref().unwrap()); + let desc = get_field_doc(f); quote_spanned! { f.span() => - self.#name.register_parameters(&format!(#format_str), params)?; + let param = ::r2r::Parameter { + value: ::r2r::ParameterValue::NotSet, // will be set for leaf params by register_parameters() below + description: #desc, + }; + self.#name.register_parameters(&format!(#format_str), Some(param), params)?; } }); quote! { @@ -95,6 +100,24 @@ fn get_register_calls(data: &Data) -> TokenStream { } } +fn get_field_doc(f: &syn::Field) -> String { + if let Some(doc) = f + .attrs + .iter() + .find(|&attr| attr.path().get_ident().is_some_and(|id| id == "doc")) + { + match &doc.meta.require_name_value().unwrap().value { + ::syn::Expr::Lit(exprlit) => match &exprlit.lit { + ::syn::Lit::Str(s) => s.value().trim().to_owned(), + _ => unimplemented!(), + }, + _ => unimplemented!(), + } + } else { + "".to_string() + } +} + // Generate match arms for RosParams::update_parameters() fn param_matches_for(call: TokenStream, data: &Data) -> TokenStream { match *data {