From 1a2c386455067b00a9e67fb70cc711add38c3582 Mon Sep 17 00:00:00 2001 From: travipross Date: Sat, 19 Sep 2020 13:41:19 -0300 Subject: [PATCH] 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 Co-authored-by: CoRoLa generic Co-authored-by: joshwapohlmann --- .../internal/message_conversion.py | 47 ++++++++++++++----- .../rosbridge_library/internal/ros_loader.py | 2 + .../scripts/rosbridge_websocket.py | 7 ++- .../src/rosbridge_server/websocket_handler.py | 5 ++ 4 files changed, 44 insertions(+), 17 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/internal/message_conversion.py b/rosbridge_library/src/rosbridge_library/internal/message_conversion.py index b9a4ac976..f7c75610c 100644 --- a/rosbridge_library/src/rosbridge_library/internal/message_conversion.py +++ b/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,14 +72,15 @@ 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\[[^\]]*\]'))] @@ -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,7 +138,6 @@ 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 """ @@ -141,7 +145,6 @@ def populate_instance(msg, 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)) diff --git a/rosbridge_library/src/rosbridge_library/internal/ros_loader.py b/rosbridge_library/src/rosbridge_library/internal/ros_loader.py index 4c94b4730..f0b1a34a4 100644 --- a/rosbridge_library/src/rosbridge_library/internal/ros_loader.py +++ b/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) diff --git a/rosbridge_server/scripts/rosbridge_websocket.py b/rosbridge_server/scripts/rosbridge_websocket.py index 761be7df0..5fb16b7b0 100755 --- a/rosbridge_server/scripts/rosbridge_websocket.py +++ b/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() diff --git a/rosbridge_server/src/rosbridge_server/websocket_handler.py b/rosbridge_server/src/rosbridge_server/websocket_handler.py index e0907bf59..9916e8c2e 100755 --- a/rosbridge_server/src/rosbridge_server/websocket_handler.py +++ b/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: