Service header and body should be a single item in the send queue (ROS2)

Author: LaurieCheers-unity

ros_tcp_endpoint/tcp_sender.py

@@ -80,9 +80,9 @@ def send_ros_service_response(self, srv_id, destination, response):
         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:
@@ -101,9 +101,9 @@ def send_unity_service_request(self, topic, service_class, request):
 
         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