Internal commands are serialized as Json, not as ros messages

Author: LaurieCheers-unity

ROS_Packages/ros_tcp_endpoint/config/params.yaml

@@ -1 +1 @@
-ROS_IP: 127.0.0.1
+ROS_IP: 0.0.0.0

ROS_Packages/ros_tcp_endpoint/src/ros_tcp_endpoint/client.py

@@ -18,6 +18,7 @@
 from io import BytesIO
 
 import threading
+import json
 
 from .exceptions import TopicOrServiceNameDoesNotExistError
 from unity_interfaces.msg import RosUnitySrvMessage
@@ -145,6 +146,43 @@ def serialize_message(destination, message):
 
         return serialized_message
 
+    @staticmethod
+    def serialize_command(command, params):
+        cmd_bytes = command.encode("utf-8")
+        cmd_length = len(cmd_bytes)
+        cmd_info = struct.pack("<I%ss" % cmd_length, cmd_length, cmd_bytes)
+
+        json_bytes = json.dumps(params.__dict__).encode("utf-8")
+        json_length = len(json_bytes)
+        json_info = struct.pack("<I%ss" % json_length, json_length, json_bytes)
+
+        return cmd_info + json_info
+
+    def send_ros_service_request(self, srv_id, destination, data):
+        if destination not in self.tcp_server.source_destination_dict.keys():
+            error_msg = "Service destination '{}' is not registered! Known topics are: {} ".format(
+                destination, self.tcp_server.source_destination_dict.keys()
+            )
+            self.tcp_server.send_unity_error(error_msg)
+            rospy.logerr(error_msg)
+            # TODO: send a response to Unity anyway?
+            return
+        else:
+            ros_communicator = self.tcp_server.source_destination_dict[
+                destination
+            ]
+            response = ros_communicator.send(data)
+            if not response:
+                error_msg = "No response data from service '{}'!".format(
+                    destination
+                )
+                self.tcp_server.send_unity_error(error_msg)
+                rospy.logerr(error_msg)
+                # TODO: send a response to Unity anyway?
+                return
+
+        self.tcp_server.unity_tcp_sender.send_ros_service_response(srv_id, destination, response)
+
     def run(self):
         """
         Read a message and determine where to send it based on the source_destination_dict
@@ -166,51 +204,20 @@ def run(self):
         try:
             while not halt_event.is_set():
                 destination, data = self.read_message(self.conn)
-
-                if destination == "":
+                
+                if self.tcp_server.pending_srv_id is not None:
+                    # if we've been told that the next message will be a service request/response, process it as such
+                    if self.tcp_server.pending_srv_is_request:
+                        self.send_ros_service_request(self.tcp_server.pending_srv_id, destination, data)
+                    else:
+                        self.tcp_server.send_unity_service_response(self.tcp_server.pending_srv_id, data)
+                    self.tcp_server.pending_srv_id = None
+                elif destination == "":
                     # ignore this keepalive message, listen for more
                     pass
-                elif destination == "__syscommand":
+                elif destination.startswith("__"):
                     # handle a system command, such as registering new topics
-                    self.tcp_server.handle_syscommand(data)
-                elif destination == "__srv":
-                    # handle a ros service message request, or a unity service message response
-                    srv_message = RosUnitySrvMessage().deserialize(data)
-                    if not srv_message.is_request:
-                        self.tcp_server.send_unity_service_response(
-                            srv_message.srv_id, srv_message.payload
-                        )
-                        continue
-                    elif srv_message.topic == "__topic_list":
-                        response = self.tcp_server.topic_list(data)
-                    elif srv_message.topic not in self.tcp_server.source_destination_dict.keys():
-                        error_msg = "Service destination '{}' is not registered! Known topics are: {} ".format(
-                            srv_message.topic, self.tcp_server.source_destination_dict.keys()
-                        )
-                        self.tcp_server.send_unity_error(error_msg)
-                        rospy.logerr(error_msg)
-                        # TODO: send a response to Unity anyway?
-                        continue
-                    else:
-                        ros_communicator = self.tcp_server.source_destination_dict[
-                            srv_message.topic
-                        ]
-                        response = ros_communicator.send(srv_message.payload)
-                        if not response:
-                            error_msg = "No response data from service '{}'!".format(
-                                srv_message.topic
-                            )
-                            self.tcp_server.send_unity_error(error_msg)
-                            rospy.logerr(error_msg)
-                            # TODO: send a response to Unity anyway?
-                            continue
-
-                    serial_response = BytesIO()
-                    response.serialize(serial_response)
-                    response_message = RosUnitySrvMessage(
-                        srv_message.srv_id, False, "", serial_response.getvalue()
-                    )
-                    self.tcp_server.unity_tcp_sender.send_unity_message("__srv", response_message)
+                    self.tcp_server.handle_syscommand(destination, data)
                 elif destination in self.tcp_server.source_destination_dict:
                     ros_communicator = self.tcp_server.source_destination_dict[destination]
                     response = ros_communicator.send(data)

ROS_Packages/ros_tcp_endpoint/src/ros_tcp_endpoint/server.py

@@ -61,6 +61,8 @@ def __init__(self, node_name, buffer_size=1024, connections=2, tcp_ip="", tcp_po
         self.buffer_size = buffer_size
         self.connections = connections
         self.syscommands = SysCommands(self)
+        self.pending_srv_id = None
+        self.pending_srv_is_request = False
 
     def start(self, source_destination_dict=None):
         if source_destination_dict is not None:
@@ -96,27 +98,26 @@ def send_unity_message(self, topic, message):
         self.unity_tcp_sender.send_unity_message(topic, message)
 
     def send_unity_service(self, topic, service_class, request):
-        return self.unity_tcp_sender.send_unity_service(topic, service_class, request)
+        return self.unity_tcp_sender.send_unity_service_request(topic, service_class, request)
 
     def send_unity_service_response(self, srv_id, data):
         self.unity_tcp_sender.send_unity_service_response(srv_id, data)
 
     def topic_list(self, data):
         return RosUnityTopicListResponse(self.source_destination_dict.keys())
 
-    def handle_syscommand(self, data):
-        message = RosUnitySysCommand().deserialize(data)
-        function = getattr(self.syscommands, message.command)
+    def handle_syscommand(self, topic, data):
+        function = getattr(self.syscommands, topic[2:])
         if function is None:
             self.send_unity_error(
-                "Don't understand SysCommand.'{}'({})".format(message.command, message.params_json)
+                "Don't understand SysCommand.'{}'".format(topic)
             )
             return
         else:
-            params = json.loads(message.params_json)
+            message_json = data.decode("utf-8")
+            params = json.loads(message_json)
             function(**params)
 
-
 class SysCommands:
     def __init__(self, tcp_server):
         self.tcp_server = tcp_server
@@ -222,6 +223,15 @@ def unity_service(self, topic, message_name):
         self.tcp_server.source_destination_dict[topic] = UnityService(
             topic.encode("ascii"), message_class, self.tcp_server
         )
+        
+    def response(self, srv_id): # the next message is a service response
+        self.tcp_server.pending_srv_id = srv_id 
+        self.tcp_server.pending_srv_is_request = False
+
+    def request(self, srv_id): # the next message is a service request
+        self.tcp_server.pending_srv_id = srv_id 
+        self.tcp_server.pending_srv_is_request = True
+
 
 
 def resolve_message_name(name, extension="msg"):

ROS_Packages/ros_tcp_endpoint/src/ros_tcp_endpoint/tcp_sender.py

@@ -34,7 +34,7 @@
 
 class UnityTcpSender:
     """
-    Connects and sends messages to the server on the Unity side.
+    Sends messages to Unity.
     """
 
     def __init__(self):
@@ -51,29 +51,55 @@ def __init__(self):
         self.srv_lock = threading.Lock()
         self.services_waiting = {}
 
-    def send_unity_error(self, error):
-        self.send_unity_message("__error", RosUnityError(error))
+    def send_unity_info(self, text):
+        if self.queue is not None:
+            command = SysCommand_Log()
+            command.text = text
+            serialized_bytes = ClientThread.serialize_command("__log", command)
+            self.queue.put(serialized_bytes)
+
+    def send_unity_warning(self, text):
+        if self.queue is not None:
+            command = SysCommand_Log()
+            command.text = text
+            serialized_bytes = ClientThread.serialize_command("__warn", command)
+            self.queue.put(serialized_bytes)
+
+    def send_unity_error(self, text):
+        if self.queue is not None:
+            command = SysCommand_Log()
+            command.text = text
+            serialized_bytes = ClientThread.serialize_command("__error", command)
+            self.queue.put(serialized_bytes)
+
+    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)
 
     def send_unity_message(self, topic, message):
-        serialized_message = ClientThread.serialize_message(topic, message)
         if self.queue is not None:
+            serialized_message = ClientThread.serialize_message(topic, message)
             self.queue.put(serialized_message)
 
-    def send_unity_service(self, topic, service_class, request):
-        request_bytes = BytesIO()
-        request.serialize(request_bytes)
+    def send_unity_service_request(self, topic, service_class, request):
+        if self.queue is None:
+            return None
+        
         thread_pauser = ThreadPauser()
-
         with self.srv_lock:
             srv_id = self.next_srv_id
             self.next_srv_id += 1
             self.services_waiting[srv_id] = thread_pauser
 
-        payload = request_bytes.getvalue()
-        srv_message = RosUnitySrvMessage(srv_id, True, topic, payload)
-        serialized_message = ClientThread.serialize_message("__srv", srv_message)
-        if self.queue is not None:
-            self.queue.put(serialized_message)
+        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)
 
         # rospy starts a new thread for each service request,
         # so it won't break anything if we sleep now while waiting for the response
@@ -130,3 +156,9 @@ def sender_loop(self, conn, tid, halt_event):
             with self.queue_lock:
                 if self.queue is local_queue:
                     self.queue = None
+
+class SysCommand_Log:
+    text = ""
+
+class SysCommand_Service:
+    srv_id = 0