Merge pull request #163 from Carter12s/persistent-service-clients

Persistent service clients

CHANGELOG.md

@@ -22,6 +22,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ## Unreleased
 
 ### Added
+- ROS1 native service servers and service clients are now supported (experimental feature)
 
 ### Fixed
 

roslibrust/examples/ros1_service_client.rs

@@ -14,14 +14,18 @@ async fn main() -> Result<(), anyhow::Error> {
     let nh = NodeHandle::new("http://localhost:11311", "service_client_rs").await?;
     log::info!("Connected!");
 
-    let response: rosapi::GetTimeResponse = nh
+    let client = nh
         .service_client::<rosapi::GetTime>("rosapi/get_time")
-        .await?
-        .call(&rosapi::GetTimeRequest {})
         .await?;
 
+    let response = client.call(&rosapi::GetTimeRequest {}).await?;
+
     log::info!("Got time: {:?}", response);
 
+    let response2 = client.call(&rosapi::GetTimeRequest {}).await?;
+
+    log::info!("Got time: {:?}", response2);
+
     Ok(())
 }
 

roslibrust/examples/ros1_service_server.rs

@@ -0,0 +1,79 @@
+roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces");
+
+#[cfg(feature = "ros1")]
+#[tokio::main]
+async fn main() -> Result<(), anyhow::Error> {
+    use log::*;
+    use roslibrust::ros1::NodeHandle;
+    use std::sync::{Arc, Mutex};
+
+    simple_logger::SimpleLogger::new()
+        .with_level(log::LevelFilter::Debug)
+        .without_timestamps()
+        .init()
+        .unwrap();
+
+    let nh = NodeHandle::new("http://localhost:11311", "service_server_rs").await?;
+    log::info!("Connected!");
+
+    // Because our service server can run from any thread at any time
+    // and *multiple service requests can process in parallel*
+    // the service function is not allowed to hold any references to external data
+    // all data must be owned by the service function
+    // See: https://tokio.rs/tokio/tutorial/spawning
+    // To work around this we want to move a "protected" version of our state into the function
+    let bool_state = Arc::new(Mutex::new(false));
+
+    let bool_state_copy = bool_state.clone(); // Make a copy (of the Arc, not the contents!)
+    let server_fn = move |request: std_srvs::SetBoolRequest| {
+        log::info!("Got request to set bool: {request:?}");
+        // Actually set the bool
+        *bool_state_copy.lock().unwrap() = request.data;
+        Ok(std_srvs::SetBoolResponse {
+            success: true,
+            message: "You set my bool!".to_string(),
+        })
+    };
+
+    // Start our service running!
+    let _handle = nh
+        .advertise_service::<std_srvs::SetBool, _>("~/my_set_bool", server_fn)
+        .await?;
+    info!("Service has started");
+
+    // Setup a task to kill this process when ctrl_c comes in:
+    tokio::spawn(async move {
+        tokio::signal::ctrl_c().await.unwrap();
+        std::process::exit(0);
+    });
+
+    // As long as _handle is kept alive our service will continue to run
+
+    // For funsies we can also spawn a task to periodically call our service
+    let service_client = nh
+        .service_client::<std_srvs::SetBool>("~/my_set_bool")
+        .await?;
+    tokio::spawn(async move {
+        let mut bool = false;
+        loop {
+            bool = !bool;
+            service_client
+                .call(&std_srvs::SetBoolRequest { data: bool })
+                .await
+                .unwrap();
+            tokio::time::sleep(tokio::time::Duration::from_secs(2)).await;
+        }
+    });
+
+    // We can still access our shared state, we just have to do it safely
+    loop {
+        let cur_bool = *bool_state.lock().unwrap();
+        info!("Current value of our bool: {cur_bool}");
+        tokio::time::sleep(tokio::time::Duration::from_secs(1)).await;
+    }
+}
+
+#[cfg(not(feature = "ros1"))]
+fn main() {
+    eprintln!("This example does nothing without compiling with the feature 'ros1'");
+}

roslibrust/src/ros1/service_client.rs

@@ -293,4 +293,44 @@ mod test {
             panic!("Unexpected error type");
         }
     }
+
+    #[test_log::test(tokio::test)]
+    async fn persistent_client_can_be_called_multiple_times() {
+        let nh = NodeHandle::new(
+            "http://localhost:11311",
+            "/persistent_client_can_be_called_multiple_times",
+        )
+        .await
+        .unwrap();
+
+        let server_fn = |request: test_msgs::AddTwoIntsRequest| {
+            Ok(test_msgs::AddTwoIntsResponse {
+                sum: request.a + request.b,
+            })
+        };
+
+        let _handle = nh
+            .advertise_service::<test_msgs::AddTwoInts, _>(
+                "/persistent_client_can_be_called_multiple_times/add_two",
+                server_fn,
+            )
+            .await
+            .unwrap();
+
+        let client = nh
+            .service_client::<test_msgs::AddTwoInts>(
+                "/persistent_client_can_be_called_multiple_times/add_two",
+            )
+            .await
+            .unwrap();
+
+        for i in 0..10 {
+            let call: test_msgs::AddTwoIntsResponse = client
+                .call(&test_msgs::AddTwoIntsRequest { a: 1, b: i })
+                .await
+                .unwrap();
+
+            assert_eq!(call.sum, 1 + i);
+        }
+    }
 }