Added support for clients and services

Author: esteve

examples/minimal_client_service/Cargo.toml

@@ -0,0 +1,32 @@
+[package]
+name = "examples_rclrs_minimal_client_service"
+version = "0.2.0"
+authors = ["Esteve Fernandez <[email protected]>"]
+edition = "2021"
+
+[[bin]]
+name = "minimal_client"
+path = "src/minimal_client.rs"
+
+[[bin]]
+name = "minimal_client_async"
+path = "src/minimal_client_async.rs"
+
+[[bin]]
+name = "minimal_service"
+path = "src/minimal_service.rs"
+
+[dependencies]
+anyhow = {version = "1", features = ["backtrace"]}
+
+[dependencies.rclrs]
+version = "*"
+
+[dependencies.rosidl_runtime_rs]
+version = "*"
+
+[dependencies.std_msgs]
+version = "*"
+
+[dependencies.example_interfaces]
+version = "*"

examples/minimal_client_service/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model
+   href="http://download.ros.org/schema/package_format3.xsd"
+   schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>examples_rclrs_minimal_client_service</name>
+  <version>0.2.0</version>
+  <description>Package containing an example of the client-service mechanism in rclrs.</description>
+  <maintainer email="[email protected]">Esteve Fernandez</maintainer>
+  <license>Apache License 2.0</license>
+
+  <build_depend>example_interfaces</build_depend>
+  <build_depend>rclrs</build_depend>
+  <build_depend>rosidl_runtime_rs</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>example_interfaces</exec_depend>
+  <exec_depend>rclrs</exec_depend>
+  <exec_depend>rosidl_runtime_rs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_cargo</build_type>
+  </export>
+</package>

examples/minimal_client_service/src/minimal_client.rs

@@ -0,0 +1,33 @@
+use anyhow::{Error, Result};
+use std::env;
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args()).unwrap();
+
+    let mut node = context.create_node("minimal_client")?;
+
+    let client = node.create_client::<example_interfaces::srv::AddTwoInts>("add_two_ints")?;
+
+    let mut request = example_interfaces::srv::AddTwoInts_Request::default();
+    request.a = 41;
+    request.b = 1;
+
+    println!("Starting client");
+
+    std::thread::sleep(std::time::Duration::from_millis(500));
+
+    client.async_send_request_with_callback(
+        &request,
+        move |response: &example_interfaces::srv::AddTwoInts_Response| {
+            println!(
+                "Result of {} + {} is: {}",
+                request.a, request.b, response.sum
+            );
+        },
+    )?;
+
+    std::thread::sleep(std::time::Duration::from_millis(500));
+
+    println!("Waiting for response");
+    rclrs::spin(&node).map_err(|err| err.into())
+}

examples/minimal_client_service/src/minimal_client_async.rs

@@ -0,0 +1,29 @@
+use anyhow::{Error, Result};
+use std::env;
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args()).unwrap();
+
+    let mut node = context.create_node("minimal_client")?;
+
+    let client = node.create_client::<example_interfaces::srv::AddTwoInts>("add_two_ints")?;
+
+    let mut request = example_interfaces::srv::AddTwoInts_Request::default();
+    request.a = 41;
+    request.b = 1;
+
+    println!("Starting client");
+
+    std::thread::sleep(std::time::Duration::from_millis(500));
+
+    let future = client.call_async(&request)?;
+
+    println!("Waiting for response");
+    let response = rclrs::spin_until_future_complete(&node, future.clone())?;
+
+    println!(
+        "Result of {} + {} is: {}",
+        request.a, request.b, response.sum
+    );
+    Ok(())
+}

examples/minimal_client_service/src/minimal_service.rs

@@ -0,0 +1,23 @@
+use anyhow::{Error, Result};
+use std::env;
+
+fn handle_service(
+    _request_header: &rclrs::rmw_request_id_t,
+    request: &example_interfaces::srv::AddTwoInts_Request,
+    response: &mut example_interfaces::srv::AddTwoInts_Response,
+) {
+    println!("request: {} + {}", request.a, request.b);
+    response.sum = request.a + request.b;
+}
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args()).unwrap();
+
+    let mut node = context.create_node("minimal_service")?;
+
+    let _server = node
+        .create_service::<example_interfaces::srv::AddTwoInts, _>("add_two_ints", handle_service)?;
+
+    println!("Starting server");
+    rclrs::spin(&node).map_err(|err| err.into())
+}

rclrs/src/future.rs

@@ -0,0 +1,72 @@
+/// Based on https://www.viget.com/articles/understanding-futures-in-rust-part-1/
+use std::future::Future;
+use std::marker::PhantomData;
+use std::pin::Pin;
+use std::sync::Arc;
+use std::task::Context;
+use std::task::Poll;
+use std::task::RawWaker;
+use std::task::RawWakerVTable;
+use std::task::Waker;
+
+use parking_lot::Mutex;
+
+#[derive(Default)]
+pub struct RclFuture<T> {
+    value: Option<T>,
+}
+
+impl<T: Default + Clone> RclFuture<T> {
+    pub fn new() -> RclFuture<T> {
+        Self { value: None }
+    }
+
+    pub fn set_value(&mut self, msg: T) {
+        self.value = Some(msg);
+    }
+}
+
+impl<T: Clone> Future for RclFuture<T> {
+    type Output = T;
+
+    fn poll(self: Pin<&mut Self>, _ctx: &mut Context) -> Poll<Self::Output> {
+        if let Some(value) = &self.value {
+            Poll::Ready(value.clone())
+        } else {
+            Poll::Pending
+        }
+    }
+}
+
+#[derive(Clone)]
+struct RclWaker {}
+
+fn rclwaker_wake(_s: &RclWaker) {}
+
+fn rclwaker_wake_by_ref(_s: &RclWaker) {}
+
+fn rclwaker_clone(s: &RclWaker) -> RawWaker {
+    let arc = unsafe { Arc::from_raw(s) };
+    std::mem::forget(arc.clone());
+    RawWaker::new(Arc::into_raw(arc) as *const (), &VTABLE)
+}
+
+const VTABLE: RawWakerVTable = unsafe {
+    RawWakerVTable::new(
+        |s| rclwaker_clone(&*(s as *const RclWaker)),
+        |s| rclwaker_wake(&*(s as *const RclWaker)),
+        |s| rclwaker_wake_by_ref(&*(s as *const RclWaker)),
+        |s| drop(Arc::from_raw(s as *const RclWaker)),
+    )
+};
+
+fn rclwaker_into_waker(s: *const RclWaker) -> Waker {
+    let raw_waker = RawWaker::new(s as *const (), &VTABLE);
+    unsafe { Waker::from_raw(raw_waker) }
+}
+
+fn create_rcl_waker_context() {
+    let rclwaker = Arc::new(RclWaker {});
+    let waker = rclwaker_into_waker(Arc::into_raw(rclwaker));
+    std::task::Context::from_waker(&waker)
+}

rclrs/src/lib.rs

@@ -5,6 +5,12 @@
 //!
 //! [1]: https://github.com/ros2-rust/ros2_rust/blob/master/README.md
 
+use std::future::Future;
+use std::pin::Pin;
+use std::sync::Arc;
+use std::task::Poll;
+use std::time::Duration;
+
 mod context;
 mod error;
 mod node;
@@ -20,37 +26,70 @@ pub use qos::*;
 pub use wait::*;
 
 use rcl_bindings::rcl_context_is_valid;
-use std::time::Duration;
+
+pub use rcl_bindings::rmw_request_id_t;
+
+use parking_lot::Mutex;
 
 /// Polls the node for new messages and executes the corresponding callbacks.
 ///
 /// See [`WaitSet::wait`] for the meaning of the `timeout` parameter.
 ///
 /// This may under some circumstances return
-/// [`SubscriptionTakeFailed`][1] when the wait set spuriously wakes up.
+/// [`SubscriptionTakeFailed`][1], [`ClientTakeFailed`][2], [`ServiceTakeFailed`][3] when the wait
+/// set spuriously wakes up.
 /// This can usually be ignored.
 ///
 /// [1]: crate::SubscriberErrorCode
+/// [2]: crate::ClientErrorCode
+/// [3]: crate::ServiceErrorCode
 pub fn spin_once(node: &Node, timeout: Option<Duration>) -> Result<(), RclrsError> {
     let live_subscriptions = node.live_subscriptions();
+    let live_clients = node.live_clients();
+    let live_services = node.live_services();
     let ctx = Context {
         handle: node.context.clone(),
     };
-    let mut wait_set = WaitSet::new(live_subscriptions.len(), &ctx)?;
+    let mut wait_set = WaitSet::new(
+        live_subscriptions.len(),
+        0,
+        0,
+        live_clients.len(),
+        live_services.len(),
+        0,
+        &ctx,
+    )?;
 
     for live_subscription in &live_subscriptions {
         wait_set.add_subscription(live_subscription.clone())?;
     }
 
+    for live_client in &live_clients {
+        wait_set.add_client(live_client.clone())?;
+    }
+
+    for live_service in &live_services {
+        wait_set.add_service(live_service.clone())?;
+    }
+
     let ready_entities = wait_set.wait(timeout)?;
+
     for ready_subscription in ready_entities.subscriptions {
         ready_subscription.execute()?;
     }
 
+    for ready_client in ready_entities.clients {
+        ready_client.execute()?;
+    }
+
+    for ready_service in ready_entities.services {
+        ready_service.execute()?;
+    }
+
     Ok(())
 }
 
-/// Convenience function for calling [`spin_once`] in a loop.
+/// Convenience function for calling [`rclrs::spin_once`] in a loop.
 ///
 /// This function additionally checks that the context is still valid.
 pub fn spin(node: &Node) -> Result<(), RclrsError> {
@@ -70,6 +109,28 @@ pub fn spin(node: &Node) -> Result<(), RclrsError> {
             };
         }
     }
-
     Ok(())
 }
+
+pub fn spin_until_future_complete<T: Unpin + Clone>(
+    node: &node::Node,
+    mut future: Arc<Mutex<Box<future::RclFuture<T>>>>,
+) -> Result<<future::RclFuture<T> as Future>::Output, RclReturnCode> {
+    let mut cx = future::create_rcl_waker_context();
+
+    loop {
+        let context_valid = unsafe { rcl_context_is_valid(&mut *node.context.lock()) };
+        if context_valid {
+            if let Some(error) = spin_once(node, None).err() {
+                match error {
+                    RclReturnCode::Timeout => continue,
+                    error => return Err(error),
+                };
+            };
+            match Future::poll(Pin::new(&mut *future.lock()), &mut cx) {
+                Poll::Ready(val) => break Ok(val),
+                Poll::Pending => continue,
+            };
+        }
+    }
+}