From f891b24c93158184c3760e757dc0f5301bb7e3c0 Mon Sep 17 00:00:00 2001
From: carter <carterjschultz@gmail.com>
Date: Fri, 5 Jul 2024 11:23:02 -0600
Subject: [PATCH 1/2] Very basic error handling for services, not good

---
 roslibrust/src/ros1/service_client.rs | 19 ++++++++++--
 roslibrust/src/ros1/service_server.rs | 44 ++++++++++++++++++++++++++-
 2 files changed, 59 insertions(+), 4 deletions(-)

diff --git a/roslibrust/src/ros1/service_client.rs b/roslibrust/src/ros1/service_client.rs
index 4a04dac..2fd006f 100644
--- a/roslibrust/src/ros1/service_client.rs
+++ b/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}"),
             ))
         }
     }
diff --git a/roslibrust/src/ros1/service_server.rs b/roslibrust/src/ros1/service_server.rs
index 37399c4..964fb98 100644
--- a/roslibrust/src/ros1/service_server.rs
+++ b/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,41 @@ 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());
+        panic!()
+    }
 }

From bd04d8bbc66644e912894af73330b2eba361b6c9 Mon Sep 17 00:00:00 2001
From: carter <carterjschultz@gmail.com>
Date: Fri, 5 Jul 2024 11:30:06 -0600
Subject: [PATCH 2/2] Remove test panic

---
 roslibrust/src/ros1/service_server.rs | 1 -
 1 file changed, 1 deletion(-)

diff --git a/roslibrust/src/ros1/service_server.rs b/roslibrust/src/ros1/service_server.rs
index 964fb98..20e68d3 100644
--- a/roslibrust/src/ros1/service_server.rs
+++ b/roslibrust/src/ros1/service_server.rs
@@ -412,6 +412,5 @@ mod test {
             .await;
         debug!("Got call: {call:?}");
         assert!(call.is_err());
-        panic!()
     }
 }