From 64fe046a2f8b84fb8509eebee9ee174c221d29fe Mon Sep 17 00:00:00 2001 From: Michal Sojka <michal.sojka@cvut.cz> Date: Wed, 13 Sep 2023 00:49:32 +0200 Subject: [PATCH 1/2] 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 | 1 + r2r/Cargo.toml | 1 + r2r/examples/parameters_derive.rs | 104 +++++++++++++++++++++++++ r2r/src/error.rs | 27 +++++++ r2r/src/lib.rs | 2 +- r2r/src/nodes.rs | 76 ++++++++++++++++--- r2r/src/parameters.rs | 107 +++++++++++++++++++++++++- r2r_macros/Cargo.toml | 12 +++ r2r_macros/src/lib.rs | 121 ++++++++++++++++++++++++++++++ 9 files changed, 440 insertions(+), 11 deletions(-) create mode 100644 r2r/examples/parameters_derive.rs create mode 100644 r2r_macros/Cargo.toml create mode 100644 r2r_macros/src/lib.rs diff --git a/Cargo.toml b/Cargo.toml index d1ce6d3b1..2d4f94bde 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,5 +5,6 @@ members = [ "r2r_actions", "r2r_common", "r2r_msg_gen", + "r2r_macros", "r2r_rcl", ] \ No newline at end of file diff --git a/r2r/Cargo.toml b/r2r/Cargo.toml index 65117431d..a65ae4ad7 100644 --- a/r2r/Cargo.toml +++ b/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] diff --git a/r2r/examples/parameters_derive.rs b/r2r/examples/parameters_derive.rs new file mode 100644 index 000000000..299b510a0 --- /dev/null +++ b/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(); + } +} diff --git a/r2r/src/error.rs b/r2r/src/error.rs index 410abc603..649b8fdf2 100644 --- a/r2r/src/error.rs +++ b/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, + } + } } diff --git a/r2r/src/lib.rs b/r2r/src/lib.rs index d23289faf..46f739fbc 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; +pub use parameters::{ParameterValue, RosParams}; mod clocks; pub use clocks::{Clock, ClockType}; diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs index 29923de6c..0f889ec9d 100644 --- a/r2r/src/nodes.rs +++ b/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) = ¶ms_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) = ¶ms_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,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) { - Some(v) => v.clone(), - None => ParameterValue::NotSet, + .map(|n| { + 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, + } + } }) .map(|v| v.into_parameter_value_msg()) .collect::<Vec<rcl_interfaces::msg::ParameterValue>>(); diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index 4ea1b10d4..f452c5e48 100644 --- a/r2r/src/parameters.rs +++ b/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()) +} diff --git a/r2r_macros/Cargo.toml b/r2r_macros/Cargo.toml new file mode 100644 index 000000000..b00a0df13 --- /dev/null +++ b/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" diff --git a/r2r_macros/src/lib.rs b/r2r_macros/src/lib.rs new file mode 100644 index 000000000..56b76e4de --- /dev/null +++ b/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(¶m_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(¶m_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!(), + } +} From 0a355ece07d9ca25c85068bc1c358b2701d2cb11 Mon Sep 17 00:00:00 2001 From: Michal Sojka <michal.sojka@cvut.cz> Date: Fri, 15 Sep 2023 16:52:53 +0200 Subject: [PATCH 2/2] doc: Parameter handling is no longer "rudimentary" --- README.md | 2 +- r2r/src/lib.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index f0036c2fe..d3224f9eb 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ What works? - Publish/subscribe - Services - Actions -- Rudimentary parameter handling +- Parameter handling Changelog -------------------- diff --git a/r2r/src/lib.rs b/r2r/src/lib.rs index 46f739fbc..e2e1211a6 100644 --- a/r2r/src/lib.rs +++ b/r2r/src/lib.rs @@ -16,7 +16,7 @@ //!- Publish/subscribe //!- Services //!- Actions -//!- Rudimentary parameter handling +//!- Parameter handling //! //! --- //!