Update parameter service handlers to be based on latest master.

examples/parameters.rs

@@ -1,13 +1,29 @@
+use futures::executor::LocalPool;
+use futures::task::LocalSpawnExt;
 use r2r;
 
 // try to run like this
 // cargo run --example parameters -- --ros-args -p key1:=[hello,world] -p key2:=5.5 -r __ns:=/demo -r __node:=my_node
 // then run
+// ros2 param get /demo/my_node key2 # (should return 5.5)
 // ros2 param set /demo/my_node key2 false
+// ros2 param get /demo/my_node key2 # (should return false)
 
 fn main() -> Result<(), Box<dyn std::error::Error>> {
+    // 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")?;
+    let mut timer = node.create_wall_timer(std::time::Duration::from_millis(2000))?;
+
+    // make a parameter handler (once per node).
+    // the parameter handler is optional, only spawn one if you need it.
+    let paramater_handler = node.make_parameter_handler()?;
+    // run parameter handler on your executor.
+    spawner.spawn_local(paramater_handler)?;
 
     println!("node name: {}", node.name()?);
     println!(
@@ -16,21 +32,20 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     );
     println!("node namespace: {}", node.namespace()?);
 
-    let mut i = 0;
+    // print all params every 2 seconds.
-    loop {
+    let params = node.params.clone();
-        node.spin_once(std::time::Duration::from_millis(100));
+    spawner.spawn_local(async move {
-        if i % 20 == 0 {
+        loop {
-            // every 2 seconds print all parameters
             println!("node parameters");
-            node.params.lock().unwrap().iter().for_each(|(k, v)| {
+            params.lock().unwrap().iter().for_each(|(k, v)| {
                 println!("{} - {:?}", k, v);
             });
+            let _elapsed = timer.tick().await.expect("could not tick");
         }
-        i += 1;
+    })?;
-        if i > 1000 {
-            break;
-        }
-    }
 
-    Ok(())
+    loop {
+        node.spin_once(std::time::Duration::from_millis(100));
+        pool.run_until_stalled();
+    }
 }

src/lib.rs

@@ -66,7 +66,6 @@ pub use context::Context;
 
 mod parameters;
 pub use parameters::ParameterValue;
-use parameters::*;
 
 mod clocks;
 pub use clocks::{Clock, ClockType};

src/nodes.rs

@@ -1,8 +1,9 @@
 use super::*;
+use futures::future::{self, join_all};
 
 pub struct Node {
     context: Context,
-    pub params: HashMap<String, ParameterValue>,
+    pub params: Arc<Mutex<HashMap<String, ParameterValue>>>,
     node_handle: Box<rcl_node_t>,
     // the node owns the subscribers
     subs: Vec<Box<dyn Subscriber_>>,
@@ -104,11 +105,12 @@ impl Node {
             let param_values =
                 unsafe { std::slice::from_raw_parts(np.parameter_values, np.num_params) };
 
+            let mut params = self.params.lock().unwrap();
             for (s, v) in param_names.iter().zip(param_values) {
                 let s = unsafe { CStr::from_ptr(*s) };
                 let key = s.to_str().unwrap_or("");
-                let val = parameter_value_from_rcl(&*v);
+                let val = ParameterValue::from_rcl(&*v);
-                self.params.insert(key.to_owned(), val);
+                params.insert(key.to_owned(), val);
             }
         }
 
@@ -139,7 +141,7 @@ impl Node {
 
         if res == RCL_RET_OK as i32 {
             let mut node = Node {
-                params: HashMap::new(),
+                params: Arc::new(Mutex::new(HashMap::new())),
                 context: ctx,
                 node_handle,
                 subs: Vec::new(),
@@ -158,6 +160,63 @@ impl Node {
         }
     }
 
+    /// Returns a future which handles any parameter change requests. Spawn this onto the executor of choice.
+    pub fn make_parameter_handler(&mut self) -> Result<impl Future<Output = ()>> {
+        let mut handlers: Vec<std::pin::Pin<Box<dyn Future<Output = ()>>>> = Vec::new();
+
+        let node_name = self.name()?;
+        let set_params_request_stream = self.create_service::<rcl_interfaces::srv::SetParameters::Service>(
+            &format!("{}/set_parameters", node_name))?;
+
+        let params = self.params.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();
+                for p in &req.message.parameters {
+                    let val = ParameterValue::from_parameter_value_msg(p.value.clone());
+                    params.lock().unwrap().insert(p.name.clone(), val);
+                    let r = rcl_interfaces::msg::SetParametersResult {
+                        successful: true,
+                        reason: "".into(),
+                    };
+                    result.results.push(r);
+                }
+                req.respond(result).expect("could not send reply to set parameter request");
+                future::ready(())
+            });
+        handlers.push(Box::pin(set_params_future));
+
+        // rcl_interfaces/srv/GetParameters
+        let get_params_request_stream = self.create_service::<rcl_interfaces::srv::GetParameters::Service>(
+            &format!("{}/get_parameters", node_name))?;
+
+        let params = self.params.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(|v| v.clone().to_parameter_value_msg())
+                    .collect::<Vec<rcl_interfaces::msg::ParameterValue>>();
+
+                let result = rcl_interfaces::srv::GetParameters::Response {
+                    values
+                };
+                req.respond(result).expect("could not send reply to set parameter request");
+                future::ready(())
+            });
+
+        handlers.push(Box::pin(get_params_future));
+
+        // we don't care about the result, the futures will not complete anyway.
+        Ok(join_all(handlers).map(|_| ()))
+    }
+
     pub fn subscribe<T: 'static>(&mut self, topic: &str) -> Result<impl Stream<Item = T> + Unpin>
     where
         T: WrappedTypesupport,

src/parameters.rs

@@ -14,53 +14,129 @@ pub enum ParameterValue {
     StringArray(Vec<String>),
 }
 
-pub fn parameter_value_from_rcl(v: &rcl_variant_t) -> ParameterValue {
+impl ParameterValue {
-    if v.bool_value != std::ptr::null_mut() {
+    pub(crate) fn from_rcl(v: &rcl_variant_t) -> Self {
-        ParameterValue::Bool(unsafe { *v.bool_value })
+        if v.bool_value != std::ptr::null_mut() {
-    } else if v.integer_value != std::ptr::null_mut() {
+            ParameterValue::Bool(unsafe { *v.bool_value })
-        ParameterValue::Integer(unsafe { *v.integer_value })
+        } else if v.integer_value != std::ptr::null_mut() {
-    } else if v.double_value != std::ptr::null_mut() {
+            ParameterValue::Integer(unsafe { *v.integer_value })
-        ParameterValue::Double(unsafe { *v.double_value })
+        } else if v.double_value != std::ptr::null_mut() {
-    } else if v.string_value != std::ptr::null_mut() {
+            ParameterValue::Double(unsafe { *v.double_value })
-        let s = unsafe { CStr::from_ptr(v.string_value) };
+        } else if v.string_value != std::ptr::null_mut() {
-        let string = s.to_str().unwrap_or("").to_owned();
+            let s = unsafe { CStr::from_ptr(v.string_value) };
-        ParameterValue::String(string)
+            let string = s.to_str().unwrap_or("").to_owned();
-    } else if v.byte_array_value != std::ptr::null_mut() {
+            ParameterValue::String(string)
-        let vals = unsafe {
+        } else if v.byte_array_value != std::ptr::null_mut() {
-            std::slice::from_raw_parts((*v.byte_array_value).values, (*v.byte_array_value).size)
+            let vals = unsafe {
-        };
+                std::slice::from_raw_parts((*v.byte_array_value).values, (*v.byte_array_value).size)
-        ParameterValue::ByteArray(vals.iter().cloned().collect())
+            };
-    } else if v.bool_array_value != std::ptr::null_mut() {
+            ParameterValue::ByteArray(vals.iter().cloned().collect())
-        let vals = unsafe {
+        } else if v.bool_array_value != std::ptr::null_mut() {
-            std::slice::from_raw_parts((*v.bool_array_value).values, (*v.bool_array_value).size)
+            let vals = unsafe {
-        };
+                std::slice::from_raw_parts((*v.bool_array_value).values, (*v.bool_array_value).size)
-        ParameterValue::BoolArray(vals.iter().cloned().collect())
+            };
-    } else if v.integer_array_value != std::ptr::null_mut() {
+            ParameterValue::BoolArray(vals.iter().cloned().collect())
-        let vals = unsafe {
+        } else if v.integer_array_value != std::ptr::null_mut() {
-            std::slice::from_raw_parts(
+            let vals = unsafe {
-                (*v.integer_array_value).values,
+                std::slice::from_raw_parts(
-                (*v.integer_array_value).size,
+                    (*v.integer_array_value).values,
-            )
+                    (*v.integer_array_value).size,
-        };
+                )
-        ParameterValue::IntegerArray(vals.iter().cloned().collect())
+            };
-    } else if v.double_array_value != std::ptr::null_mut() {
+            ParameterValue::IntegerArray(vals.iter().cloned().collect())
-        let vals = unsafe {
+        } else if v.double_array_value != std::ptr::null_mut() {
-            std::slice::from_raw_parts((*v.double_array_value).values, (*v.double_array_value).size)
+            let vals = unsafe {
-        };
+                std::slice::from_raw_parts(
-        ParameterValue::DoubleArray(vals.iter().cloned().collect())
+                    (*v.double_array_value).values,
-    } else if v.string_array_value != std::ptr::null_mut() {
+                    (*v.double_array_value).size,
-        let vals = unsafe {
+                )
-            std::slice::from_raw_parts((*v.string_array_value).data, (*v.string_array_value).size)
+            };
-        };
+            ParameterValue::DoubleArray(vals.iter().cloned().collect())
-        let s = vals
+        } else if v.string_array_value != std::ptr::null_mut() {
-            .iter()
+            let vals = unsafe {
-            .map(|cs| {
+                std::slice::from_raw_parts(
-                let s = unsafe { CStr::from_ptr(*cs) };
+                    (*v.string_array_value).data,
-                s.to_str().unwrap_or("").to_owned()
+                    (*v.string_array_value).size,
-            })
+                )
-            .collect();
+            };
-        ParameterValue::StringArray(s)
+            let s = vals
-    } else {
+                .iter()
-        ParameterValue::NotSet
+                .map(|cs| {
+                    let s = unsafe { CStr::from_ptr(*cs) };
+                    s.to_str().unwrap_or("").to_owned()
+                })
+                .collect();
+            ParameterValue::StringArray(s)
+        } else {
+            ParameterValue::NotSet
+        }
     }
+
+    pub(crate) fn from_parameter_value_msg(msg: rcl_interfaces::msg::ParameterValue) -> Self {
+        // todo: use constants from ParameterType message
+        match msg.type_ {
+            0 => ParameterValue::NotSet,
+            1 => ParameterValue::Bool(msg.bool_value),
+            2 => ParameterValue::Integer(msg.integer_value),
+            3 => ParameterValue::Double(msg.double_value),
+            4 => ParameterValue::String(msg.string_value),
+            5 => ParameterValue::ByteArray(msg.byte_array_value),
+            6 => ParameterValue::BoolArray(msg.bool_array_value),
+            7 => ParameterValue::IntegerArray(msg.integer_array_value),
+            8 => ParameterValue::DoubleArray(msg.double_array_value),
+            9 => ParameterValue::StringArray(msg.string_array_value),
+            _ => {
+                println!("warning: malformed parametervalue message");
+                ParameterValue::NotSet
+            }
+        }
+    }
+
+    pub(crate) fn to_parameter_value_msg(self) -> rcl_interfaces::msg::ParameterValue {
+        let mut ret = rcl_interfaces::msg::ParameterValue::default();
+
+        match self {
+            ParameterValue::NotSet => {
+                ret.type_ = 0; // uint8 PARAMETER_NOT_SET=0
+            },
+            ParameterValue::Bool(b) => {
+                ret.type_ = 1; // uint8 PARAMETER_BOOL=1
+                ret.bool_value = b;
+            },
+            ParameterValue::Integer(i) => {
+                ret.type_ = 2; // uint8 PARAMETER_INTEGER=2
+                ret.integer_value = i;
+            },
+            ParameterValue::Double(d) => {
+                ret.type_ = 3; // uint8 PARAMETER_DOUBLE=3
+                ret.double_value = d;
+            },
+            ParameterValue::String(s) => {
+                ret.type_ = 4; // uint8 PARAMETER_STRING=4
+                ret.string_value = s;
+            },
+            ParameterValue::ByteArray(ba) => {
+                ret.type_ = 5; // uint8 PARAMETER_BYTE_ARRAY=5
+                ret.byte_array_value = ba;
+            },
+            ParameterValue::BoolArray(ba) => {
+                ret.type_ = 6;  // uint8 PARAMETER_BOOL_ARRAY=6
+                ret.bool_array_value = ba;
+            },
+            ParameterValue::IntegerArray(ia) => {
+                ret.type_ = 7;  // uint8 PARAMETER_INTEGER_ARRAY=7
+                ret.integer_array_value = ia;
+            },
+            ParameterValue::DoubleArray(da) => {
+                ret.type_ = 8;  // uint8 PARAMETER_DOUBLE_ARRAY=8
+                ret.double_array_value = da;
+            },
+            ParameterValue::StringArray(sa) => {
+                ret.type_ = 9;  // int PARAMETER_STRING_ARRAY=9
+                ret.string_array_value = sa;
+            },
+        }
+        ret
+    }
+
 }