Implement derive(RosParams) macro and surrounding infrastructure

With this, declaring and handling node parameters becomes easy. One
just needs to define a structure(s) containing the parameters such as:

    #[derive(RosParams, Default, Debug)]
    struct Params {
        par1: f64,
        par2: i32,
        str: String,
    }

And then instantiate and register it with:

    let params = Arc::new(Mutex::new(Params::default()));
    let (paramater_handler, _) = node.make_derived_parameter_handler(params.clone())?;

This will add three parameters `par1`, `par2` and `str` to the node.
Their type will be `Double`, `Integer` and `String` respectively.
Other Rust types such as `f32` or differently sized integers, e.g.
`u16` are also supported and registered as appropriate ROS parameter
types.

After spawning the handler, e.g.:

    spawner.spawn_local(paramater_handler)?;

changing a parameter with external ROS tools (e.g. `ros2 param set`)
will result in changing the appropriate field in the `Params`
structure. Type conversion is handled automatically. For example,
setting an `i8` field (represented as `Integer` ROS parameter) will
succeed if the value is in range -128 to 127 and fail with appropriate
error message for other values.

The other direction also works: Changing a value in the `Params`
structure will be visible outside of the Node via the `get_parameters`
service.

It is also possible to organize the parameters as several nested
structures with parameters. Then, parameter names of different nesting
levels will be separated by `.`. For example `nested.par3`. See the
full example in `parameters_derive.rs`.

Cargo.toml

@@ -5,5 +5,6 @@ members = [
     "r2r_actions",
     "r2r_common",
     "r2r_msg_gen",
+    "r2r_macros",
     "r2r_rcl",
 ]

r2r/Cargo.toml

@@ -31,6 +31,7 @@ phf = "0.11.1"
 serde_json = "1.0.89"
 futures = "0.3.25"
 tokio = { version = "1.22.0", features = ["rt-multi-thread", "macros"] }
+r2r_macros = { path = "../r2r_macros", version = "0.1.0" }
 rand = "0.8.5"
 
 [build-dependencies]

r2r/examples/parameters_derive.rs

@@ -0,0 +1,104 @@
+use futures::executor::LocalPool;
+use futures::prelude::*;
+use futures::task::LocalSpawnExt;
+use r2r_macros::RosParams;
+use std::sync::{Arc, Mutex};
+
+// try to run like this
+// cargo run --example parameters_derive -- --ros-args -p par1:=5.1 -p nested.par4:=42 -r __ns:=/demo -r __node:=my_node
+// then run
+// ros2 param get /demo/my_node nested.par4 # should return 42
+// ros2 param set /demo/my_node nested.par4 43
+// ros2 param set /demo/my_node nested.par4 xxx # fails due to invalid type
+// ros2 param set /demo/my_node nested.nested2.par5 999 # fails with conversion error
+// ros2 param dump /demo/my_node
+// Prints:
+//   /demo/my_node:
+//     ros__parameters:
+//       nested:
+//         nested2:
+//           par5: 0
+//         par3: initial value
+//         par4: 43
+//       par1: 5.1
+//       par2: 0
+//
+// Error handling:
+// cargo run --example parameters_derive -- --ros-args -p nested.par4:=xxx
+
+// Explore how is RosParams derived by running:
+// cargo expand --example=parameters_derive
+
+#[derive(RosParams, Default, Debug)]
+struct Params {
+    par1: f64,
+    par2: i32,
+    nested: NestedParams,
+}
+
+#[derive(RosParams, Default, Debug)]
+struct NestedParams {
+    par3: String,
+    par4: u16,
+    nested2: NestedParams2,
+}
+
+#[derive(RosParams, Default, Debug)]
+struct NestedParams2 {
+    par5: i8,
+}
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    println!("Ros version: {}", r2r::ROS_DISTRO);
+
+    // set up executor
+    let mut pool = LocalPool::new();
+    let spawner = pool.spawner();
+
+    // set up ros node
+    let ctx = r2r::Context::create()?;
+    let mut node = r2r::Node::create(ctx, "to_be_replaced", "to_be_replaced")?;
+
+    // create our parameters and set default values
+    let params = Arc::new(Mutex::new({
+        let mut p = Params::default();
+        p.nested.par3 = "initial value".into();
+        p
+    }));
+
+    // make a parameter handler (once per node).
+    // the parameter handler is optional, only spawn one if you need it.
+    let (paramater_handler, parameter_events) =
+        node.make_derived_parameter_handler(params.clone())?;
+    // run parameter handler on your executor.
+    spawner.spawn_local(paramater_handler)?;
+
+    println!("node name: {}", node.name()?);
+    println!("node fully qualified name: {}", node.fully_qualified_name()?);
+    println!("node namespace: {}", node.namespace()?);
+
+    // parameter event stream. just print them
+    let params_clone = params.clone();
+    spawner.spawn_local(async move {
+        parameter_events
+            .for_each(|_| {
+                println!("event: {:#?}", params_clone.lock().unwrap());
+                future::ready(())
+            })
+            .await
+    })?;
+
+    // print all params every 5 seconds.
+    let mut timer = node.create_wall_timer(std::time::Duration::from_secs(5))?;
+    spawner.spawn_local(async move {
+        loop {
+            println!("timer: {:#?}", params.lock().unwrap());
+            let _elapsed = timer.tick().await.expect("could not tick");
+        }
+    })?;
+
+    loop {
+        node.spin_once(std::time::Duration::from_millis(100));
+        pool.run_until_stalled();
+    }
+}

r2r/src/error.rs

@@ -116,6 +116,15 @@ pub enum Error {
 
     #[error("Goal already in a terminal state.")]
     GoalCancelAlreadyTerminated,
+
+    #[error("Invalid parameter name: {name}")]
+    InvalidParameterName { name: String },
+
+    #[error("Invalid type for parameter {name} (should be {ty})")]
+    InvalidParameterType { name: String, ty: &'static str },
+
+    #[error("Parameter {name} conversion failed: {msg}")]
+    ParameterValueConv { name: String, msg: String },
 }
 
 impl Error {
@@ -170,4 +179,22 @@ impl Error {
             _ => panic!("TODO: add error code {}", e),
         }
     }
+
+    /// Internal function used by code derived for the RosParams trait.
+    pub fn update_param_name(self, param_name: &str) -> Error {
+        match self {
+            Error::InvalidParameterName { name: _ } => Error::InvalidParameterName {
+                name: param_name.to_string(),
+            },
+            Error::InvalidParameterType { name: _, ty } => Error::InvalidParameterType {
+                name: param_name.to_string(),
+                ty,
+            },
+            Error::ParameterValueConv { name: _, msg } => Error::ParameterValueConv {
+                name: param_name.to_string(),
+                msg,
+            },
+            _ => self,
+        }
+    }
 }

r2r/src/lib.rs

@@ -112,7 +112,7 @@ mod context;
 pub use context::Context;
 
 mod parameters;
-pub use parameters::ParameterValue;
+pub use parameters::{ParameterValue, RosParams};
 
 mod clocks;
 pub use clocks::{Clock, ClockType};

r2r/src/nodes.rs

@@ -209,6 +209,42 @@ impl Node {
         &mut self,
     ) -> Result<(impl Future<Output = ()> + Send, impl Stream<Item = (String, ParameterValue)>)>
     {
+        self.make_parameter_handler_internal(None)
+    }
+
+    /// Creates parameter service handlers for the Node based on the
+    /// [`RosParams`] trait.
+    ///
+    /// Supported parameter names and types are given by the
+    /// `params_struct` parameter (usually referring to a structure).
+    /// Fields of the structure will be updated based on the command
+    /// line parameters (if any) and later whenever a parameter gets
+    /// changed from external sources. Updated fields will be visible
+    /// outside of the node via the GetParameters service.
+    ///
+    /// This function returns a tuple (`Future`, `Stream`), where the
+    /// future should be spawned on onto the executor of choice. The
+    /// `Stream` produces events whenever parameters change from
+    /// external sources. The event elements of the event stream
+    /// include the name of the parameter which was updated as well as
+    /// its new value.
+    pub fn make_derived_parameter_handler(
+        &mut self, params_struct: Arc<Mutex<dyn RosParams + Send>>,
+    ) -> Result<(impl Future<Output = ()> + Send, impl Stream<Item = (String, ParameterValue)>)>
+    {
+        self.make_parameter_handler_internal(Some(params_struct))
+    }
+
+    fn make_parameter_handler_internal(
+        &mut self, params_struct: Option<Arc<Mutex<dyn RosParams + Send>>>,
+    ) -> Result<(impl Future<Output = ()> + Send, impl Stream<Item = (String, ParameterValue)>)>
+    {
+        if let Some(ps) = &params_struct {
+            // register all parameters
+            ps.lock()
+                .unwrap()
+                .register_parameters("", &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);
 
@@ -220,6 +256,7 @@ impl Node {
             ))?;
 
         let params = self.params.clone();
+        let params_struct_clone = params_struct.as_ref().map(|p| p.clone());
         let set_params_future = set_params_request_stream.for_each(
             move |req: ServiceRequest<rcl_interfaces::srv::SetParameters::Service>| {
                 let mut result = rcl_interfaces::srv::SetParameters::Response::default();
@@ -231,18 +268,29 @@ impl Node {
                         .get(&p.name)
                         .map(|v| v != &val)
                         .unwrap_or(true); // changed=true if new
+                    let r = if let Some(ps) = &params_struct_clone {
+                        let result = ps.lock().unwrap().set_parameter(&p.name, &val);
+                        if result.is_ok() {
                             params.lock().unwrap().insert(p.name.clone(), val.clone());
-                    let r = rcl_interfaces::msg::SetParametersResult {
+                        }
+                        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());
+                        rcl_interfaces::msg::SetParametersResult {
                             successful: true,
                             reason: "".into(),
+                        }
                     };
-                    result.results.push(r);
                     // if the value changed, send out new value on parameter event stream
-                    if changed {
+                    if changed && r.successful {
                         if let Err(e) = event_tx.try_send((p.name.clone(), val)) {
                             log::debug!("Warning: could not send parameter event ({}).", e);
                         }
                     }
+                    result.results.push(r);
                 }
                 req.respond(result)
                     .expect("could not send reply to set parameter request");
@@ -259,6 +307,7 @@ impl Node {
             ))?;
 
         let params = self.params.clone();
+        let params_struct_clone = params_struct.as_ref().map(|p| p.clone());
         let get_params_future = get_params_request_stream.for_each(
             move |req: ServiceRequest<rcl_interfaces::srv::GetParameters::Service>| {
                 let params = params.lock().unwrap();
@@ -266,9 +315,18 @@ impl Node {
                     .message
                     .names
                     .iter()
-                    .map(|n| match params.get(n) {
+                    .map(|n| {
+                        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,
+                            }
+                        }
                     })
                     .map(|v| v.into_parameter_value_msg())
                     .collect::<Vec<rcl_interfaces::msg::ParameterValue>>();

r2r/src/parameters.rs

@@ -1,4 +1,5 @@
-use std::ffi::CStr;
+use crate::{Error, Result};
+use std::{collections::HashMap, ffi::CStr};
 
 use crate::msg_types::generated_msgs::rcl_interfaces;
 use r2r_rcl::*;
@@ -143,3 +144,107 @@ impl ParameterValue {
         ret
     }
 }
+
+/// Trait for use it with
+/// [`Node::make_derived_parameter_handler()`](crate::Node::make_derived_parameter_handler()).
+///
+/// The trait is usually derived with `r2r_macros::RosParams`. See
+/// `parameters_derive.rs` example.
+pub trait RosParams {
+    fn register_parameters(
+        &mut self, prefix: &str, params: &mut HashMap<String, ParameterValue>,
+    ) -> Result<()>;
+    fn get_parameter(&mut self, param_name: &str) -> Result<ParameterValue>;
+    fn set_parameter(&mut self, param_name: &str, param_val: &ParameterValue) -> Result<()>;
+}
+
+// Implementation of RosParams for primitive types, i.e. leaf parameters
+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>,
+            ) -> Result<()> {
+                if let Some(param_val) = params.get(prefix) {
+                    // Apply parameter value if set from command line or launch file
+                    self.set_parameter("", param_val)
+                        .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)?));
+                }
+                Ok(())
+            }
+
+            fn get_parameter(&mut self, param_name: &str) -> Result<ParameterValue> {
+                match param_name {
+                    "" => Ok($param_value_type($to_param_conv(self)?)),
+                    _ => Err(Error::InvalidParameterName {
+                        name: param_name.to_owned(),
+                    }),
+                }
+            }
+
+            fn set_parameter(
+                &mut self, param_name: &str, param_val: &ParameterValue,
+            ) -> Result<()> {
+                if param_name != "" {
+                    return Err(Error::InvalidParameterName {
+                        name: param_name.to_owned(),
+                    });
+                }
+                match param_val {
+                    $param_value_type(val) => {
+                        *self = $from_param_conv(val)?;
+                        Ok(())
+                    }
+                    _ => Err(Error::InvalidParameterType {
+                        name: "".to_string(), // will be completed by callers who know the name
+                        ty: std::stringify!($param_value_type),
+                    }),
+                }
+            }
+        }
+    };
+}
+
+impl_ros_params!(bool, ParameterValue::Bool, noop, noop);
+impl_ros_params!(i8, ParameterValue::Integer, try_conv, try_conv);
+impl_ros_params!(i16, ParameterValue::Integer, try_conv, try_conv);
+impl_ros_params!(i32, ParameterValue::Integer, try_conv, try_conv);
+impl_ros_params!(i64, ParameterValue::Integer, noop, noop);
+impl_ros_params!(u8, ParameterValue::Integer, try_conv, try_conv);
+impl_ros_params!(u16, ParameterValue::Integer, try_conv, try_conv);
+impl_ros_params!(u32, ParameterValue::Integer, try_conv, try_conv);
+impl_ros_params!(f64, ParameterValue::Double, noop, noop);
+impl_ros_params!(f32, ParameterValue::Double, to_f64, to_f32);
+impl_ros_params!(String, ParameterValue::String, to_string, to_string);
+// TODO: Implement array parameters
+
+// Helper conversion functions
+fn noop<T: Copy>(x: &T) -> Result<T> {
+    Ok(*x)
+}
+
+fn to_f32(x: &f64) -> Result<f32> {
+    Ok(*x as f32)
+}
+fn to_f64(x: &f32) -> Result<f64> {
+    Ok(*x as f64)
+}
+
+fn try_conv<T, U>(x: &T) -> Result<U>
+where
+    T: Copy,
+    U: TryFrom<T>,
+    <U as TryFrom<T>>::Error: std::error::Error,
+{
+    U::try_from(*x).map_err(|e| Error::ParameterValueConv {
+        name: "".into(),
+        msg: e.to_string(),
+    })
+}
+
+fn to_string(x: &str) -> Result<String> {
+    Ok(x.to_string())
+}

r2r_macros/Cargo.toml

@@ -0,0 +1,12 @@
+[package]
+name = "r2r_macros"
+version = "0.1.0"
+edition = "2021"
+
+[lib]
+proc-macro = true
+
+[dependencies]
+proc-macro2 = "1.0.66"
+quote = "1.0.33"
+syn = "2.0.32"

r2r_macros/src/lib.rs

@@ -0,0 +1,121 @@
+use proc_macro2::TokenStream;
+use quote::{quote, quote_spanned};
+use syn::spanned::Spanned;
+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)]
+pub fn derive_r2r_params(input: proc_macro::TokenStream) -> proc_macro::TokenStream {
+    // Parse the input tokens into a syntax tree.
+    let input = parse_macro_input!(input as DeriveInput);
+
+    // Used in the quasi-quotation below as `#name`.
+    let name = input.ident;
+
+    let register_calls = get_register_calls(&input.data);
+    let get_param_matches = param_matches_for(quote!(get_parameter(suffix)), &input.data);
+    let set_param_matches =
+        param_matches_for(quote!(set_parameter(suffix, param_val)), &input.data);
+
+    let expanded = quote! {
+        // The generated impl.
+        impl ::r2r::RosParams for #name {
+            fn register_parameters(
+                &mut self,
+                prefix: &str,
+                params: &mut ::std::collections::hash_map::HashMap<String, ::r2r::ParameterValue>,
+            ) -> ::r2r::Result<()> {
+                let prefix = if prefix.is_empty() {
+                    String::from("")
+                } else {
+                    format!("{prefix}.")
+                };
+                #register_calls
+                Ok(())
+            }
+            fn get_parameter(&mut self, param_name: &str) -> ::r2r::Result<::r2r::ParameterValue>
+            {
+                let (prefix, suffix) = match param_name.split_once('.') {
+                    None => (param_name, ""),
+                    Some((prefix, suffix)) => (prefix, suffix)
+                };
+                let result = match prefix {
+                    #get_param_matches
+                    _ => Err(::r2r::Error::InvalidParameterName {
+                        name: "".into(),
+                    }),
+                };
+                result.map_err(|e| e.update_param_name(&param_name))
+            }
+            fn set_parameter(&mut self, param_name: &str, param_val: &::r2r::ParameterValue) -> ::r2r::Result<()>
+            {
+                let (prefix, suffix) = match param_name.split_once('.') {
+                    None => (param_name, ""),
+                    Some((prefix, suffix)) => (prefix, suffix)
+                };
+                let result = match prefix {
+                    #set_param_matches
+                    _ => Err(::r2r::Error::InvalidParameterName {
+                        name: "".into(),
+                    }),
+                };
+                result.map_err(|e| e.update_param_name(&param_name))
+            }
+        }
+    };
+
+    // Hand the output tokens back to the compiler.
+    proc_macro::TokenStream::from(expanded)
+}
+
+// Generate calls to register functions of struct fields
+fn get_register_calls(data: &Data) -> TokenStream {
+    match *data {
+        Data::Struct(ref data) => match data.fields {
+            Fields::Named(ref fields) => {
+                let field_matches = fields.named.iter().map(|f| {
+                    let name = &f.ident;
+                    let format_str = format!("{{prefix}}{}", name.as_ref().unwrap());
+                    quote_spanned! {
+                        f.span() =>
+                            self.#name.register_parameters(&format!(#format_str), params)?;
+                    }
+                });
+                quote! {
+                    #(#field_matches)*
+                }
+            }
+            _ => unimplemented!(),
+        },
+        Data::Enum(_) | Data::Union(_) => unimplemented!(),
+    }
+}
+
+// Generate match arms for RosParams::update_parameters()
+fn param_matches_for(call: TokenStream, data: &Data) -> TokenStream {
+    match *data {
+        Data::Struct(ref data) => match data.fields {
+            Fields::Named(ref fields) => {
+                let field_matches = fields.named.iter().map(|f| {
+                    let name = &f.ident;
+                    let name_str = format!("{}", name.as_ref().unwrap());
+                    quote_spanned! {
+                        f.span() =>
+                            #name_str => {
+                                self.#name.#call
+                            }
+                    }
+                });
+                quote! {
+                    #(#field_matches)*
+                }
+            }
+            _ => unimplemented!(),
+        },
+        Data::Enum(_) | Data::Union(_) => unimplemented!(),
+    }
+}