Adaptations to Eloquent [Again] (#533)

* increase spin period to 1000Hz to allow 1000 messages per second into the websocket
* allow interpreting int as float when needed
* better handling array.array and numpy arrays
* allow bytes and str websocket messages
* add boolean type
* handle type extraction of static array rostypes
* missing cls variable

Co-authored-by: Maximilian Matthe <maxi.matthe@googlemail.com>
Co-authored-by: CoRoLa generic <corola@bi>
Co-authored-by: joshwapohlmann <joshwa.pohlmann@barkhauseninstitut.org>

rosbridge_library/src/rosbridge_library/internal/message_conversion.py

@@ -34,6 +34,8 @@
 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
 
@@ -47,11 +49,11 @@
 import sys
 if sys.version_info >= (3, 0):
     type_map = {
-    "bool":    ["bool"],
+    "bool":    ["bool", "boolean"],
     "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,21 +72,24 @@
     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",
+ros_primitive_types = ["bool", "boolean", "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('<(.+?)>')
+bounded_array_tokens = re.compile('(.+)\[.*\]')
 ros_binary_types_list_braces = [("uint8[]", re.compile(r'uint8\[[^\]]*\]')),
                                 ("char[]", re.compile(r'char\[[^\]]*\]'))]
 
 binary_encoder = None
 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
@@ -198,7 +205,10 @@ def _from_list_inst(inst, rostype):
         return []
 
     # Remove the list indicators from the rostype
-    rostype = re.search(list_tokens, rostype).group(1)
+    try:
+        rostype = re.search(list_tokens, rostype).group(1)
+    except AttributeError:
+        rostype = re.search(bounded_array_tokens, rostype).group(1)
 
     # Shortcut for primitives
     if rostype in ros_primitive_types and not rostype in type_map.get('float'):
@@ -217,7 +227,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 +279,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 +292,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,14 +314,21 @@ 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)
+    try:
+        rostype = re.search(list_tokens, rostype).group(1)
+    except AttributeError:
+        rostype = re.search(bounded_array_tokens, rostype).group(1)
 
     # Call to _to_inst for every element of the list
     return [_to_inst(x, rostype, roottype, None, stack) for x in msg]
 
 
 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))

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_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
@@ -189,6 +193,7 @@ def send_message(self, message):
 
     @coroutine
     def prewrite_message(self, message, binary):
+        cls = self.__class__
         # Use a try block because the log decorator doesn't cooperate with @coroutine.
         try:
             with self._write_lock: