* Unity can implement services (#26)

Author: LaurieCheers-unity

src/ros_tcp_endpoint/client.py

@@ -40,23 +40,25 @@ def __init__(self, conn, tcp_server, incoming_ip, incoming_port):
         self.incoming_ip = incoming_ip
         self.incoming_port = incoming_port
 
-    def read_int32(self):
+    @staticmethod
+    def read_int32(conn):
         """
         Reads four bytes from socket connection and unpacks them to an int
 
         Returns: int
 
         """
         try:
-            raw_bytes = self.conn.recv(4)
+            raw_bytes = conn.recv(4)
             num = struct.unpack('<I', raw_bytes)[0]
             return num
         except Exception as e:
             print("Unable to read integer from connection. {}".format(e))
 
         return None
 
-    def read_string(self):
+    @staticmethod
+    def read_string(conn):
         """
         Reads int32 from socket connection to determine how many bytes to
         read to get the string that follows. Read that number of bytes and
@@ -66,9 +68,9 @@ def read_string(self):
 
         """
         try:
-            str_len = self.read_int32()
+            str_len = ClientThread.read_int32(conn)
 
-            str_bytes = self.conn.recv(str_len)
+            str_bytes = conn.recv(str_len)
             decoded_str = str_bytes.decode('utf-8')
 
             return decoded_str
@@ -78,6 +80,34 @@ def read_string(self):
 
         return None
 
+    @staticmethod
+    def read_message(conn):
+        """
+        Decode destination and full message size from socket connection.
+        Grab bytes in chunks until full message has been read.
+        """
+        data = b''
+
+        destination = ClientThread.read_string(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 = conn.recv(grab)
+
+            if not packet:
+                print("No packets...")
+                break
+
+            data += packet
+
+        if not data:
+            print("No data for a message size of {}, breaking!".format(full_message_size))
+            return
+
+        return destination, data
+
     @staticmethod
     def serialize_message(destination, message):
         """
@@ -111,9 +141,7 @@ def serialize_message(destination, message):
 
     def run(self):
         """
-        Decode destination and full message size from socket connection.
-        Grab bytes in chunks until full message has been read.
-        Determine where to send the message based on the source_destination_dict
+        Read a message and determine where to send it based on the source_destination_dict
          and destination string. Then send the read message.
 
         If there is a response after sending the serialized data, assume it is a
@@ -126,25 +154,7 @@ def run(self):
             msg: the ROS msg type as bytes
 
         """
-        data = b''
-
-        destination = self.read_string()
-        full_message_size = self.read_int32()
-
-        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 = self.conn.recv(grab)
-
-            if not packet:
-                print("No packets...")
-                break
-
-            data += packet
-
-        if not data:
-            print("No data for a message size of {}, breaking!".format(full_message_size))
-            return
+        destination, data = self.read_message(self.conn)
 
         if destination == '__syscommand':
             self.tcp_server.handle_syscommand(data)

src/ros_tcp_endpoint/publisher.py

@@ -56,4 +56,4 @@ def unregister(self):
         Returns:
 
         """
-        self.pub.unregister()
+        self.pub.unregister()

src/ros_tcp_endpoint/server.py

@@ -30,7 +30,7 @@ class TcpServer:
     Initializes ROS node and TCP server.
     """
 
-    def __init__(self, node_name, buffer_size=1024, connections=10):
+    def __init__(self, node_name, tcp_ip="", tcp_port=-1, buffer_size=1024, connections=10):
         """
         Initializes ROS node and class variables.
 
@@ -39,8 +39,15 @@ def __init__(self, node_name, buffer_size=1024, connections=10):
             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 = rospy.get_param("/ROS_IP")
-        self.tcp_port = rospy.get_param("/ROS_TCP_PORT", 10000)
+        if tcp_ip != "":
+            self.tcp_ip = tcp_ip
+        else:
+            self.tcp_ip = rospy.get_param("/ROS_IP")
+            
+        if tcp_port != -1:
+            self.tcp_port = tcp_port
+        else:
+            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)
@@ -52,7 +59,9 @@ def __init__(self, node_name, buffer_size=1024, connections=10):
         self.connections = connections
         self.syscommands = SysCommands(self)
 
-    def start(self):
+    def start(self, source_destination_dict=None):
+        if source_destination_dict is not None:
+            self.source_destination_dict = source_destination_dict
         server_thread = threading.Thread(target=self.listen_loop)
         # Exit the server thread when the main thread terminates
         server_thread.daemon = True
@@ -83,6 +92,9 @@ def send_unity_error(self, error):
     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)
+
     def handle_syscommand(self, data):
         message = RosUnitySysCommand().deserialize(data)
         function = getattr(self.syscommands, message.command)

src/ros_tcp_endpoint/service.py

@@ -67,4 +67,4 @@ def unregister(self):
 
         """
         if not self.srv is None:
-            self.srv.close()
+            self.srv.close()

src/ros_tcp_endpoint/subscriber.py

@@ -62,4 +62,4 @@ def unregister(self):
 
         """
         if not self.sub is None:
-            self.sub.unregister()
+            self.sub.unregister()

src/ros_tcp_endpoint/tcp_sender.py

@@ -61,4 +61,27 @@ def send_unity_message(self, topic, message):
             s.send(serialized_message)
             s.close()
         except Exception as e:
-            rospy.loginfo("Exception {}".format(e))
+            rospy.loginfo("Exception {}".format(e))
+
+    def send_unity_service(self, topic, service_class, request):
+        if self.unity_ip == '':
+            print("Can't send a message, no defined unity IP!".format(topic, request))
+            return
+
+        serialized_message = ClientThread.serialize_message(topic, request)
+
+        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)
+
+            destination, data = ClientThread.read_message(s)
+
+            response = service_class._response_class().deserialize(data)
+
+            s.close()
+            return response
+        except Exception as e:
+            rospy.loginfo("Exception {}".format(e))

src/ros_tcp_endpoint/unity_service.py

@@ -0,0 +1,60 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rospy
+import socket
+
+from .communication import RosReceiver
+from .client import ClientThread
+
+
+class UnityService(RosReceiver):
+    """
+    Class to register a ROS service that's implemented in Unity.
+    """
+    def __init__(self, topic, service_class, tcp_server, queue_size=10):
+        """
+
+        Args:
+            topic:         Topic name to publish messages to
+            service_class: The message class in catkin workspace
+            queue_size:    Max number of entries to maintain in an outgoing queue
+        """
+        self.topic = topic
+        self.node_name = "{}_service".format(topic)
+        self.service_class = service_class
+        self.tcp_server = tcp_server
+        self.queue_size = queue_size
+
+        # Start Subscriber listener function
+        self.service = rospy.Service(self.topic, self.service_class, self.send)
+
+    def send(self, data):
+        """
+        Connect to TCP endpoint on client, pass along message and get reply
+        Args:
+            data: message data to send outside of ROS network
+
+        Returns:
+            The response message
+        """
+        return self.tcp_server.send_unity_service(self.topic, self.service_class, data)
+
+    def unregister(self):
+        """
+
+        Returns:
+
+        """
+        self.service.unregister()