Return a Vec of errors from spinning

Signed-off-by: Michael X. Grey <[email protected]>

Author: mxgrey

rclrs/src/error.rs

@@ -34,6 +34,33 @@ pub enum RclrsError {
     AlreadyAddedToWaitSet,
 }
 
+impl RclrsError {
+    /// Returns true if the error was due to a timeout, otherwise returns false.
+    pub fn is_timeout(&self) -> bool {
+        matches!(
+            self,
+            RclrsError::RclError {
+                code: RclReturnCode::Timeout,
+                ..
+            }
+        )
+    }
+
+    /// Returns true if the error was because a subscription, service, or client
+    /// take failed, otherwise returns false.
+    pub fn is_take_failed(&self) -> bool {
+        matches!(
+            self,
+            RclrsError::RclError {
+                code: RclReturnCode::SubscriptionTakeFailed
+                    | RclReturnCode::ServiceTakeFailed
+                    | RclReturnCode::ClientTakeFailed,
+                ..
+            }
+        )
+    }
+}
+
 impl Display for RclrsError {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
         match self {
@@ -355,27 +382,77 @@ impl ToResult for rcl_ret_t {
 
 /// A helper trait to disregard timeouts as not an error.
 pub trait RclrsErrorFilter {
+    /// Get the first error available, or Ok(()) if there are no errors.
+    fn first_error(self) -> Result<(), RclrsError>;
+
     /// If the result was a timeout error, change it to `Ok(())`.
-    fn timeout_ok(self) -> Result<(), RclrsError>;
+    fn timeout_ok(self) -> Self;
+
+    /// If a subscription, service, or client take failed, change the result
+    /// to be `Ok(())`.
+    fn take_failed_ok(self) -> Self;
+
+    /// Some error types just indicate an early termination but do not indicate
+    /// that anything in the system has misbehaved. This filters out anything
+    /// that is part of the normal operation of rcl.
+    fn ignore_non_errors(self) -> Self
+    where
+        Self: Sized,
+    {
+        self.timeout_ok().take_failed_ok()
+    }
 }
 
 impl RclrsErrorFilter for Result<(), RclrsError> {
+    fn first_error(self) -> Result<(), RclrsError> {
+        self
+    }
+
     fn timeout_ok(self) -> Result<(), RclrsError> {
         match self {
             Ok(()) => Ok(()),
             Err(err) => {
-                if matches!(
-                    err,
-                    RclrsError::RclError {
-                        code: RclReturnCode::Timeout,
-                        ..
-                    }
-                ) {
-                    return Ok(());
+                if err.is_timeout() {
+                    Ok(())
+                } else {
+                    Err(err)
                 }
+            }
+        }
+    }
 
-                Err(err)
+    fn take_failed_ok(self) -> Result<(), RclrsError> {
+        match self {
+            Err(err) => {
+                if err.is_take_failed() {
+                    // Spurious wakeup - this may happen even when a waitset indicated that
+                    // work was ready, so we won't report it as an error
+                    Ok(())
+                } else {
+                    Err(err)
+                }
             }
+            other => other,
+        }
+    }
+}
+
+impl RclrsErrorFilter for Vec<RclrsError> {
+    fn first_error(mut self) -> Result<(), RclrsError> {
+        if self.is_empty() {
+            return Ok(());
         }
+
+        Err(self.remove(0))
+    }
+
+    fn timeout_ok(mut self) -> Self {
+        self.retain(|err| !err.is_timeout());
+        self
+    }
+
+    fn take_failed_ok(mut self) -> Self {
+        self.retain(|err| !err.is_take_failed());
+        self
     }
 }

rclrs/src/executor.rs

@@ -30,18 +30,20 @@ impl Executor {
     /// [`SpinOptions`] can be used to automatically stop the spinning when
     /// certain conditions are met. Use `SpinOptions::default()` to allow the
     /// Executor to keep spinning indefinitely.
-    pub fn spin(&mut self, options: SpinOptions) -> Result<(), RclrsError> {
+    pub fn spin(&mut self, options: SpinOptions) -> Vec<RclrsError> {
         loop {
             if self.nodes_mtx.lock().unwrap().is_empty() {
                 // Nothing to spin for, so just quit here
-                return Ok(());
+                return Vec::new();
             }
 
-            self.spin_once(options.timeout)?;
+            if let Err(err) = self.spin_once(options.timeout) {
+                return vec![err];
+            }
 
             if options.only_next_available_work {
                 // We were only suppposed to spin once, so quit here
-                return Ok(());
+                return Vec::new();
             }
 
             std::thread::yield_now();

rclrs/src/parameter/service.rs

@@ -446,6 +446,7 @@ mod tests {
                 executor
                     .spin(SpinOptions::spin_once().timeout(Duration::ZERO))
                     .timeout_ok()
+                    .first_error()
                     .unwrap();
 
                 *inner_done.read().unwrap()
@@ -589,6 +590,7 @@ mod tests {
                 executor
                     .spin(SpinOptions::spin_once().timeout(Duration::ZERO))
                     .timeout_ok()
+                    .first_error()
                     .unwrap();
 
                 *inner_done.read().unwrap()
@@ -825,6 +827,7 @@ mod tests {
                 executor
                     .spin(SpinOptions::spin_once().timeout(Duration::ZERO))
                     .timeout_ok()
+                    .first_error()
                     .unwrap();
 
                 *inner_done.read().unwrap()