Merge branch 'laurie/SyncRos1ToRos2' into laurie/RedoRos2

Author: LaurieCheers-unity

ros_tcp_endpoint/__init__.py

@@ -12,8 +12,4 @@
 #  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
-from .unity_service import UnityService

ros_tcp_endpoint/client.py

@@ -71,8 +71,7 @@ def read_int32(conn):
         num = struct.unpack("<I", raw_bytes)[0]
         return num
 
-    @staticmethod
-    def read_string(conn):
+    def read_string(self):
         """
         Reads int32 from socket connection to determine how many bytes to
         read to get the string that follows. Read that number of bytes and
@@ -81,30 +80,32 @@ def read_string(conn):
         Returns: string
 
         """
-        str_len = ClientThread.read_int32(conn)
+        str_len = ClientThread.read_int32(self.conn)
 
-        str_bytes = ClientThread.recvall(conn, str_len)
+        str_bytes = ClientThread.recvall(self.conn, str_len)
         decoded_str = str_bytes.decode("utf-8")
 
         return decoded_str
 
-    @staticmethod
-    def read_message(conn):
+    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""
 
-        destination = ClientThread.read_string(conn)
+        destination = self.read_string()
         full_message_size = ClientThread.read_int32(conn)
 
         data = ClientThread.recvall(conn, full_message_size)
 
         if full_message_size > 0 and not data:
-            rospy.logerr("No data for a message size of {}, breaking!".format(full_message_size))
+            self.tcp_server.logerr(
+                "No data for a message size of {}, breaking!".format(full_message_size)
+            )
             return
 
+        destination = destination.rstrip("\x00")
         return destination, data
 
     @staticmethod
@@ -133,8 +134,8 @@ def serialize_message(destination, message):
         # SEEK_END or 2 - end of the stream; offset is usually negative
         response_len = serial_response.seek(0, 2)
 
-        msg_length = struct.pack("<I", response_len)
-        serialized_message = dest_info + msg_length + serial_response.getvalue()
+        msg_length = struct.pack("<I", len(serial_response))
+        serialized_message = dest_info + msg_length + serial_response
 
         return serialized_message
 
@@ -151,16 +152,16 @@ def serialize_command(command, params):
         return cmd_info + json_info
 
     def send_ros_service_request(self, srv_id, destination, data):
-        if destination not in self.tcp_server.ros_services.keys():
+        if destination not in self.tcp_server.ros_services_table.keys():
             error_msg = "Service destination '{}' is not registered! Known services are: {} ".format(
-                destination, self.tcp_server.ros_services.keys()
+                destination, self.tcp_server.ros_services_table.keys()
             )
             self.tcp_server.send_unity_error(error_msg)
-            rospy.logerr(error_msg)
+            self.tcp_server.logerr(error_msg)
             # TODO: send a response to Unity anyway?
             return
         else:
-            ros_communicator = self.tcp_server.ros_services[destination]
+            ros_communicator = self.tcp_server.ros_services_table[destination]
             service_thread = threading.Thread(
                 target=self.service_call_thread, args=(srv_id, destination, data, ros_communicator)
             )
@@ -173,7 +174,7 @@ def service_call_thread(self, srv_id, destination, data, ros_communicator):
         if not response:
             error_msg = "No response data from service '{}'!".format(destination)
             self.tcp_server.send_unity_error(error_msg)
-            rospy.logerr(error_msg)
+            self.tcp_server.logerr(error_msg)
             # TODO: send a response to Unity anyway?
             return
 
@@ -194,7 +195,7 @@ def run(self):
             msg: the ROS msg type as bytes
 
         """
-        rospy.loginfo("Connection from {}".format(self.incoming_ip))
+        self.tcp_server.loginfo("Connection from {}".format(self.incoming_ip))
         halt_event = threading.Event()
         self.tcp_server.unity_tcp_sender.start_sender(self.conn, halt_event)
         try:
@@ -219,18 +220,18 @@ def run(self):
                 elif destination.startswith("__"):
                     # handle a system command, such as registering new topics
                     self.tcp_server.handle_syscommand(destination, data)
-                elif destination in self.tcp_server.publishers:
-                    ros_communicator = self.tcp_server.publishers[destination]
+                elif destination in self.tcp_server.publishers_table:
+                    ros_communicator = self.tcp_server.publishers_table[destination]
                     ros_communicator.send(data)
                 else:
                     error_msg = "Not registered to publish topic '{}'! Valid publish topics are: {} ".format(
-                        destination, self.tcp_server.publishers.keys()
+                        destination, self.tcp_server.publishers_table.keys()
                     )
                     self.tcp_server.send_unity_error(error_msg)
-                    rospy.logerr(error_msg)
+                    self.tcp_server.logerr(error_msg)
         except IOError as e:
-            rospy.logerr("Exception: {}".format(e))
+            self.tcp_server.logerr("Exception: {}".format(e))
         finally:
             halt_event.set()
             self.conn.close()
-            rospy.loginfo("Disconnected from {}".format(self.incoming_ip))
+            self.tcp_server.loginfo("Disconnected from {}".format(self.incoming_ip))

ros_tcp_endpoint/communication.py

@@ -15,10 +15,11 @@
 
 class RosSender:
     """
-    Base class for ROS communication where data is sent to the ROS network.
+        Base class for ROS communication where data is sent to the ROS network.
     """
 
-    def __init__(self):
+    def __init__(self, node_name):
+        self.node_name = node_name
         pass
 
     def send(self, *args):
@@ -27,10 +28,11 @@ def send(self, *args):
 
 class RosReceiver:
     """
-    Base class for ROS communication where data is being sent outside of the ROS network.
+        Base class for ROS communication where data is being sent outside of the ROS network.
     """
 
-    def __init__(self):
+    def __init__(self, node_name):
+        self.node_name = node_name
         pass
 
     def send(self, *args):

ros_tcp_endpoint/default_server_endpoint.py

@@ -5,9 +5,8 @@
 from ros_tcp_endpoint import TcpServer
 
 
-def main():
-    ros_node_name = rospy.get_param("/TCP_NODE_NAME", "TCPServer")
-    tcp_server = TcpServer(ros_node_name)
+def main(args=None):
+    tcp_server = TcpServer("UnityEndpoint")
 
     # Start the Server Endpoint
     rospy.init_node(ros_node_name, anonymous=True)

ros_tcp_endpoint/publisher.py

@@ -22,6 +22,7 @@ class RosPublisher(RosSender):
     Class to publish messages to a ROS topic
     """
 
+    # TODO: surface latch functionality
     def __init__(self, topic, message_class, queue_size=10, latch=False):
         """
 
@@ -30,7 +31,9 @@ def __init__(self, topic, message_class, queue_size=10, latch=False):
             message_class: The message class in catkin workspace
             queue_size:    Max number of entries to maintain in an outgoing queue
         """
-        RosSender.__init__(self)
+        strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic)
+        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)