AIRO-1647 Service header and body should be a single item in the send queue (#126)

* Send service response as one queue item

Co-authored-by: Hamid Younesy <hyounesy@users.noreply.github.com>

src/ros_tcp_endpoint/tcp_sender.py

@@ -76,9 +76,9 @@ class UnityTcpSender:
         if self.queue is not None:
             command = SysCommand_Service()
             command.srv_id = srv_id
-            serialized_bytes = ClientThread.serialize_command("__response", command)
-            self.queue.put(serialized_bytes)
-            self.send_unity_message(destination, response)
+            serialized_header = ClientThread.serialize_command("__response", command)
+            serialized_message = ClientThread.serialize_message(destination, response)
+            self.queue.put(b"".join([serialized_header, serialized_message]))
 
     def send_unity_message(self, topic, message):
         if self.queue is not None:
@@ -97,9 +97,9 @@ class UnityTcpSender:
 
         command = SysCommand_Service()
         command.srv_id = srv_id
-        serialized_bytes = ClientThread.serialize_command("__request", command)
-        self.queue.put(serialized_bytes)
-        self.send_unity_message(topic, request)
+        serialized_header = ClientThread.serialize_command("__request", command)
+        serialized_message = ClientThread.serialize_message(topic, request)
+        self.queue.put(b"".join([serialized_header, serialized_message]))
 
         # rospy starts a new thread for each service request,
         # so it won't break anything if we sleep now while waiting for the response