diff --git a/Cargo.toml b/Cargo.toml index d1ce6d3..2d4f94b 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 6511743..a65ae4a 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 0000000..299b510 --- /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> { + 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 410abc6..649b8fd 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 d23289f..46f739f 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 29923de..0f889ec 100644 --- a/r2r/src/nodes.rs +++ b/r2r/src/nodes.rs @@ -209,6 +209,42 @@ impl Node { &mut self, ) -> Result<(impl Future + Send, impl Stream)> { + 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>, + ) -> Result<(impl Future + Send, impl Stream)> + { + self.make_parameter_handler_internal(Some(params_struct)) + } + + fn make_parameter_handler_internal( + &mut self, params_struct: Option>>, + ) -> Result<(impl Future + Send, impl Stream)> + { + if let Some(ps) = ¶ms_struct { + // register all parameters + ps.lock() + .unwrap() + .register_parameters("", &mut self.params.lock().unwrap())?; + } let mut handlers: Vec + 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| { 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| { 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::>(); diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index 4ea1b10..f452c5e 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, + ) -> Result<()>; + fn get_parameter(&mut self, param_name: &str) -> Result; + 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, + ) -> 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 { + 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(x: &T) -> Result { + Ok(*x) +} + +fn to_f32(x: &f64) -> Result { + Ok(*x as f32) +} +fn to_f64(x: &f32) -> Result { + Ok(*x as f64) +} + +fn try_conv(x: &T) -> Result +where + T: Copy, + U: TryFrom, + >::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 { + Ok(x.to_string()) +} diff --git a/r2r_macros/Cargo.toml b/r2r_macros/Cargo.toml new file mode 100644 index 0000000..b00a0df --- /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 0000000..56b76e4 --- /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, + ) -> ::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!(), + } +}