Added support for clients and services

Author: esteve

examples/message_demo/package.xml

@@ -12,10 +12,12 @@
   <build_depend>rclrs</build_depend>
   <build_depend>rosidl_runtime_rs</build_depend>
   <build_depend>rclrs_example_msgs</build_depend>
+  <build_depend>example_interfaces</build_depend>
 
   <exec_depend>rclrs</exec_depend>
   <exec_depend>rosidl_runtime_rs</exec_depend>
   <exec_depend>rclrs_example_msgs</exec_depend>
+  <exec_depend>example_interfaces</exec_depend>
 
   <export>
     <build_type>ament_cargo</build_type>

examples/minimal_client_service/Cargo.toml

@@ -0,0 +1,35 @@
+[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.rclrs_example_msgs]
+version = "*"
+
+[dependencies.example_interfaces]
+version = "*"

examples/minimal_client_service/package.xml

@@ -0,0 +1,23 @@
+<?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_pub_sub</name>
+  <version>0.2.0</version>
+  <description>Package containing an example of the publish-subscribe mechanism in rclrs.</description>
+  <maintainer email="[email protected]">Esteve Fernandez</maintainer>
+  <license>Apache License 2.0</license>
+
+  <build_depend>rclrs</build_depend>
+  <build_depend>rosidl_runtime_rs</build_depend>
+  <build_depend>std_msgs</build_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_publisher.rs

@@ -0,0 +1,24 @@
+use anyhow::{Error, Result};
+use std::env;
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args())?;
+
+    let node = context.create_node("minimal_publisher")?;
+
+    let publisher =
+        node.create_publisher::<std_msgs::msg::String>("topic", rclrs::QOS_PROFILE_DEFAULT)?;
+
+    let mut message = std_msgs::msg::String::default();
+
+    let mut publish_count: u32 = 1;
+
+    while context.ok() {
+        message.data = format!("Hello, world! {}", publish_count);
+        println!("Publishing: [{}]", message.data);
+        publisher.publish(&message)?;
+        publish_count += 1;
+        std::thread::sleep(std::time::Duration::from_millis(500));
+    }
+    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())
+}

examples/minimal_client_service/src/minimal_subscriber.rs

@@ -0,0 +1,23 @@
+use std::env;
+
+use anyhow::{Error, Result};
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args())?;
+
+    let mut node = context.create_node("minimal_subscriber")?;
+
+    let mut num_messages: usize = 0;
+
+    let _subscription = node.create_subscription::<std_msgs::msg::String, _>(
+        "topic",
+        rclrs::QOS_PROFILE_DEFAULT,
+        move |msg: std_msgs::msg::String| {
+            num_messages += 1;
+            println!("I heard: '{}'", msg.data);
+            println!("(Got {} messages so far)", num_messages);
+        },
+    )?;
+
+    rclrs::spin(&node).map_err(|err| err.into())
+}

examples/minimal_pub_sub/Cargo.toml

@@ -13,6 +13,10 @@ path = "src/minimal_subscriber.rs"
 name = "minimal_publisher"
 path = "src/minimal_publisher.rs"
 
+[[bin]]
+name = "message_demo"
+path = "src/message_demo.rs"
+
 [dependencies]
 anyhow = {version = "1", features = ["backtrace"]}
 
@@ -24,3 +28,9 @@ version = "*"
 
 [dependencies.std_msgs]
 version = "*"
+
+[dependencies.rclrs_example_msgs]
+version = "*"
+
+[dependencies.example_interfaces]
+version = "*"

rclrs/src/future.rs

@@ -0,0 +1,35 @@
+/// Based on https://www.viget.com/articles/understanding-futures-in-rust-part-1/
+use core::marker::PhantomData;
+use parking_lot::Mutex;
+use std::future::Future;
+use std::pin::Pin;
+use std::sync::Arc;
+use std::task::Context;
+use std::task::Poll;
+
+#[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
+        }
+    }
+}

rclrs/src/lib.rs

@@ -22,35 +22,69 @@ pub use wait::*;
 use rcl_bindings::rcl_context_is_valid;
 use std::time::Duration;
 
+use std::pin::Pin;
+
+pub use rcl_bindings::rmw_request_id_t;
+
 /// 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 +104,57 @@ pub fn spin(node: &Node) -> Result<(), RclrsError> {
             };
         }
     }
-
     Ok(())
 }
+
+#[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) }
+}
+
+pub fn spin_until_future_complete<T: Unpin + Clone>(
+    node: &node::Node,
+    mut future: Arc<Mutex<Box<RclFuture<T>>>>,
+) -> Result<<future::RclFuture<T> as Future>::Output, RclReturnCode> {
+    let rclwaker = Arc::new(RclWaker {});
+    let waker = rclwaker_into_waker(Arc::into_raw(rclwaker));
+    let mut cx = std::task::Context::from_waker(&waker);
+
+    loop {
+        let context_valid = unsafe { rcl_context_is_valid(&mut *node.context.lock() as *mut _) };
+        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,
+            };
+        }
+    }
+}