[NEW] ros2 - Adaptaions to Eloquent

rosbridge_library/src/rosbridge_library/internal/message_conversion.py

@@ -34,8 +34,11 @@
 from __future__ import print_function
 import rclpy
 from rclpy.clock import ROSClock
+import numpy as np
+import array
 
 from rosbridge_library.internal import ros_loader
+from rosbridge_library.internal.object_decoders import shortcut_object_decoders
 
 import math
 import re
@@ -51,7 +54,7 @@
     "int":     ["int8", "byte", "uint8", "char",
                 "int16", "uint16", "int32", "uint32",
                 "int64", "uint64", "float32", "float64"],
-    "float":   ["float32", "float64", "double"],
+    "float":   ["float32", "float64", "double", "float"],
     "str":     ["string"]
     }
     primitive_types = [bool, int, float]
@@ -70,11 +73,11 @@
     primitive_types = [bool, int, long, float]
     python2 = True
 
-list_types = [list, tuple]
+list_types = [list, tuple, np.ndarray, array.array]
 ros_time_types = ["builtin_interfaces/Time", "builtin_interfaces/Duration"]
 ros_primitive_types = ["bool", "byte", "char", "int8", "uint8", "int16",
                        "uint16", "int32", "uint32", "int64", "uint64",
-                       "float32", "float64", "double", "string"]
+                       "float32", "float64", "float", "double", "string"]
 ros_header_types = ["Header", "std_msgs/Header", "roslib/Header"]
 ros_binary_types = ["uint8[]", "char[]"]
 list_tokens = re.compile('<(.+?)>')
@@ -85,6 +88,8 @@
 binary_encoder_type = 'default'
 bson_only_mode = False
 
+
+
 # TODO(@jubeira): configure module with a node handle.
 # The original code doesn't seem to actually use these parameters.
 def configure(node_handle=None):
@@ -133,15 +138,13 @@ def extract_values(inst):
         raise InvalidMessageException(inst=inst)
     return _from_inst(inst, rostype)
 
-
 def populate_instance(msg, inst):
     """ Returns an instance of the provided class, with its fields populated
     according to the values in msg """
     inst_type = msg_instance_type_repr(inst)
 
     return _to_inst(msg, inst_type, inst_type, inst)
 
-
 def msg_instance_type_repr(msg_inst):
     """Returns a string representation of a ROS2 message type from a message instance"""
     # Message representation: '{package}.msg.{message_name}({fields})'.
@@ -173,7 +176,11 @@ def _from_inst(inst, rostype):
 
     # Check for time or duration
     if rostype in ros_time_types:
-        return {"secs": inst.secs, "nsecs": inst.nsecs}
+        try:
+            return {"sec": inst.sec, "nanosec": inst.nanosec}
+        except AttributeError:
+            return {"secs": inst.secs, "nsecs": inst.nsecs}
+
 
     if(bson_only_mode is None):bson_only_mode = rospy.get_param('~bson_only_mode', False)
     # Check for primitive types
@@ -217,7 +224,6 @@ def _from_object_inst(inst, rostype):
         msg[field_name] = _from_inst(field_inst, field_rostype)
     return msg
 
-
 def _to_inst(msg, rostype, roottype, inst=None, stack=[]):
     # Check if it's uint8[], and if it's a string, try to b64decode
     for binary_type, expression in ros_binary_types_list_braces:
@@ -270,8 +276,8 @@ def _to_time_inst(msg, rostype, inst=None):
         else:
             return None
 
-    # Copy across the fields
-    for field in ["secs", "nsecs"]:
+    # Copy across the fields, try ROS1 and ROS2 fieldnames
+    for field in ["secs", "nsecs", "sec", "nanosec"]:
         try:
             if field in msg:
                 setattr(inst, field, msg[field])
@@ -283,6 +289,11 @@ def _to_time_inst(msg, rostype, inst=None):
 
 def _to_primitive_inst(msg, rostype, roottype, stack):
     # Typecheck the msg
+    if type(msg) == int and rostype in type_map['float']:
+        # propably wrong parsing,
+        # fix that by casting the int to the expected float
+        msg = float(msg)
+
     msgtype = type(msg)
     if msgtype in primitive_types and rostype in type_map[msgtype.__name__]:
         return msg
@@ -300,6 +311,9 @@ def _to_list_inst(msg, rostype, roottype, inst, stack):
     if len(msg) == 0:
         return []
 
+    if type(inst) is np.ndarray:
+        return list(inst.astype(float))
+
     # Remove the list indicators from the rostype
     rostype = re.search(list_tokens, rostype).group(1)
 
@@ -308,6 +322,7 @@ def _to_list_inst(msg, rostype, roottype, inst, stack):
 
 
 def _to_object_inst(msg, rostype, roottype, inst, stack):
+
     # Typecheck the msg
     if type(msg) is not dict:
         raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
@@ -316,6 +331,10 @@ def _to_object_inst(msg, rostype, roottype, inst, stack):
     if rostype in ros_header_types:
         inst.stamp = ROSClock().now().to_msg()
 
+    # Do we have a dedicated decoder for the object?
+    if rostype in shortcut_object_decoders:
+        return shortcut_object_decoders[rostype](msg, inst)
+
     inst_fields = inst.get_fields_and_field_types()
 
     for field_name in msg:

rosbridge_library/src/rosbridge_library/internal/object_decoders.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020 Maximilian Matthe, Barkhausen Institut gGmbH
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# This file contains functions to decode specific ROS messages in an
+# optimized way compared to the generic way of extracting the fields
+# from the message type and the iterating through them. This way,
+# speedups can be obtained.
+
+import array
+
+def decode_compressed_image(msg, inst):
+    inst.header.frame_id = msg["header"]["frame_id"]
+    inst.header.stamp.sec = msg["header"]["stamp"]["sec"]
+    inst.header.stamp.nanosec = msg["header"]["stamp"]["nanosec"]
+    inst.format = msg["format"]
+
+    # explicitely cast to array.array to overcome a costly runtime
+    # check within inst.data instance.
+    # See also https://github.com/RobotWebTools/rosbridge_suite/pull/493#discussion_r425151919
+    inst.data = array.array("B", msg["data"])
+    return inst
+
+
+shortcut_object_decoders = {"sensor_msgs/CompressedImage": decode_compressed_image}

rosbridge_library/src/rosbridge_library/internal/ros_loader.py

@@ -199,6 +199,8 @@ def _splittype(typestring):
     splits = [x for x in typestring.split("/") if x]
     if len(splits) == 3:
         return (splits[0], splits[2])
+    if len(splits) == 2:
+        return (splits[0], splits[1])
     raise InvalidTypeStringException(typestring)
 
 

rosbridge_library/src/rosbridge_library/internal/subscribers.py

@@ -33,6 +33,8 @@
 
 from threading import Lock
 
+import time
+
 from rosbridge_library.internal import ros_loader
 from rosbridge_library.internal.message_conversion import msg_class_type_repr
 from rosbridge_library.internal.topics import TopicNotEstablishedException
@@ -178,11 +180,12 @@ def callback(self, msg, callbacks=None):
         """
         outgoing = OutgoingMessage(msg)
 
+
+
         # Get the callbacks to call
         if not callbacks:
             with self.lock:
                 callbacks = self.subscriptions.values()
-
         # Pass the JSON to each of the callbacks
         for callback in callbacks:
             try:
@@ -257,4 +260,3 @@ def unsubscribe(self, client_id, topic):
 
 
 manager = SubscriberManager()
-

rosbridge_library/src/rosbridge_library/protocol.py

@@ -167,7 +167,6 @@ def incoming(self, message_string=""):
                                 break
                         except Exception as e:
                             # debug json-decode errors with this line
-                            #print e
                             pass
                     # if load was successfull --> break outer loop, too.. -> no need to check if json begins at a "later" opening bracket..
                     if msg != None:
@@ -294,7 +293,7 @@ def serialize(self, msg, cid=None):
                 return msg
             if has_binary(msg) or self.bson_only_mode:
                 return bson.BSON.encode(msg)
-            else:    
+            else:
                 return json.dumps(msg)
         except:
             if cid is not None:

rosbridge_server/scripts/rosbridge_websocket.py

@@ -109,7 +109,7 @@ def __init__(self):
         RosbridgeWebSocket.authenticate = self.declare_parameter('authenticate', False).value
 
         port = self.declare_parameter('port', 9090).value
-        
+
         address = self.declare_parameter('address', '').value
 
         RosbridgeWebSocket.client_manager = ClientManager(self)
@@ -292,15 +292,14 @@ def __init__(self):
                     "Retrying in {}s.".format(e, retry_startup_delay))
                 time.sleep(retry_startup_delay)
 
-
 def main(args=None):
     if args is None:
         args = sys.argv
-    
+
     rclpy.init(args=args)
     rosbridge_websocket_node = RosbridgeWebsocketNode()
 
-    spin_callback = PeriodicCallback(lambda: rclpy.spin_once(rosbridge_websocket_node, timeout_sec=0.01), 100)
+    spin_callback = PeriodicCallback(lambda: rclpy.spin_once(rosbridge_websocket_node, timeout_sec=0.01), 1)
     spin_callback.start()
     start_hook()
 

rosbridge_server/src/rosbridge_server/websocket_handler.py

@@ -161,9 +161,13 @@ def on_message(self, message):
                 self.close()
             except:
                 # proper error will be handled in the protocol class
+                if type(message) is bytes:
+                    message = message.decode('utf-8')
                 self.protocol.incoming(message)
         else:
             # no authentication required
+            if type(message) is bytes:
+                message = message.decode('utf-8')
             self.protocol.incoming(message)
 
     @log_exceptions