From 052525e0d86d9db7f5894055fcb8069d8f48bfcc Mon Sep 17 00:00:00 2001
From: Romain Reignier <rre@robopec.com>
Date: Thu, 4 Jan 2018 09:45:49 +0100
Subject: [PATCH] Modify service client to retrieve if the service call has
 succeeded

---
 src/ros_lib/ros/service_client.h | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/ros_lib/ros/service_client.h b/src/ros_lib/ros/service_client.h
index 2b578fd..b6169ee 100644
--- a/src/ros_lib/ros/service_client.h
+++ b/src/ros_lib/ros/service_client.h
@@ -49,7 +49,7 @@ namespace ros {
     public:
       typedef typename MType::Request MReq;
       typedef typename MType::Response MRes;
-      typedef std::function<void(const MRes&)> CallbackT;
+      typedef std::function<void(const MRes&, bool)> CallbackT;
 
       ServiceClient(const char* topic_name) : 
         pub(topic_name, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)
@@ -74,11 +74,18 @@ namespace ros {
       // these refer to the subscriber
       void callback(unsigned char *data) override{
         MRes ret;
-        ret.deserialize(data);
+        // First byte give us the success status
+        bool success = data[0];
+
+        if(success)
+        {
+            // Access from the second byte
+            ret.deserialize(data+1);
+        }
         waiting = false;
         if(cb_)
         {
-          cb_(ret);
+          cb_(ret, success);
         }
         cb_ = nullptr;
       }