Merge pull request #169 from Carter12s/service-server-errors-ros1

Add proper error handling for Ros1 Services

roslibrust/src/ros1/service_client.rs

@@ -214,10 +214,23 @@ impl ServiceClientLink {
 
             Ok(full_body)
         } else {
-            // MAJOR TODO: need to parse error from stream here!
+            let mut body_len_bytes = [0u8; 4];
+            let _body_bytes_read = stream.read_exact(&mut body_len_bytes).await?;
+            let body_len = u32::from_le_bytes(body_len_bytes) as usize;
+            let mut body = vec![0u8; body_len];
+            stream.read_exact(&mut body).await?;
+            let full_body = [body_len_bytes.to_vec(), body].concat();
+            let err_msg: String = serde_rosmsg::from_slice(&full_body).map_err(|err| {
+                log::error!("Failed to parse service call error message: {err}");
+                std::io::Error::new(
+                    std::io::ErrorKind::InvalidData,
+                    "Failed to parse service call error message",
+                )
+            })?;
+            // TODO probably specific error type for this
             Err(std::io::Error::new(
-                std::io::ErrorKind::InvalidData,
-                "Failure response from service:",
+                std::io::ErrorKind::Other,
+                format!("Failure response from service server: {err_msg}"),
             ))
         }
     }

roslibrust/src/ros1/service_server.rs

@@ -235,7 +235,12 @@ impl ServiceServerLink {
                 }
                 Err(e) => {
                     warn!("Error from user service method for {service_name}: {e:?}");
-                    // MAJOR TODO: respond with error
+
+                    let error_string = format!("{:?}", e);
+                    let error_bytes = serde_rosmsg::to_vec(&error_string).unwrap();
+                    let full_response = [vec![0u8], error_bytes].concat();
+
+                    stream.write_all(&full_response).await.unwrap();
                 }
             }
         }
@@ -372,4 +377,40 @@ mod test {
             .services
             .contains(&"/dropping_service_node/add_two".to_string()));
     }
+
+    #[test_log::test(tokio::test)]
+    async fn service_error_behavior() {
+        debug!("Getting node handle");
+        let nh = NodeHandle::new("http://localhost:11311", "/service_error_behavior")
+            .await
+            .unwrap();
+
+        let server_fn = |request: test_msgs::AddTwoIntsRequest| -> Result<
+            test_msgs::AddTwoIntsResponse,
+            Box<dyn std::error::Error + Send + Sync>,
+        > {
+            info!("Got request: {request:?}");
+            return Err(Box::new(std::io::Error::new(
+                std::io::ErrorKind::NotFound,
+                "test message",
+            )));
+        };
+
+        // Create the server
+        let _handle = nh
+            .advertise_service::<test_msgs::AddTwoInts, _>("~/add_two", server_fn)
+            .await
+            .unwrap();
+
+        // Make the request (should fail)
+        let client = nh
+            .service_client::<test_msgs::AddTwoInts>("~/add_two")
+            .await
+            .unwrap();
+        let call = client
+            .call(&test_msgs::AddTwoIntsRequest { a: 1, b: 2 })
+            .await;
+        debug!("Got call: {call:?}");
+        assert!(call.is_err());
+    }
 }