Merging dev branch into main (Unity-Technologies#22)

* Renaming and reorganizing package to better catkinify/pep8ify
* Commands Unity can send to manage the ROS endpoint
* Unregister the old handler when a new one is registered (Unity-Technologies#21)
* Rewrite server core loop
* Handle socket timeout
* Remove the pointless threads list
* Start the server on its own thread

Co-authored-by: Devin Miller <[email protected]>

.gitignore

@@ -1,3 +1,6 @@
 .DS_Store
 *.pyc
 .idea
+*~
+build
+devel

CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_tcp_endpoint)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+  message_generation
+)
+
+catkin_python_setup()
+
+add_message_files(DIRECTORY msg)
+
+add_service_files(DIRECTORY srv)
+
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(CATKIN_DEPENDS message_runtime)

msg/RosUnitySysCommand.msg

@@ -0,0 +1,2 @@
+string command
+string params_json

package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_tcp_endpoint</name>
+  <version>0.0.1</version>
+  <description>Acts as the bridge between Unity messages sent via Websocket and ROS messages.</description>
+
+  <maintainer email="[email protected]">Unity Robotics</maintainer>
+
+  <license>Apache 2.0</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

tcp_endpoint/src/setup.py → setup.py

@@ -5,7 +5,7 @@
 
 # fetch values from package.xml
 setup_args = generate_distutils_setup(
-    packages=['tcp_endpoint'],
+    packages=['ros_tcp_endpoint'],
     package_dir={'': 'src'})
 
-setup(**setup_args)
+setup(**setup_args)

src/ros_tcp_endpoint/__init__.py

@@ -0,0 +1,19 @@
+#  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.
+
+from .publisher import RosPublisher
+from .subscriber import RosSubscriber
+from .service import RosService
+from .server import TcpServer
+

tcp_endpoint/src/tcp_endpoint/RosTCPClientThread.py → src/ros_tcp_endpoint/client.py

@@ -1,4 +1,16 @@
-#!/usr/bin/env python
+#  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 struct
 import socket
@@ -7,7 +19,8 @@
 
 from threading import Thread
 
-from tcp_endpoint.TCPEndpointExceptions import TopicOrServiceNameDoesNotExistError
+from .exceptions import TopicOrServiceNameDoesNotExistError
+
 
 class ClientThread(Thread):
     """
@@ -133,16 +146,14 @@ def run(self):
             print("No data for a message size of {}, breaking!".format(full_message_size))
             return
 
-        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)
+        if destination == '__syscommand':
+            self.tcp_server.handle_syscommand(data)
+            return
+        elif destination == '__handshake':
+            response = self.tcp_server.unity_tcp_sender.handshake(self.incoming_ip, data)
+            response_message = self.serialize_message(destination, response)
+            self.conn.send(response_message)
+            return
         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())

src/ros_tcp_endpoint/communication.py

@@ -0,0 +1,35 @@
+#  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.
+
+
+class RosSender:
+    """
+        Base class for ROS communication where data is sent to the ROS network.
+    """
+    def __init__(self):
+        pass
+
+    def send(self, *args):
+        raise NotImplementedError
+
+
+class RosReceiver:
+    """
+        Base class for ROS communication where data is being sent outside of the ROS network.
+    """
+    def __init__(self):
+        pass
+
+    def send(self, *args):
+        raise NotImplementedError

src/ros_tcp_endpoint/default_server_endpoint.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+
+import rospy
+import actionlib_msgs.msg
+import diagnostic_msgs.msg
+import geometry_msgs.msg
+import nav_msgs.msg
+import sensor_msgs.msg
+import shape_msgs.msg
+import stereo_msgs.msg
+import trajectory_msgs.msg
+import visualization_msgs.msg
+
+from ros_tcp_endpoint import TcpServer
+
+def main():
+    ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer')
+    tcp_server = TcpServer(ros_node_name)
+
+    # Start the Server Endpoint
+    rospy.init_node(ros_node_name, anonymous=True)
+    tcp_server.start()
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()

src/ros_tcp_endpoint/exceptions.py

@@ -0,0 +1,23 @@
+#  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.
+
+
+class RosTcpEndpointError(Exception):
+    """Base class for this package's custom exceptions"""
+    pass
+
+
+class TopicOrServiceNameDoesNotExistError(RosTcpEndpointError):
+    """The topic or service name passed does not exist in the source destination dictionary."""
+    pass

tcp_endpoint/src/tcp_endpoint/RosPublisher.py → src/ros_tcp_endpoint/publisher.py

@@ -1,15 +1,27 @@
-#!/usr/bin/env python
+#  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
 
-from tcp_endpoint.RosCommunication import RosSender
+from .communication import RosSender
 
 
 class RosPublisher(RosSender):
     """
     Class to publish messages to a ROS topic
     """
-    # TODO: surrface latch functionality
+    # TODO: surface latch functionality
     def __init__(self, topic, message_class, queue_size=10):
         """
 
@@ -18,6 +30,7 @@ def __init__(self, topic, message_class, queue_size=10):
             message_class: The message class in catkin workspace
             queue_size:    Max number of entries to maintain in an outgoing queue
         """
+        RosSender.__init__(self)
         self.msg = message_class()
         self.pub = rospy.Publisher(topic, message_class, queue_size=queue_size)
 
@@ -36,3 +49,11 @@ def send(self, data):
         self.pub.publish(self.msg)
 
         return None
+
+    def unregister(self):
+        """
+
+        Returns:
+
+        """
+        self.pub.unregister()