From 48423f03425dc1681ab404e60f18db356e602e81 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Thu, 21 Sep 2023 23:57:11 +0200 Subject: [PATCH 1/3] Minimal implementation of GetParameterTypes and DescribeParameters services With this, the parameters are shown in Rqt or Foxglove Studio. --- r2r/src/nodes.rs | 70 +++++++++++++++++++++++++++++++++++++++++++ r2r/src/parameters.rs | 15 ++++++++++ 2 files changed, 85 insertions(+) diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs index 0f889ec9d..6fd7e12f2 100644 --- a/r2r/src/nodes.rs +++ b/r2r/src/nodes.rs @@ -354,6 +354,48 @@ 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(pv) => pv.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)) } @@ -401,6 +443,34 @@ 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 { + if let Some(pv) = params.get(name) { + descriptors.push(ParameterDescriptor { + name: name.clone(), + type_: pv.into_parameter_type(), + ..Default::default() + }); + } else { + // parameter not found, but undeclared allowed, so return empty + descriptors.push(ParameterDescriptor { + name: name.clone(), + ..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..465cf243b 100644 --- a/r2r/src/parameters.rs +++ b/r2r/src/parameters.rs @@ -143,6 +143,21 @@ 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 + } + } } /// Trait for use it with From 437bd41d15f7691798ac49368f24ab4d747f1143 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Tue, 26 Sep 2023 18:00:38 +0200 Subject: [PATCH 2/3] r2r_macros: Remove TODO The code is already merged, so renaming makes little sense. --- r2r_macros/src/lib.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/r2r_macros/src/lib.rs b/r2r_macros/src/lib.rs index 56b76e4de..1d18aba58 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)] From 86d16054a79e099a9c78b533236cde1734c3fb52 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Tue, 26 Sep 2023 17:36:05 +0200 Subject: [PATCH 3/3] Derive ROS parameter description from field doc comments With this change, adding doc comments to fields of structures used with `#[derive(RosParams)]` results in those comments being used as parameter description. See r2r/examples/parameters_derive.rs for how to use and test this feature. *BREAKING CHANGE* This commit changes r2r public API. Previously Node::params contained HashMap, now it contains HashMap. If you previously used the ParameterValue from this HashMap, now you can get the same by using the .value field of the Parameter structure. --- r2r/examples/parameters.rs | 2 +- r2r/examples/parameters_derive.rs | 14 +++++++ r2r/src/lib.rs | 2 +- r2r/src/nodes.rs | 69 +++++++++++++++++-------------- r2r/src/parameters.rs | 39 +++++++++++++---- r2r_macros/src/lib.rs | 28 ++++++++++++- 6 files changed, 112 insertions(+), 42 deletions(-) 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 6fd7e12f2..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::>(); @@ -384,7 +396,7 @@ impl Node { .names .iter() .map(|name| match params.get(name) { - Some(pv) => pv.into_parameter_type(), + Some(param) => param.value.into_parameter_type(), None => rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET as u8, }) .collect(); @@ -402,7 +414,7 @@ impl Node { fn handle_list_parameters( req: ServiceRequest, - params: &Arc>>, + params: &Arc>>, ) -> future::Ready<()> { use rcl_interfaces::srv::ListParameters; @@ -445,26 +457,21 @@ impl Node { fn handle_desc_parameters( req: ServiceRequest, - params: &Arc>>, + 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 { - if let Some(pv) = params.get(name) { - descriptors.push(ParameterDescriptor { - name: name.clone(), - type_: pv.into_parameter_type(), - ..Default::default() - }); - } else { - // parameter not found, but undeclared allowed, so return empty - descriptors.push(ParameterDescriptor { - name: name.clone(), - ..Default::default() - }); - } + 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"); diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index 465cf243b..d454e2825 100644 --- a/r2r/src/parameters.rs +++ b/r2r/src/parameters.rs @@ -160,6 +160,29 @@ impl ParameterValue { } } +/// 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 /// [`Node::make_derived_parameter_handler()`](crate::Node::make_derived_parameter_handler()). /// @@ -167,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<()>; @@ -178,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 1d18aba58..2e511d1ff 100644 --- a/r2r_macros/src/lib.rs +++ b/r2r_macros/src/lib.rs @@ -26,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("") @@ -79,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! { @@ -94,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 {