ROS2 changes

Author: LaurieCheers-unity

ros_tcp_endpoint/client.py

@@ -12,14 +12,15 @@
 #  See the License for the specific language governing permissions and
 #  limitations under the License.
 
+import rclpy
 import struct
-import socket
-import rospy
-from io import BytesIO
 
 import threading
 import json
 
+from rclpy.serialization import deserialize_message
+from rclpy.serialization import serialize_message
+
 from .exceptions import TopicOrServiceNameDoesNotExistError
 
 
@@ -100,9 +101,7 @@ def read_message(self, conn):
         data = ClientThread.recvall(conn, full_message_size)
 
         if full_message_size > 0 and not data:
-            self.tcp_server.logerr(
-                "No data for a message size of {}, breaking!".format(full_message_size)
-            )
+            self.logerr("No data for a message size of {}, breaking!".format(full_message_size))
             return
 
         destination = destination.rstrip("\x00")
@@ -124,15 +123,7 @@ def serialize_message(destination, message):
         length = len(dest_bytes)
         dest_info = struct.pack("<I%ss" % length, length, dest_bytes)
 
-        serial_response = BytesIO()
-        message.serialize(serial_response)
-
-        # Per documention, https://docs.python.org/3.8/library/io.html#io.IOBase.seek,
-        # seek to end of stream for length
-        # SEEK_SET or 0 - start of the stream (the default); offset should be zero or positive
-        # SEEK_CUR or 1 - current stream position; offset may be negative
-        # SEEK_END or 2 - end of the stream; offset is usually negative
-        response_len = serial_response.seek(0, 2)
+        serial_response = serialize_message(message)
 
         msg_length = struct.pack("<I", len(serial_response))
         serialized_message = dest_info + msg_length + serial_response

ros_tcp_endpoint/communication.py

@@ -12,27 +12,30 @@
 #  See the License for the specific language governing permissions and
 #  limitations under the License.
 
+import rclpy
+from rclpy.node import Node
 
-class RosSender:
+
+class RosSender(Node):
     """
         Base class for ROS communication where data is sent to the ROS network.
     """
 
     def __init__(self, node_name):
-        self.node_name = node_name
+        super().__init__(node_name)
         pass
 
     def send(self, *args):
         raise NotImplementedError
 
 
-class RosReceiver:
+class RosReceiver(Node):
     """
         Base class for ROS communication where data is being sent outside of the ROS network.
     """
 
     def __init__(self, node_name):
-        self.node_name = node_name
+        super().__init__(node_name)
         pass
 
     def send(self, *args):

ros_tcp_endpoint/default_server_endpoint.py

@@ -1,17 +1,20 @@
 #!/usr/bin/env python
 
-import rospy
+import rclpy
 
 from ros_tcp_endpoint import TcpServer
 
 
 def main(args=None):
+    rclpy.init(args=args)
     tcp_server = TcpServer("UnityEndpoint")
 
-    # Start the Server Endpoint
-    rospy.init_node(ros_node_name, anonymous=True)
     tcp_server.start()
-    rospy.spin()
+
+    tcp_server.setup_executor()
+
+    tcp_server.destroy_nodes()
+    rclpy.shutdown()
 
 
 if __name__ == "__main__":

ros_tcp_endpoint/publisher.py

@@ -12,7 +12,10 @@
 #  See the License for the specific language governing permissions and
 #  limitations under the License.
 
-import rospy
+import rclpy
+import re
+
+from rclpy.serialization import deserialize_message
 
 from .communication import RosSender
 
@@ -35,7 +38,7 @@ def __init__(self, topic, message_class, queue_size=10, latch=False):
         node_name = f"{strippedTopic}_RosPublisher"
         RosSender.__init__(self, node_name)
         self.msg = message_class()
-        self.pub = rospy.Publisher(topic, message_class, queue_size=queue_size, latch=latch)
+        self.pub = self.create_publisher(message_class, topic, queue_size)
 
     def send(self, data):
         """
@@ -48,8 +51,10 @@ def send(self, data):
         Returns:
             None: Explicitly return None so behaviour can be
         """
-        self.msg.deserialize(data)
-        self.pub.publish(self.msg)
+        # message_type = type(self.msg)
+        # message = deserialize_message(data, message_type)
+
+        self.pub.publish(data)
 
         return None
 
@@ -59,4 +64,5 @@ def unregister(self):
         Returns:
 
         """
-        self.pub.unregister()
+        self.destroy_publisher(self.pub)
+        self.destroy_node()

ros_tcp_endpoint/server.py

@@ -12,14 +12,18 @@
 #  See the License for the specific language governing permissions and
 #  limitations under the License.
 
-import rospy
+import rclpy
 import socket
-import logging
 import json
 import sys
 import threading
 import importlib
 
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.serialization import deserialize_message
+
 from .tcp_sender import UnityTcpSender
 from .client import ClientThread
 from .subscriber import RosSubscriber
@@ -28,7 +32,7 @@
 from .unity_service import UnityService
 
 
-class TcpServer:
+class TcpServer(Node):
     """
     Initializes ROS node and TCP server.
     """
@@ -42,17 +46,22 @@ def __init__(self, node_name, buffer_size=1024, connections=10, tcp_ip=None, tcp
             buffer_size:             The read buffer size used when reading from a socket
             connections:             Max number of queued connections. See Python Socket documentation
         """
+        super().__init__(node_name)
+
+        self.declare_parameter("ROS_IP", "0.0.0.0")
+        self.declare_parameter("ROS_TCP_PORT", 10000)
+        
         if tcp_ip:
             self.loginfo("Using ROS_IP override from constructor: {}".format(tcp_ip))
             self.tcp_ip = tcp_ip
         else:
-            self.tcp_ip = rospy.get_param("/ROS_IP")
+            self.tcp_ip = self.get_parameter("ROS_IP").get_parameter_value().string_value
 
         if tcp_port:
             self.loginfo("Using ROS_TCP_PORT override from constructor: {}".format(tcp_port))
             self.tcp_port = tcp_port
         else:
-            self.tcp_port = rospy.get_param("/ROS_TCP_PORT", 10000)
+            self.tcp_port = self.get_parameter("ROS_TCP_PORT").get_parameter_value().integer_value
 
         self.unity_tcp_sender = UnityTcpSender(self)
 
@@ -118,17 +127,59 @@ def handle_syscommand(self, topic, data):
             function(**params)
 
     def loginfo(self, text):
-        rospy.loginfo(text)
+        self.get_logger().info(text)
 
     def logwarn(self, text):
-        rospy.logwarn(text)
+        self.get_logger().warning(text)
 
     def logerr(self, text):
-        rospy.logerr(text)
+        self.get_logger().error(text)
+        
+    def setup_executor(self):
+        """
+            Since rclpy.spin() is a blocking call the server needed a way
+            to spin all of the relevant nodes at the same time.
+
+            MultiThreadedExecutor allows us to set the number of threads
+            needed as well as the nodes that need to be spun.
+        """
+        num_threads = len(self.publishers_table.keys()) + len(self.subscribers_table.keys()) + len(self.ros_services_table.keys()) + len(self.unity_services_table.keys()) + 1
+        executor = MultiThreadedExecutor(num_threads)
+
+        executor.add_node(self)
 
-    def unregister_node(old_node):
+        for ros_node in self.publishers_table.values():
+            executor.add_node(ros_node)
+        for ros_node in self.subscribers_table.values():
+            executor.add_node(ros_node)
+        for ros_node in self.ros_services_table.values():
+            executor.add_node(ros_node)
+        for ros_node in self.unity_services_table.values():
+            executor.add_node(ros_node)
+
+        self.executor = executor
+        executor.spin()
+
+    def unregister_node(self, old_node):
         if old_node is not None:
             old_node.unregister()
+            if self.executor is not None:
+                self.executor.remove_node(old_node)
+
+    def destroy_nodes(self):
+        """
+            Clean up all of the nodes
+        """
+        for ros_node in self.publishers_table.values():
+            ros_node.destroy_node()
+        for ros_node in self.subscribers_table.values():
+            ros_node.destroy_node()
+        for ros_node in self.ros_services_table.values():
+            ros_node.destroy_node()
+        for ros_node in self.unity_services_table.values():
+            ros_node.destroy_node()
+
+        self.destroy_node()
 
 
 class SysCommands:
@@ -157,6 +208,8 @@ def subscribe(self, topic, message_name):
 
         new_subscriber = RosSubscriber(topic, message_class, self.tcp_server)
         self.tcp_server.subscribers_table[topic] = new_subscriber
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_subscriber)
 
         self.tcp_server.loginfo("RegisterSubscriber({}, {}) OK".format(topic, message_class))
 
@@ -183,6 +236,8 @@ def publish(self, topic, message_name, queue_size=10, latch=False):
         new_publisher = RosPublisher(topic, message_class, queue_size=queue_size, latch=latch)
 
         self.tcp_server.publishers_table[topic] = new_publisher
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_publisher)
 
         self.tcp_server.loginfo("RegisterPublisher({}, {}) OK".format(topic, message_class))
 
@@ -210,6 +265,8 @@ def ros_service(self, topic, message_name):
         new_service = RosService(topic, message_class)
 
         self.tcp_server.ros_services_table[topic] = new_service
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_service)
 
         self.tcp_server.loginfo("RegisterRosService({}, {}) OK".format(topic, message_class))
 
@@ -238,6 +295,8 @@ def unity_service(self, topic, message_name):
         new_service = UnityService(str(topic), message_class, self.tcp_server)
 
         self.tcp_server.unity_services_table[topic] = new_service
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_service)
 
         self.tcp_server.loginfo("RegisterUnityService({}, {}) OK".format(topic, message_class))
 

ros_tcp_endpoint/service.py

@@ -12,30 +12,32 @@
 #  See the License for the specific language governing permissions and
 #  limitations under the License.
 
-import rospy
-from rospy.service import ServiceException
+import rclpy
 import re
 
+from rclpy.serialization import deserialize_message
+
 from .communication import RosSender
 
 
 class RosService(RosSender):
     """
     Class to send messages to a ROS service.
     """
-
     def __init__(self, service, service_class):
         """
         Args:
             service:        The service name in ROS
             service_class:  The service class in catkin workspace
         """
-        strippedService = re.sub("[^A-Za-z0-9_]+", "", service)
-        node_name = f"{strippedService}_RosService"
+        strippedService = re.sub('[^A-Za-z0-9_]+', '', service)
+        node_name = f'{strippedService}_RosService'
         RosSender.__init__(self, node_name)
+        
+        self.service_topic = service
+        self.cli = self.create_client(service_class, service)
+        self.req = service_class.Request()
 
-        self.srv_class = service_class._request_class()
-        self.srv = rospy.ServiceProxy(service, service_class)
 
     def send(self, data):
         """
@@ -49,27 +51,33 @@ def send(self, data):
         Returns:
             service response
         """
-        message = self.srv_class.deserialize(data)
+        message_type = type(self.req)
+        message = deserialize_message(data, message_type)
 
-        attempt = 0
+        if not self.cli.service_is_ready():
+            self.get_logger().error('Ignoring service call to {} - service is not ready.'.format(self.service_topic))
+            return None
+            
+        self.future = self.cli.call_async(message)
 
-        while attempt < 3:
-            try:
-                service_response = self.srv(message)
-                return service_response
-            except ServiceException:
-                attempt += 1
-                print("Service Exception raised. Attempt: {}".format(attempt))
-            except Exception as e:
-                print("Exception Raised: {}".format(e))
+        while rclpy.ok():
+            if self.future.done():
+                try:
+                    response = self.future.result()
+                    return response
+                except Exception as e:
+                    self.get_logger().info(f'Service call failed {e}')
+
+                break
 
         return None
 
+
     def unregister(self):
         """
 
         Returns:
 
         """
-        if not self.srv is None:
-            self.srv.close()
+        self.destroy_client(self.cli)
+        self.destroy_node()