Experimenting with only taking from services and clients in the same thread as the wait set

mxgrey · mxgrey · commit 8f0f192f387b · 2024-10-14T13:55:04.000+08:00
Signed-off-by: Michael X. Grey &lt;grey@openrobotics.org&gt;
diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs
@@ -1,18 +1,16 @@
 use std::{
-    boxed::Box,
     ffi::CString,
     sync::{Arc, Mutex, MutexGuard},
+    collections::HashMap,
 };
 
-use futures::channel::mpsc::{UnboundedSender, unbounded};
-
 use rosidl_runtime_rs::Message;
 
 use crate::{
     error::ToResult,
     rcl_bindings::*,
-    MessageCow, Node, RclrsError, Promise, ENTITY_LIFECYCLE_MUTEX,
-    ExecutorCommands, Executable, QoSProfile, Waitable, WaitableLifecycle,
+    MessageCow, Node, RclrsError, RclReturnCode, Promise, ENTITY_LIFECYCLE_MUTEX,
+    Executable, QoSProfile, Waitable, WaitableLifecycle,
     ExecutableHandle, ExecutableKind, ServiceInfo,
 };
 
@@ -25,9 +23,6 @@ pub use client_callback::*;
 mod client_output;
 pub use client_output::*;
 
-mod client_task;
-use client_task::*;
-
 /// Main class responsible for sending requests to a ROS service.
 ///
 /// The only available way to instantiate clients is via [`Node::create_client`][1], this is to
@@ -40,7 +35,7 @@ where
     T: rosidl_runtime_rs::Service,
 {
     handle: Arc<ClientHandle>,
-    action: UnboundedSender<ClientAction<T>>,
+    board: Arc<Mutex<ClientRequestBoard<T>>>,
     #[allow(unused)]
     lifecycle: WaitableLifecycle,
 }
@@ -86,10 +81,9 @@ where
         }
         .ok()?;
 
+        println!("vvvvvvvvv Sent client request {sequence_number} vvvvvvvvvvvv");
         // TODO(@mxgrey): Log errors here when logging becomes available.
-        self.action.unbounded_send(
-            ClientAction::NewRequest { sequence_number, sender }
-        ).ok();
+        self.board.lock().unwrap().new_request(sequence_number, sender);
 
         Ok(promise)
     }
@@ -231,39 +225,39 @@ where
         });
 
         let commands = node.commands();
-
-        let (action, receiver) = unbounded();
-        let _ = commands.run(client_task(receiver, Arc::clone(&handle)));
+        let board = Arc::new(Mutex::new(ClientRequestBoard::new()));
 
         let (waitable, lifecycle) = Waitable::new(
             Box::new(ClientExecutable {
                 handle: Arc::clone(&handle),
-                action: action.clone(),
+                board: Arc::clone(&board),
             }),
             Some(Arc::clone(&commands.get_guard_condition())),
         );
         commands.add_to_wait_set(waitable);
 
         Ok(Arc::new(Self {
             handle,
-            action,
+            board,
             lifecycle,
         }))
     }
 }
 
-struct ClientExecutable<T: rosidl_runtime_rs::Service> {
+struct ClientExecutable<T>
+where
+    T: rosidl_runtime_rs::Service,
+{
     handle: Arc<ClientHandle>,
-    action: UnboundedSender<ClientAction<T>>
+    board: Arc<Mutex<ClientRequestBoard<T>>>
 }
 
 impl<T> Executable for ClientExecutable<T>
 where
     T: rosidl_runtime_rs::Service,
 {
     fn execute(&mut self) -> Result<(), RclrsError> {
-        self.action.unbounded_send(ClientAction::TakeResponse).ok();
-        Ok(())
+        self.board.lock().unwrap().execute(&self.handle)
     }
 
     fn handle(&self) -> ExecutableHandle {
@@ -275,6 +269,107 @@ where
     }
 }
 
+type SequenceNumber = i64;
+
+/// This is used internally to monitor the state of active requests, as well as
+/// responses that have arrived without a known request.
+struct ClientRequestBoard<T>
+where
+    T: rosidl_runtime_rs::Service,
+{
+    // This stores all active requests that have not received a response yet
+    active_requests: HashMap<SequenceNumber, AnyClientOutputSender<T::Response>>,
+    // This holds responses that came in when no active request matched the
+    // sequence number. This could happen if take_response is triggered before
+    // the new_request for the same sequence number. That is extremely unlikely
+    // to ever happen but is theoretically possible on systems that may exhibit
+    // very strange CPU scheduling patterns, so we should account for it.
+    loose_responses: HashMap<SequenceNumber, (T::Response, rmw_service_info_t)>,
+}
+
+impl<T> ClientRequestBoard<T>
+where
+    T: rosidl_runtime_rs::Service,
+{
+    fn new() -> Self {
+        Self {
+            active_requests: Default::default(),
+            loose_responses: Default::default(),
+        }
+    }
+
+    fn new_request(
+        &mut self,
+        sequence_number: SequenceNumber,
+        sender: AnyClientOutputSender<T::Response>,
+    ) {
+        if let Some((response, info)) = self.loose_responses.remove(&sequence_number) {
+            // Weirdly the response for this request already arrived, so we'll
+            // send it off immediately.
+            sender.send_response(response, info);
+        } else {
+            self.active_requests.insert(sequence_number, sender);
+        }
+    }
+
+    fn execute(&mut self, handle: &Arc<ClientHandle>) -> Result<(), RclrsError> {
+        match self.take_response(handle) {
+            Ok((response, info)) => {
+                let seq = info.request_id.sequence_number;
+                if let Some(sender) = self.active_requests.remove(&seq) {
+                    dbg!();
+                    println!("Received response for {info:?}");
+                    // The active request is available, so send this response off
+                    sender.send_response(response, info);
+                } else {
+                    dbg!();
+                    println!("Received loose response for {info:?}");
+                    // Weirdly there isn't an active request for this, so save
+                    // it in the loose responses map.
+                    self.loose_responses.insert(seq, (response, info));
+                }
+            }
+            Err(err) => {
+                match err {
+                    RclrsError::RclError { code: RclReturnCode::ClientTakeFailed, .. } => {
+                        // This is okay, it means a spurious wakeup happened
+                        dbg!();
+                        println!("Spurious wakeup for client");
+                    }
+                    err => {
+                        dbg!();
+                        // TODO(@mxgrey): Log the error here once logging is available
+                        eprintln!("Error while taking a response for a client: {err}");
+                    }
+                }
+            }
+        }
+        Ok(())
+    }
+
+    fn take_response(
+        &self,
+        handle: &Arc<ClientHandle>,
+    ) -> Result<(T::Response, rmw_service_info_t), RclrsError> {
+        let mut service_info_out = ServiceInfo::zero_initialized_rmw();
+        let mut response_out = <T::Response as Message>::RmwMsg::default();
+        let handle = &*handle.lock();
+        unsafe {
+            // SAFETY: The three pointers are all kept valid by the handle
+            rcl_take_response_with_info(
+                handle,
+                &mut service_info_out,
+                &mut response_out as *mut <T::Response as Message>::RmwMsg as *mut _,
+            )
+        }
+        .ok()
+        .map(|_| (
+            T::Response::from_rmw_message(response_out),
+            service_info_out,
+        ))
+    }
+}
+
 /// Manage the lifecycle of an `rcl_client_t`, including managing its dependencies
 /// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
 /// [dropped after][1] the `rcl_client_t`.
diff --git a/rclrs/src/client/client_task.rs b/rclrs/src/client/client_task.rs
@@ -54,10 +54,12 @@ pub(super) async fn client_task<T: Service>(
                         let seq = info.request_id.sequence_number;
                         if let Some(sender) = active_requests.remove(&seq) {
                             dbg!();
+                            println!("Received response for {info:?}");
                             // The active request is available, so send this response off
                             sender.send_response(response, info);
                         } else {
                             dbg!();
+                            println!("Received loose response for {info:?}");
                             // Weirdly there isn't an active request for this, so save
                             // it in the loose responses map.
                             loose_responses.insert(seq, (response, info));
@@ -68,6 +70,7 @@ pub(super) async fn client_task<T: Service>(
                             RclrsError::RclError { code: RclReturnCode::ClientTakeFailed, .. } => {
                                 // This is okay, it means a spurious wakeup happened
                                 dbg!();
+                                println!("Spurious wakeup for client");
                             }
                             err => {
                                 dbg!();
diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs
@@ -4,12 +4,10 @@ use std::{
     sync::{Arc, Mutex, MutexGuard},
 };
 
-use futures::channel::mpsc::{unbounded, UnboundedSender};
-
 use crate::{
     error::ToResult,
     rcl_bindings::*,
-    NodeHandle, RclrsError, Waitable, WaitableLifecycle, GuardCondition, QoSProfile,
+    NodeHandle, RclrsError, Waitable, WaitableLifecycle, QoSProfile,
     Executable, ExecutableKind, ExecutableHandle, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands,
 };
 
@@ -25,9 +23,6 @@ pub use service_callback::*;
 mod service_info;
 pub use service_info::*;
 
-mod service_task;
-use service_task::*;
-
 /// Struct for responding to requests sent by ROS service clients.
 ///
 /// The only way to instantiate a service is via [`Node::create_service()`][1]
@@ -45,11 +40,8 @@ where
 {
     /// This handle is used to access the data that rcl holds for this service.
     handle: Arc<ServiceHandle>,
-    /// This allows us to replace the callback in the service task.
-    ///
-    /// Holding onto this sender will keep the service task alive. Once this
-    /// sender is dropped, the service task will end itself.
-    action: UnboundedSender<ServiceAction<T>>,
+    /// This is the callback that will be executed each time a request arrives.
+    callback: Arc<Mutex<AnyServiceCallback<T>>>,
     /// Holding onto this keeps the waiter for this service alive in the wait
     /// set of the executor.
     lifecycle: WaitableLifecycle,
@@ -84,7 +76,7 @@ where
     ) {
         let callback = callback.into_service_callback();
         // TODO(@mxgrey): Log any errors here when logging becomes available
-        self.action.unbounded_send(ServiceAction::SetCallback(callback)).ok();
+        *self.callback.lock().unwrap() = callback;
     }
 
     /// Set the callback of this service, replacing the callback that was
@@ -96,8 +88,7 @@ where
         callback: impl ServiceAsyncCallback<T, Args>,
     ) {
         let callback = callback.into_service_async_callback();
-        // TODO(@mxgrey): Log any errors here when logging becomes available.
-        self.action.unbounded_send(ServiceAction::SetCallback(callback)).ok();
+        *self.callback.lock().unwrap() = callback;
     }
 
     /// Used by [`Node`][crate::Node] to create a new service
@@ -108,35 +99,7 @@ where
         node_handle: &Arc<NodeHandle>,
         commands: &Arc<ExecutorCommands>,
     ) -> Result<Arc<Self>, RclrsError> {
-        let (sender, receiver) = unbounded();
-        let (service, waitable) = Self::new(
-            topic,
-            qos,
-            sender,
-            Arc::clone(&node_handle),
-            Arc::clone(commands.get_guard_condition()),
-        )?;
-
-        let _ = commands.run(service_task(
-            callback,
-            receiver,
-            Arc::clone(&service.handle),
-            Arc::clone(commands),
-        ));
-
-        commands.add_to_wait_set(waitable);
-
-        Ok(service)
-    }
-
-    /// Instantiate the service.
-    fn new(
-        topic: &str,
-        qos: QoSProfile,
-        action: UnboundedSender<ServiceAction<T>>,
-        node_handle: Arc<NodeHandle>,
-        guard_condition: Arc<GuardCondition>,
-    ) -> Result<(Arc<Self>, Waitable), RclrsError> {
+        let callback = Arc::new(Mutex::new(callback));
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_service = unsafe { rcl_get_zero_initialized_service() };
         let type_support = <T as rosidl_runtime_rs::Service>::get_type_support()
@@ -174,34 +137,40 @@ where
 
         let handle = Arc::new(ServiceHandle {
             rcl_service: Mutex::new(rcl_service),
-            node_handle,
+            node_handle: Arc::clone(&node_handle),
         });
 
-        let (waiter, lifecycle) = Waitable::new(
+        let (waitable, lifecycle) = Waitable::new(
             Box::new(ServiceExecutable {
                 handle: Arc::clone(&handle),
-                action: action.clone(),
+                callback: Arc::clone(&callback),
+                commands: Arc::clone(&commands),
             }),
-            Some(guard_condition),
+            Some(Arc::clone(commands.get_guard_condition())),
         );
 
-        let service = Arc::new(Self { handle, action, lifecycle });
+        let service = Arc::new(Self { handle, callback, lifecycle });
+        commands.add_to_wait_set(waitable);
 
-        Ok((service, waiter))
+        Ok(service)
     }
 }
 
 struct ServiceExecutable<T: rosidl_runtime_rs::Service> {
     handle: Arc<ServiceHandle>,
-    action: UnboundedSender<ServiceAction<T>>,
+    callback: Arc<Mutex<AnyServiceCallback<T>>>,
+    commands: Arc<ExecutorCommands>,
 }
 
 impl<T> Executable for ServiceExecutable<T>
 where
     T: rosidl_runtime_rs::Service,
 {
     fn execute(&mut self) -> Result<(), RclrsError> {
-        self.action.unbounded_send(ServiceAction::Execute).ok();
+        if let Err(err) = self.callback.lock().unwrap().execute(&self.handle, &self.commands) {
+            // TODO(@mxgrey): Log the error here once logging is implemented
+            eprintln!("Error while executing a service callback: {err}");
+        }
         Ok(())
     }
 
diff --git a/rclrs/src/service/any_service_callback.rs b/rclrs/src/service/any_service_callback.rs
