Macro for deriving ROS parameters code

Cargo.toml

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

README.md

@@ -39,7 +39,7 @@ What works?
 - Publish/subscribe
 - Services
 - Actions
-- Rudimentary parameter handling
+- Parameter handling
 
 Changelog
 --------------------

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

@@ -16,7 +16,7 @@
 //!- Publish/subscribe
 //!- Services
 //!- Actions
-//!- Rudimentary parameter handling
+//!- Parameter handling
 //!
 //! ---
 //!
@@ -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
-                    params.lock().unwrap().insert(p.name.clone(), val.clone());
-                    let r = rcl_interfaces::msg::SetParametersResult {
-                        successful: true,
-                        reason: "".into(),
+                    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());
+                        }
+                        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,16 +307,26 @@ 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();
                 let values = req
                     .message
                     .names
                     .iter()
-                    .map(|n| match params.get(n) {
-                        Some(v) => v.clone(),
-                        None => ParameterValue::NotSet,
+                    .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>>();