Add caching for rosparam get requests (#193)

Author: adnanademovic

CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog
 
+## Rosrust Unreleased
+### Added
+- Automatic caching of parameters
+
 ## Rosrust Msg 0.1.7 (2023-04-01)
 ### Added
 - Find messages in `ROS_PACKAGE_PATH`

rosrust/examples/publish_from_param.rs

@@ -0,0 +1,47 @@
+use std::time::Instant;
+
+mod msg {
+    rosrust::rosmsg_include!(std_msgs / String);
+}
+
+fn main() {
+    env_logger::init();
+
+    // Initialize node
+    rosrust::init("talker");
+
+    // Create publisher
+    let chatter_pub = rosrust::publish("chatter", 2).unwrap();
+    chatter_pub.wait_for_subscribers(None).unwrap();
+
+    let message = rosrust::param("~message").unwrap();
+
+    let log_names = rosrust::param("~log_names").unwrap().get().unwrap_or(false);
+
+    // Create object that maintains 10Hz between sleep requests
+    let rate = rosrust::rate(10.0);
+
+    // Breaks when a shutdown signal is sent
+    while rosrust::is_ok() {
+        let t = Instant::now();
+        let message = message.get_raw().unwrap_or(xml_rpc::Value::Bool(false));
+        let message_fetch_time = t.elapsed();
+        // Create string message
+        let msg = msg::std_msgs::String {
+            data: format!("{:?} from rosrust in {:?}", message, message_fetch_time),
+        };
+
+        // Log event
+        rosrust::ros_info!("Publishing: {}", msg.data);
+
+        // Send string message to topic via publisher
+        chatter_pub.send(msg).unwrap();
+
+        if log_names {
+            rosrust::ros_info!("Subscriber names: {:?}", chatter_pub.subscriber_names());
+        }
+
+        // Sleep to maintain 10Hz rate
+        rate.sleep();
+    }
+}

rosrust/src/api/ros.rs

@@ -8,9 +8,11 @@ use super::resolve;
 use super::slave::Slave;
 use crate::api::clock::Delay;
 use crate::api::handlers::CallbackSubscriptionHandler;
+use crate::api::slave::ParamCache;
 use crate::api::ShutdownManager;
 use crate::msg::rosgraph_msgs::{Clock as ClockMsg, Log};
 use crate::msg::std_msgs::Header;
+use crate::rosxmlrpc::client::bad_response_structure;
 use crate::tcpros::{Client, Message, ServicePair, ServiceResult};
 use crate::util::FAILED_TO_LOCK;
 use crate::{RawMessage, RawMessageDescription, SubscriptionHandler};
@@ -31,6 +33,7 @@ use yaml_rust::{Yaml, YamlLoader};
 pub struct Ros {
     master: Arc<Master>,
     slave: Arc<Slave>,
+    param_cache: ParamCache,
     hostname: String,
     bind_address: String,
     resolver: Resolver,
@@ -111,19 +114,22 @@ impl Ros {
             move || drop(logger.lock().unwrap().take())
         }));
 
+        let param_cache = Arc::new(Mutex::new(Default::default()));
         let slave = Slave::new(
             master_uri,
             hostname,
             bind_host,
             0,
             &name,
+            Arc::clone(&param_cache),
             Arc::clone(&shutdown_manager),
         )?;
         let master = Master::new(master_uri, &name, slave.uri())?;
 
         Ok(Ros {
             master: Arc::new(master),
             slave: Arc::new(slave),
+            param_cache,
             hostname: String::from(hostname),
             bind_address: String::from(bind_host),
             resolver,
@@ -195,6 +201,7 @@ impl Ros {
 
     pub fn param(&self, name: &str) -> Option<Parameter> {
         self.resolver.translate(name).ok().map(|v| Parameter {
+            param_cache: Arc::clone(&self.param_cache),
             master: Arc::clone(&self.master),
             name: v,
         })
@@ -484,6 +491,7 @@ impl Ros {
 }
 
 pub struct Parameter {
+    param_cache: ParamCache,
     master: Arc<Master>,
     name: String,
 }
@@ -494,11 +502,30 @@ impl Parameter {
     }
 
     pub fn get<'b, T: Deserialize<'b>>(&self) -> Response<T> {
-        self.master.get_param::<T>(&self.name)
+        let data = self.get_raw()?;
+        Deserialize::deserialize(data).map_err(bad_response_structure)
     }
 
     pub fn get_raw(&self) -> Response<xml_rpc::Value> {
-        self.master.get_param_any(&self.name)
+        let subscribed;
+        {
+            let cache = self.param_cache.lock().expect(FAILED_TO_LOCK);
+            if let Some(data) = cache.data.get(&self.name) {
+                return data.clone();
+            }
+            subscribed = cache.subscribed;
+        }
+        if !subscribed {
+            self.master.subscribe_param_any("/")?;
+            self.param_cache.lock().expect(FAILED_TO_LOCK).subscribed = true;
+        }
+        let data = self.master.get_param_any(&self.name);
+        self.param_cache
+            .lock()
+            .expect(FAILED_TO_LOCK)
+            .data
+            .insert(self.name.clone(), data.clone());
+        data
     }
 
     pub fn set<T: Serialize>(&self, value: &T) -> Response<()> {

rosrust/src/api/slave/handler.rs

@@ -2,7 +2,7 @@ use super::publications::PublicationsTracker;
 use super::subscriptions::SubscriptionsTracker;
 use crate::rosxmlrpc::{self, Response, ResponseError, Server};
 use crate::tcpros::Service;
-use crate::util::kill;
+use crate::util::{kill, FAILED_TO_LOCK};
 use log::{error, info};
 use std::collections::HashMap;
 use std::net::SocketAddr;
@@ -23,11 +23,20 @@ fn unwrap_array_case(params: Params) -> Params {
     params
 }
 
+#[derive(Default)]
+pub struct ParamCacheState {
+    pub data: HashMap<String, Response<Value>>,
+    pub subscribed: bool,
+}
+
+pub type ParamCache = Arc<Mutex<ParamCacheState>>;
+
 impl SlaveHandler {
     pub fn new(
         master_uri: &str,
         hostname: &str,
         name: &str,
+        param_cache: ParamCache,
         shutdown_signal: kill::Sender,
     ) -> SlaveHandler {
         let mut server = Server::default();
@@ -105,8 +114,33 @@ impl SlaveHandler {
             ))
         });
 
-        server.register_value("paramUpdate", "Parameter updated", |_args| {
-            // We don't do anything with parameter updates
+        server.register_value("paramUpdate", "Parameter updated", move |args| {
+            let mut args = unwrap_array_case(args).into_iter();
+            let _caller_id = args
+                .next()
+                .ok_or_else(|| ResponseError::Client("Missing argument 'caller_id'".into()))?;
+            let parameter_key = match args.next() {
+                Some(Value::String(parameter_key)) => parameter_key,
+                _ => {
+                    return Err(ResponseError::Client(
+                        "Missing argument 'parameter_key'".into(),
+                    ))
+                }
+            };
+            let _parameter_value = match args.next() {
+                Some(parameter_value) => parameter_value,
+                _ => {
+                    return Err(ResponseError::Client(
+                        "Missing argument 'parameter_key'".into(),
+                    ))
+                }
+            };
+            let key = parameter_key.trim_end_matches('/');
+            param_cache
+                .lock()
+                .expect(FAILED_TO_LOCK)
+                .data
+                .retain(|k, _| !k.starts_with(key) && !key.starts_with(k));
             Ok(Value::Int(0))
         });
 

rosrust/src/api/slave/mod.rs

@@ -2,6 +2,7 @@ mod handler;
 mod publications;
 mod subscriptions;
 
+pub use self::handler::ParamCache;
 use self::handler::SlaveHandler;
 use super::error::{self, ErrorKind, Result};
 use crate::api::ShutdownManager;
@@ -33,12 +34,14 @@ impl Slave {
         bind_address: &str,
         port: u16,
         name: &str,
+        param_cache: ParamCache,
         shutdown_manager: Arc<ShutdownManager>,
     ) -> Result<Slave> {
         use std::net::ToSocketAddrs;
 
         let (shutdown_tx, shutdown_rx) = kill::channel(kill::KillMode::Sync);
-        let handler = SlaveHandler::new(master_uri, hostname, name, shutdown_tx.clone());
+        let handler =
+            SlaveHandler::new(master_uri, hostname, name, param_cache, shutdown_tx.clone());
         let publications = handler.publications.clone();
         let subscriptions = handler.subscriptions.clone();
         let services = Arc::clone(&handler.services);

rosrust/src/rosxmlrpc/client.rs

@@ -63,6 +63,6 @@ fn bad_request_structure<T: ::std::fmt::Display>(err: T) -> ResponseError {
     ResponseError::Client(format!("Failed to serialize parameters: {}", err))
 }
 
-fn bad_response_structure<T: ::std::fmt::Display>(err: T) -> ResponseError {
+pub(crate) fn bad_response_structure<T: ::std::fmt::Display>(err: T) -> ResponseError {
     ResponseError::Server(format!("Response data has unexpected structure: {}", err))
 }

rosrust/src/rosxmlrpc/mod.rs

@@ -9,7 +9,7 @@ pub mod server;
 
 pub type Response<T> = Result<T, ResponseError>;
 
-#[derive(Debug)]
+#[derive(Clone, Debug)]
 pub enum ResponseError {
     Client(String),
     Server(String),