Unity server update (#8)

ROSConnection runs a single listen server, auto determine IP

Author: LaurieCheers-unity

tcp_endpoint/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -47,18 +48,16 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  RosUnityError.msg
+)
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  RosUnityHandshake.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -68,10 +67,10 @@ catkin_python_setup()
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -106,6 +105,7 @@ catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES tcp_endpoint
 #  CATKIN_DEPENDS roscpp rospy std_msgs
+CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 

tcp_endpoint/msg/RosUnityError.msg

@@ -0,0 +1 @@
+string message

tcp_endpoint/package.xml

@@ -43,7 +43,7 @@
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
+     <exec_depend>message_runtime</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->

tcp_endpoint/src/tcp_endpoint/RosSubscriber.py

@@ -12,7 +12,7 @@ class RosSubscriber(RosReceiver):
     Class to send messages outside of ROS network
     """
 
-    def __init__(self, topic, message_class, server_ip, server_port, queue_size=10):
+    def __init__(self, topic, message_class, tcp_server, queue_size=10):
         """
 
         Args:
@@ -23,8 +23,7 @@ def __init__(self, topic, message_class, server_ip, server_port, queue_size=10):
         self.topic = topic
         self.node_name = "{}_subsciber".format(topic)
         self.msg = message_class
-        self.tcp_id = server_ip
-        self.tcp_port = server_port
+        self.tcp_server = tcp_server
         self.queue_size = queue_size
 
         # Start Subscriber listener function
@@ -41,16 +40,7 @@ def send(self, data):
 
         """
 
-        serialized_message = ClientThread.serialize_message(self.topic, data)
-
-        try:
-            s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-            s.connect((self.tcp_id, self.tcp_port))
-            s.send(serialized_message)
-            s.close()
-        except Exception as e:
-            rospy.loginfo("Exception {}".format(e))
-
+        self.tcp_server.send_unity_message(self.topic, data)
         return self.msg
 
     def listener(self):

tcp_endpoint/src/tcp_endpoint/RosTCPClientThread.py

@@ -1,19 +1,20 @@
 #!/usr/bin/env python
 
 import struct
+import socket
+import rospy
 from io import BytesIO
 
 from threading import Thread
 
 from tcp_endpoint.TCPEndpointExceptions import TopicOrServiceNameDoesNotExistError
 
-
 class ClientThread(Thread):
     """
     Thread class to read all data from a connection and pass along the data to the
     desired source.
     """
-    def __init__(self, conn, source_destination_dict):
+    def __init__(self, conn, tcp_server, incoming_ip, incoming_port):
         """
         Set class variables
         Args:
@@ -22,7 +23,9 @@ def __init__(self, conn, source_destination_dict):
         """
         Thread.__init__(self)
         self.conn = conn
-        self.source_destination_dict = source_destination_dict
+        self.tcp_server = tcp_server
+        self.incoming_ip = incoming_ip
+        self.incoming_port = incoming_port
 
     def read_int32(self):
         """
@@ -130,14 +133,26 @@ def run(self):
             print("No data for a message size of {}, breaking!".format(full_message_size))
             return
 
-        if destination not in self.source_destination_dict.keys():
-            error_msg = "Topic/Service destination '{}' does not exist in source destination dictionary {} "\
-                .format(destination, self.source_destination_dict.keys())
+        if destination.startswith('__'):
+            if destination not in self.tcp_server.special_destination_dict.keys():
+                error_msg = "System message '{}' is undefined! Known system calls are: {}"\
+                    .format(destination, self.tcp_server.special_destination_dict.keys())
+                self.conn.close()
+                self.tcp_server.send_unity_error(error_msg)
+                raise TopicOrServiceNameDoesNotExistError(error_msg)
+            else:
+                ros_communicator = self.tcp_server.special_destination_dict[destination]
+                ros_communicator.set_incoming_ip(self.incoming_ip)
+        elif destination not in self.tcp_server.source_destination_dict.keys():
+            error_msg = "Topic/service destination '{}' is not defined! Known topics are: {} "\
+                .format(destination, self.tcp_server.source_destination_dict.keys())
             self.conn.close()
+            self.tcp_server.send_unity_error(error_msg)
             raise TopicOrServiceNameDoesNotExistError(error_msg)
+        else:
+            ros_communicator = self.tcp_server.source_destination_dict[destination]
 
         try:
-            ros_communicator = self.source_destination_dict[destination]
             response = ros_communicator.send(data)
 
             # Responses only exist for services

tcp_endpoint/src/tcp_endpoint/RosTCPServer.py

@@ -3,14 +3,16 @@
 import rospy
 import socket
 
+from tcp_endpoint.UnityTCPSender import UnityTCPSender
 from tcp_endpoint.RosTCPClientThread import ClientThread
+from tcp_endpoint.RosUnityHandshakeService import RosUnityHandshakeService
 
 class TCPServer:
     """
     Initializes ROS node and TCP server.
     """
 
-    def __init__(self, tcp_ip, tcp_port, node_name, source_destination_dict, buffer_size=1024, connections=10):
+    def __init__(self, node_name, buffer_size=1024, connections=10):
         """
         Initializes ROS node and class variables.
 
@@ -22,10 +24,18 @@ def __init__(self, tcp_ip, tcp_port, node_name, source_destination_dict, buffer_
             buffer_size:             The read buffer size used when reading from a socket
             connections:             Max number of queued connections. See Python Socket documentation
         """
-        self.tcp_ip = tcp_ip
-        self.tcp_port = tcp_port
+        self.tcp_ip = rospy.get_param("/ROS_IP")
+        self.tcp_port = rospy.get_param("/ROS_TCP_PORT", 10000)
+
+        unity_machine_ip = rospy.get_param("/UNITY_IP", '')
+        unity_machine_port = rospy.get_param("/UNITY_SERVER_PORT", 5005)
+        self.unity_tcp_sender = UnityTCPSender(unity_machine_ip, unity_machine_port)
+
         self.node_name = node_name
-        self.source_destination_dict = source_destination_dict
+        self.source_destination_dict = {}
+        self.special_destination_dict = {
+            '__handshake': RosUnityHandshakeService(self.unity_tcp_sender)
+        }
         self.buffer_size = buffer_size
         self.connections = connections
 
@@ -44,9 +54,15 @@ def start(self):
             tcp_server.listen(self.connections)
 
             (conn, (ip, port)) = tcp_server.accept()
-            new_thread = ClientThread(conn, self.source_destination_dict)
+            new_thread = ClientThread(conn, self, ip, port)
             new_thread.start()
             threads.append(new_thread)
 
         for t in threads:
             t.join()
+
+    def send_unity_error(self, error):
+        self.unity_tcp_sender.send_unity_error(error)
+
+    def send_unity_message(self, topic, message):
+        self.unity_tcp_sender.send_unity_message(topic, message)

tcp_endpoint/src/tcp_endpoint/RosUnityHandshakeService.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+
+import rospy
+from tcp_endpoint.srv import RosUnityHandshake, RosUnityHandshakeResponse
+
+class RosUnityHandshakeService:
+    """
+    Class to auto-detect Unity IP.
+    """
+    def __init__(self, tcp_sender):
+        """
+        Args:
+            tcp_sender:     sends messages to Unity
+        """
+        self.srv_class = RosUnityHandshake._request_class()
+        self.tcp_sender = tcp_sender
+        self.incoming_ip = ''
+
+    # The client thread lets us know what the incoming IP is, so we can use it later
+    def set_incoming_ip(self, ip):
+        self.incoming_ip = ip
+
+    def send(self, data):
+        message = self.srv_class.deserialize(data)
+        if message.ip == '': # if the message specifies an IP, Unity has set an IP override, so use it
+            self.tcp_sender.process_handshake(self.incoming_ip, message.port)
+        else: # if not, just talk back to the incoming IP
+            self.tcp_sender.process_handshake(message.ip, message.port)
+        return RosUnityHandshakeResponse(self.tcp_sender.unity_ip)

tcp_endpoint/src/tcp_endpoint/UnityTCPSender.py

@@ -0,0 +1,40 @@
+import rospy
+import socket
+from tcp_endpoint.RosTCPClientThread import ClientThread
+from tcp_endpoint.msg import RosUnityError
+
+class UnityTCPSender:
+    """
+    Connects and sends messages to the server on the Unity side.
+    """
+    def __init__(self, unity_ip, unity_port):
+        self.unity_ip = unity_ip
+        self.unity_port = unity_port
+        # if we have a valid IP at this point, it was overridden locally so always use that
+        self.ip_is_overridden = (self.unity_ip != '')
+
+    def process_handshake(self, ip, port):
+        self.unity_port = port
+        if ip != '' and not self.ip_is_overridden:
+            self.unity_ip = ip # hello Unity, we'll talk to you from now on
+        print("ROS-Unity Handshake received, will connect to {}:{}".format(self.unity_ip, self.unity_port))
+
+    def send_unity_error(self, error):
+        self.send_unity_message("__error", RosUnityError(error))
+
+    def send_unity_message(self, topic, message):
+        if self.unity_ip == '':
+            print("Can't send a message, no defined unity IP!".format(topic, message))
+            return
+
+        serialized_message = ClientThread.serialize_message(topic, message)
+
+        try:
+            s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            s.settimeout(2)
+            s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            s.connect((self.unity_ip, self.unity_port))
+            s.send(serialized_message)
+            s.close()
+        except Exception as e:
+            rospy.loginfo("Exception {}".format(e))

tcp_endpoint/srv/RosUnityHandshake.srv

@@ -0,0 +1,4 @@
+string ip
+uint16 port
+---
+string ip