mirror
/
r2r
mirror of https://github.com/sequenceplanner/r2r.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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`.
This commit is contained in:
Michal Sojka 2023-09-13 00:49:32 +02:00 committed by Martin Dahl
parent 1c22daf8a9
commit a3fe422cd6
9 changed files with 440 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Cargo.toml
View File

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

1
r2r/Cargo.toml
View File

@ -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]

104
r2r/examples/parameters_derive.rs Normal file
View File

@ -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();
}
}

27
r2r/src/error.rs
View File

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

2
r2r/src/lib.rs
View File

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

66
r2r/src/nodes.rs
View File

@ -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>>();

107
r2r/src/parameters.rs
View File

@ -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())
}

12
r2r_macros/Cargo.toml Normal file
View File

@ -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"

121
r2r_macros/src/lib.rs Normal file
View File

@ -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!(),
}
}