Support ROS parameter descriptions

r2r/examples/parameters.rs

@@ -47,7 +47,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         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");
         }

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,
 }
 

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};

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<Mutex<HashMap<String, ParameterValue>>>,
+    /// ROS parameters.
+    pub params: Arc<Mutex<HashMap<String, Parameter>>>,
     node_handle: Box<rcl_node_t>,
     // the node owns the subscribers
     subscribers: Vec<Box<dyn Subscriber_>>,
@@ -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<std::pin::Pin<Box<dyn Future<Output = ()> + 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) = &params_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) = &params_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::<Vec<rcl_interfaces::msg::ParameterValue>>();
@@ -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::<DescribeParameters::Service>(
+            &format!("{node_name}/describe_parameters"),
+        )?;
+
+        let params = self.params.clone();
+        let desc_params_future = desc_params_request_stream.for_each(
+            move |req: ServiceRequest<DescribeParameters::Service>| {
+                Self::handle_desc_parameters(req, &params)
+            },
+        );
+
+        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::<GetParameterTypes::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<GetParameterTypes::Service>| {
+                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<rcl_interfaces::srv::ListParameters::Service>,
-        params: &Arc<Mutex<HashMap<String, ParameterValue>>>,
+        params: &Arc<Mutex<HashMap<String, Parameter>>>,
     ) -> future::Ready<()> {
         use rcl_interfaces::srv::ListParameters;
 
@@ -401,6 +455,29 @@ impl Node {
         future::ready(())
     }
 
+    fn handle_desc_parameters(
+        req: ServiceRequest<rcl_interfaces::srv::DescribeParameters::Service>,
+        params: &Arc<Mutex<HashMap<String, Parameter>>>,
+    ) -> future::Ready<()> {
+        use rcl_interfaces::msg::ParameterDescriptor;
+        use rcl_interfaces::srv::DescribeParameters;
+        let mut descriptors = Vec::<ParameterDescriptor>::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.

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<String, ParameterValue>,
+        &mut self, prefix: &str, param: Option<Parameter>, params: &mut HashMap<String, Parameter>,
     ) -> Result<()>;
     fn get_parameter(&mut self, param_name: &str) -> Result<ParameterValue>;
     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<String, ParameterValue>,
+                &mut self, prefix: &str, param: Option<Parameter>,
+                params: &mut HashMap<String, Parameter>,
             ) -> 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(())
             }
 

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<String, ::r2r::ParameterValue>,
+                desc: ::std::option::Option<::r2r::Parameter>,
+                params: &mut ::std::collections::hash_map::HashMap<String, ::r2r::Parameter>,
             ) -> ::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 {