SetParameters service

examples/parameters.rs

@@ -1,11 +1,13 @@
 use r2r;
 
 // try to run like this
-// cargo run --example parameters -- --ros-args -p param_key:=[hej,hopp] -p key2:=5.5 key2=true -r __ns:=/demo -r __node:=my_node
+// 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 set /demo/my_node key2 false
 
 fn main() -> Result<(), Box<dyn std::error::Error>> {
     let ctx = r2r::Context::create()?;
-    let node = r2r::Node::create(ctx, "testnode", "")?;
+    let mut node = r2r::Node::create(ctx, "to_be_replaced", "to_be_replaced")?;
 
     println!("node name: {}", node.name()?);
     println!(
@@ -14,10 +16,20 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     );
     println!("node namespace: {}", node.namespace()?);
 
+    let mut i = 0;
+    loop {
+        node.spin_once(std::time::Duration::from_millis(100));
+        if i % 20 == 0 { // every 2 seconds print all parameters
             println!("node parameters");
-    node.params.iter().for_each(|(k, v)| {
+            node.params.lock().unwrap().iter().for_each(|(k, v)| {
                 println!("{} - {:?}", k, v);
             });
+        }
+        i+=1;
+        if i > 1000 {
+            break;
+        }
+    }
 
     Ok(())
 }

src/lib.rs

@@ -1608,7 +1608,7 @@ impl Drop for ContextHandle {
     }
 }
 
-#[derive(Debug)]
+#[derive(Debug, Clone, PartialEq)]
 pub enum ParameterValue {
     NotSet,
     Bool(bool),
@@ -1679,11 +1679,31 @@ impl ParameterValue {
             ParameterValue::NotSet
         }
     }
+
+    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 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 Sub>>,
@@ -1785,11 +1805,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 = ParameterValue::from_rcl(&*v);
-                self.params.insert(key.to_owned(), val);
+                params.insert(key.to_owned(), val);
             }
         }
 
@@ -1821,9 +1842,9 @@ 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: node_handle,
+                node_handle,
                 subs: Vec::new(),
                 services: Vec::new(),
                 clients: Vec::new(),
@@ -1833,6 +1854,9 @@ impl Node {
                 pubs: Vec::new(),
             };
             node.load_params()?;
+
+            node.setup_parameter_services()?;
+
             Ok(node)
         } else {
             eprintln!("could not create node{}", res);
@@ -1840,6 +1864,31 @@ impl Node {
         }
     }
 
+    fn setup_parameter_services(&mut self) -> Result<()> {
+        let node_name = self.name()?;
+        let params_cb = self.params.clone();
+        self.create_service::<rcl_interfaces::srv::SetParameters::Service>(&format!("{}/set_parameters", node_name),
+                                                      Box::new(move |req: rcl_interfaces::srv::SetParameters::Request| {
+                                                          let mut result = rcl_interfaces::srv::SetParameters::Response::default();
+                                                          for p in req.parameters {
+                                                              let val = ParameterValue::from_parameter_value_msg(p.value);
+                                                              params_cb.lock().unwrap().insert(p.name, val);
+                                                              let r = rcl_interfaces::msg::SetParametersResult {
+                                                                  successful: true,
+                                                                  reason: "".into(),
+                                                              };
+                                                              result.results.push(r);
+                                                          }
+                                                          result
+                                                      }))?;
+
+        Ok(())
+    }
+
+    pub fn get_param(&self, name: &str) -> Option<ParameterValue> {
+        self.params.lock().unwrap().get(name).cloned()
+    }
+
     fn create_subscription_helper(
         &mut self,
         topic: &str,