Merge from ros1

Author: LaurieCheers-unity

ros_tcp_endpoint/client.py

@@ -29,6 +29,7 @@ class ClientThread(threading.Thread):
     Thread class to read all data from a connection and pass along the data to the
     desired source.
     """
+
     def __init__(self, conn, tcp_server, incoming_ip, incoming_port):
         """
         Set class variables
@@ -66,7 +67,7 @@ def read_int32(conn):
 
         """
         raw_bytes = ClientThread.recvall(conn, 4)
-        num = struct.unpack('<I', raw_bytes)[0]
+        num = struct.unpack("<I", raw_bytes)[0]
         return num
 
     def read_string(self):
@@ -81,24 +82,24 @@ def read_string(self):
         str_len = ClientThread.read_int32(self.conn)
 
         str_bytes = ClientThread.recvall(self.conn, str_len)
-        decoded_str = str_bytes.decode('utf-8')
+        decoded_str = str_bytes.decode("utf-8")
 
         return decoded_str
 
-    def read_message(self):
+    def read_message(self, conn):
         """
         Decode destination and full message size from socket connection.
         Grab bytes in chunks until full message has been read.
         """
-        data = b''
+        data = b""
 
         destination = self.read_string()
-        full_message_size = ClientThread.read_int32(self.conn)
+        full_message_size = ClientThread.read_int32(conn)
 
         while len(data) < full_message_size:
             # Only grabs max of 1024 bytes TODO: change to TCPServer's buffer_size
             grab = 1024 if full_message_size - len(data) > 1024 else full_message_size - len(data)
-            packet = ClientThread.recvall(self.conn, grab)
+            packet = ClientThread.recvall(conn, grab)
 
             if not packet:
                 self.logerr("No packets...")
@@ -110,7 +111,7 @@ def read_message(self):
             self.logerr("No data for a message size of {}, breaking!".format(full_message_size))
             return
 
-        destination = destination.rstrip('\x00')
+        destination = destination.rstrip("\x00")
         return destination, data
 
     @staticmethod
@@ -125,13 +126,13 @@ def serialize_message(destination, message):
         Returns:
             serialized destination and message as a list of bytes
         """
-        dest_bytes = destination.encode('utf-8')
+        dest_bytes = destination.encode("utf-8")
         length = len(dest_bytes)
-        dest_info = struct.pack('<I%ss' % length, length, dest_bytes)
+        dest_info = struct.pack("<I%ss" % length, length, dest_bytes)
 
         serial_response = serialize_message(message)
 
-        msg_length = struct.pack('<I', len(serial_response))
+        msg_length = struct.pack("<I", len(serial_response))
         serialized_message = dest_info + msg_length + serial_response
 
         return serialized_message
@@ -147,7 +148,7 @@ def serialize_command(command, params):
         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(
@@ -159,17 +160,17 @@ def send_ros_service_request(self, srv_id, destination, data):
             return
         else:
             ros_communicator = self.tcp_server.source_destination_dict[destination]
-            service_thread = threading.Thread(target=self.service_call_thread, args=(srv_id, destination, data, ros_communicator))
+            service_thread = threading.Thread(
+                target=self.service_call_thread, args=(srv_id, destination, data, ros_communicator)
+            )
             service_thread.daemon = True
             service_thread.start()
 
     def service_call_thread(self, srv_id, destination, data, ros_communicator):
         response = ros_communicator.send(data)
-        
+
         if not response:
-            error_msg = "No response data from service '{}'!".format(
-                destination
-            )
+            error_msg = "No response data from service '{}'!".format(destination)
             self.tcp_server.send_unity_error(error_msg)
             self.logerr(error_msg)
             # TODO: send a response to Unity anyway?
@@ -182,7 +183,7 @@ def loginfo(self, text):
 
     def logerr(self, text):
         self.tcp_server.get_logger().error(text)
-        
+
     def run(self):
         """
         Read a message and determine where to send it based on the source_destination_dict
@@ -203,32 +204,37 @@ def run(self):
         self.tcp_server.unity_tcp_sender.start_sender(self.conn, halt_event)
         try:
             while not halt_event.is_set():
-                destination, data = self.read_message()
+                destination, data = self.read_message(self.conn)
 
                 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)
+                        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.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
+                    # ignore this keepalive message, listen for more
                     pass
                 elif destination.startswith("__"):
-                    #handle a system command, such as registering new topics
+                    # handle a system command, such as registering new topics
                     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)
                 else:
-                    error_msg = "Topic '{}' is not registered! Known topics are: {} "\
-                        .format(destination, self.tcp_server.source_destination_dict.keys())
+                    error_msg = "Topic '{}' is not registered! Known topics are: {} ".format(
+                        destination, self.tcp_server.source_destination_dict.keys()
+                    )
                     self.tcp_server.send_unity_error(error_msg)
                     self.logerr(error_msg)
         except IOError as e:
             self.logerr("Exception: {}".format(e))
         finally:
             halt_event.set()
             self.conn.close()
-            self.loginfo("Disconnected from {}".format(self.incoming_ip));
+            self.loginfo("Disconnected from {}".format(self.incoming_ip))

ros_tcp_endpoint/tcp_sender.py

@@ -32,12 +32,14 @@
     from Queue import Queue
     from Queue import Empty
 
+
 class UnityTcpSender:
     """
     Sends messages to Unity.
     """
+
     def __init__(self, tcp_server):
-        #super().__init__(f'UnityTcpSender')
+        # super().__init__(f'UnityTcpSender')
 
         self.sender_id = 1
         self.time_between_halt_checks = 5
@@ -46,7 +48,7 @@ def __init__(self, tcp_server):
         # Each sender thread has its own queue: this is always the queue for the currently active thread.
         self.queue = None
         self.queue_lock = threading.Lock()
-        
+
         # variables needed for matching up unity service requests with responses
         self.next_srv_id = 1001
         self.srv_lock = threading.Lock()
@@ -91,12 +93,12 @@ def send_unity_service_request(self, topic, service_class, request):
             return None
 
         thread_pauser = ThreadPauser()
-        
+
         with self.srv_lock:
             srv_id = self.next_srv_id
-            self.next_srv_id+=1
+            self.next_srv_id += 1
             self.services_waiting[srv_id] = thread_pauser
-        
+
         command = SysCommand_Service()
         command.srv_id = srv_id
         serialized_bytes = ClientThread.serialize_command("__request", command)
@@ -106,16 +108,16 @@ def send_unity_service_request(self, topic, service_class, 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
         thread_pauser.sleep_until_resumed()
-        
+
         response = deserialize_message(thread_pauser.result, service_class.Response())
         return response
-    
+
     def send_unity_service_response(self, srv_id, data):
         thread_pauser = None
         with self.srv_lock:
             thread_pauser = self.services_waiting[srv_id]
             del self.services_waiting[srv_id]
-        
+
         thread_pauser.resume_with_result(data)
 
     def send_topic_list(self):
@@ -126,32 +128,34 @@ def send_topic_list(self):
             topic_list.types = [item[1] for item in topics_and_types]
             serialized_bytes = ClientThread.serialize_command("__topic_list", topic_list)
             self.queue.put(serialized_bytes)
-            
+
     def start_sender(self, conn, halt_event):
-        sender_thread = threading.Thread(target=self.sender_loop, args=(conn, self.sender_id, halt_event))
+        sender_thread = threading.Thread(
+            target=self.sender_loop, args=(conn, self.sender_id, halt_event)
+        )
         self.sender_id += 1
 
         # Exit the server thread when the main thread terminates
         sender_thread.daemon = True
         sender_thread.start()
- 
+
     def sender_loop(self, conn, tid, halt_event):
         s = None
         local_queue = Queue()
-        local_queue.put(b'\0\0\0\0\0\0\0\0') # send an empty message to confirm connection
+        local_queue.put(b"\0\0\0\0\0\0\0\0")  # send an empty message to confirm connection
         with self.queue_lock:
             self.queue = local_queue
-        
+
         try:
             while not halt_event.is_set():
                 try:
                     item = local_queue.get(timeout=self.time_between_halt_checks)
                 except Empty:
-                    # I'd like to just wait on the queue, but we also need to check occasionally for the connection being closed 
+                    # I'd like to just wait on the queue, but we also need to check occasionally for the connection being closed
                     # (otherwise the thread never terminates.)
                     continue
-                
-                #print("Sender {} sending an item".format(tid))
+
+                # print("Sender {} sending an item".format(tid))
 
                 try:
                     conn.sendall(item)
@@ -164,12 +168,15 @@ def sender_loop(self, conn, tid, halt_event):
                 if self.queue is local_queue:
                     self.queue = None
 
+
 class SysCommand_Log:
     text = ""
 
+
 class SysCommand_Service:
     srv_id = 0
 
+
 class SysCommand_TopicsResponse:
     topics = []
-    types = [] 
+    types = []