diff --git a/ros2service/package.xml b/ros2service/package.xml index 8e4ab4335..23357e317 100644 --- a/ros2service/package.xml +++ b/ros2service/package.xml @@ -14,7 +14,7 @@ python3-yaml ros2srv - ros2topic + rosidl_runtime_py ament_copyright ament_flake8 diff --git a/ros2service/ros2service/verb/call.py b/ros2service/ros2service/verb/call.py index d097adb2b..e0c807a8c 100644 --- a/ros2service/ros2service/verb/call.py +++ b/ros2service/ros2service/verb/call.py @@ -20,8 +20,7 @@ from ros2service.api import ServiceNameCompleter from ros2service.api import ServiceTypeCompleter from ros2service.verb import VerbExtension -from ros2topic.api import set_msg_fields -from ros2topic.api import SetFieldError +from rosidl_runtime_py import set_message_fields import yaml @@ -84,10 +83,9 @@ def requester(service_type, service_name, values, period): request = srv_module.Request() try: - set_msg_fields(request, values_dictionary) - except SetFieldError as e: # noqa: F841 - return "Failed to populate field '{e.field_name}': {e.exception}" \ - .format_map(locals()) + set_message_fields(request, values_dictionary) + except Exception as e: + return 'Failed to populate field: {0}'.format(e) if not cli.service_is_ready(): print('waiting for service to become available...') diff --git a/ros2topic/package.xml b/ros2topic/package.xml index 86b71ce3a..8ab50fd0a 100644 --- a/ros2topic/package.xml +++ b/ros2topic/package.xml @@ -16,6 +16,7 @@ python3-yaml rclpy ros2msg + rosidl_runtime_py ament_copyright ament_flake8 diff --git a/ros2topic/ros2topic/api/__init__.py b/ros2topic/ros2topic/api/__init__.py index 0156c4324..dbded8efa 100644 --- a/ros2topic/ros2topic/api/__init__.py +++ b/ros2topic/ros2topic/api/__init__.py @@ -90,35 +90,6 @@ def __call__(self, prefix, parsed_args, **kwargs): return message_type_completer() -class SetFieldError(Exception): - - def __init__(self, field_name, exception): - super(SetFieldError, self).__init__() - self.field_name = field_name - self.exception = exception - - -def set_msg_fields(msg, values): - for field_name, field_value in values.items(): - field_type = type(getattr(msg, field_name)) - try: - value = field_type(field_value) - except TypeError: - value = field_type() - try: - set_msg_fields(value, field_value) - except SetFieldError as e: - raise SetFieldError( - '{field_name}.{e.field_name}'.format_map(locals()), - e.exception) - except ValueError as e: - raise SetFieldError(field_name, e) - try: - setattr(msg, field_name, value) - except Exception as e: - raise SetFieldError(field_name, e) - - def get_msg_class(node, topic, blocking=False, include_hidden_topics=False): msg_class = _get_msg_class(node, topic, include_hidden_topics) if msg_class: diff --git a/ros2topic/ros2topic/verb/echo.py b/ros2topic/ros2topic/verb/echo.py index 91dec7017..91e75cf31 100644 --- a/ros2topic/ros2topic/verb/echo.py +++ b/ros2topic/ros2topic/verb/echo.py @@ -13,11 +13,7 @@ # limitations under the License. from argparse import ArgumentTypeError -import array -from collections import OrderedDict -import sys -import numpy import rclpy from rclpy.expand_topic_name import expand_topic_name from rclpy.qos import qos_profile_sensor_data @@ -27,7 +23,8 @@ from ros2topic.api import import_message_type from ros2topic.api import TopicNameCompleter from ros2topic.verb import VerbExtension -import yaml +from rosidl_runtime_py import message_to_csv +from rosidl_runtime_py import message_to_yaml DEFAULT_TRUNCATE_LENGTH = 128 @@ -74,31 +71,14 @@ def main(self, *, args): def main(args): if not args.csv: - register_yaml_representer() - callback = subscriber_cb(args) + callback = subscriber_cb(args.truncate_length if not args.full_length else None) else: - callback = subscriber_cb_csv(args) + callback = subscriber_cb_csv(args.truncate_length if not args.full_length else None) with DirectNode(args) as node: subscriber( node.node, args.topic_name, args.message_type, callback) -def register_yaml_representer(): - # Register our custom representer for YAML output - yaml.add_representer(OrderedDict, represent_ordereddict) - - -# Custom representer for getting clean YAML output that preserves the order in -# an OrderedDict. -# Inspired by: -# http://stackoverflow.com/a/16782282/7169408 -def represent_ordereddict(dumper, data): - items = [] - for k, v in data.items(): - items.append((dumper.represent_data(k), dumper.represent_data(v))) - return yaml.nodes.MappingNode(u'tag:yaml.org,2002:map', items) - - def subscriber(node, topic_name, message_type, callback): if message_type is None: topic_names_and_types = get_topic_names_and_types(node=node, include_hidden_topics=True) @@ -132,117 +112,15 @@ def subscriber(node, topic_name, message_type, callback): rclpy.spin_once(node) -def subscriber_cb(args): +def subscriber_cb(truncate_length): def cb(msg): - nonlocal args - print(msg_to_yaml(args, msg)) + nonlocal truncate_length + print(message_to_yaml(msg, truncate_length)) return cb -def msg_to_yaml(args, msg): - return yaml.dump( - msg_to_ordereddict( - msg, - truncate_length=args.truncate_length if not args.full_length else None - ), width=sys.maxsize) - - -def subscriber_cb_csv(args): +def subscriber_cb_csv(truncate_length): def cb(msg): - nonlocal args - print(msg_to_csv(args, msg)) + nonlocal truncate_length + print(message_to_csv(msg, truncate_length)) return cb - - -def msg_to_csv(args, msg): - def to_string(val): - nonlocal args - r = '' - if (any( - isinstance(val, t) - for t in [list, tuple, array.array, numpy.ndarray] - )): - for i, v in enumerate(val): - if r: - r += ',' - if not args.full_length and i >= args.truncate_length: - r += '...' - break - r += to_string(v) - elif (any( - isinstance(val, t) - for t in [bool, bytes, float, int, str, numpy.number] - )): - if any(isinstance(val, t) for t in [bytes, str]): - if not args.full_length and len(val) > args.truncate_length: - val = val[:args.truncate_length] - if isinstance(val, bytes): - val += b'...' - else: - val += '...' - r = str(val) - else: - r = msg_to_csv(args, val) - return r - result = '' - # We rely on __slots__ retaining the order of the fields in the .msg file. - for field_name in msg.__slots__: - value = getattr(msg, field_name, None) - if result: - result += ',' - result += to_string(value) - return result - - -# Convert a msg to an OrderedDict. We do this instead of implementing a generic -# __dict__() method in the msg because we want to preserve order of fields from -# the .msg file(s). -def msg_to_ordereddict(msg, truncate_length=None): - d = OrderedDict() - # We rely on __slots__ retaining the order of the fields in the .msg file. - for field_name in msg.__slots__: - value = getattr(msg, field_name, None) - value = _convert_value(value, truncate_length=truncate_length) - # remove leading underscore from field name - d[field_name[1:]] = value - return d - - -def _convert_value(value, truncate_length=None): - if isinstance(value, bytes): - if truncate_length is not None and len(value) > truncate_length: - value = ''.join([chr(c) for c in value[:truncate_length]]) + '...' - else: - value = ''.join([chr(c) for c in value]) - elif isinstance(value, str): - if truncate_length is not None and len(value) > truncate_length: - value = value[:truncate_length] + '...' - elif (any( - isinstance(value, t) for t in [list, tuple, array.array, numpy.ndarray] - )): - # since arrays and ndarrays can't contain mixed types convert to list - typename = tuple if isinstance(value, tuple) else list - if truncate_length is not None and len(value) > truncate_length: - # Truncate the sequence - value = value[:truncate_length] - # Truncate every item in the sequence - value = typename( - [_convert_value(v, truncate_length) for v in value] + ['...']) - else: - # Truncate every item in the list - value = typename( - [_convert_value(v, truncate_length) for v in value]) - elif isinstance(value, dict) or isinstance(value, OrderedDict): - # convert each key and value in the mapping - new_value = {} if isinstance(value, dict) else OrderedDict() - for k, v in value.items(): - # don't truncate keys because that could result in key collisions and data loss - new_value[_convert_value(k)] = _convert_value(v, truncate_length=truncate_length) - value = new_value - elif ( - not any(isinstance(value, t) for t in (bool, float, int, numpy.number)) - ): - # assuming value is a message - # since it is neither a collection nor a primitive type - value = msg_to_ordereddict(value, truncate_length=truncate_length) - return value diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index 00e77edb0..7eb8028a3 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -17,11 +17,11 @@ import rclpy from ros2cli.node import NODE_NAME_PREFIX from ros2topic.api import import_message_type -from ros2topic.api import set_msg_fields -from ros2topic.api import SetFieldError from ros2topic.api import TopicNameCompleter from ros2topic.api import TopicTypeCompleter from ros2topic.verb import VerbExtension +from rosidl_runtime_py import set_message_fields + import yaml @@ -87,10 +87,9 @@ def publisher( msg = msg_module() try: - set_msg_fields(msg, values_dictionary) - except SetFieldError as e: # noqa: F841 - return "Failed to populate field '{e.field_name}': {e.exception}" \ - .format_map(locals()) + set_message_fields(msg, values_dictionary) + except Exception as e: + return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 diff --git a/ros2topic/test/test_echo.py b/ros2topic/test/test_echo.py deleted file mode 100644 index e8db57ec7..000000000 --- a/ros2topic/test/test_echo.py +++ /dev/null @@ -1,85 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from collections import OrderedDict - -from ros2topic.verb.echo import _convert_value - - -def test_convert_primitives(): - assert 5 == _convert_value(5) - assert 5 == _convert_value(5, truncate_length=0) - assert 5 == _convert_value(5, truncate_length=1) - assert 5 == _convert_value(5, truncate_length=10000) - assert 42.0 == _convert_value(42.0) - assert 42.0 == _convert_value(42.0, truncate_length=0) - assert 42.0 == _convert_value(42.0, truncate_length=1) - assert 42.0 == _convert_value(42.0, truncate_length=10000) - assert True is _convert_value(True) - assert True is _convert_value(True, truncate_length=0) - assert True is _convert_value(True, truncate_length=1) - assert True is _convert_value(True, truncate_length=10000) - assert False is _convert_value(False) - assert False is _convert_value(False, truncate_length=0) - assert False is _convert_value(False, truncate_length=1) - assert False is _convert_value(False, truncate_length=10000) - - -def test_convert_tuple(): - assert (1, 2, 3) == _convert_value((1, 2, 3)) - assert ('...',) == _convert_value((1, 2, 3), truncate_length=0) - assert (1, 2, '...') == _convert_value((1, 2, 3), truncate_length=2) - assert ('123', '456', '789') == _convert_value(('123', '456', '789')) - assert ('12...', '45...', '...') == _convert_value(('123', '456', '789'), truncate_length=2) - assert ('123', '456', '789') == _convert_value(('123', '456', '789'), truncate_length=5) - - -def test_convert_list(): - assert [1, 2, 3] == _convert_value([1, 2, 3]) - assert ['...'] == _convert_value([1, 2, 3], truncate_length=0) - assert [1, 2, '...'] == _convert_value([1, 2, 3], truncate_length=2) - assert ['123', '456', '789'] == _convert_value(['123', '456', '789']) - assert ['12...', '45...', '...'] == _convert_value(['123', '456', '789'], truncate_length=2) - assert ['123', '456', '789'] == _convert_value(['123', '456', '789'], truncate_length=5) - - -def test_convert_str(): - assert 'hello world' == _convert_value('hello world') - assert 'hello...' == _convert_value('hello world', truncate_length=5) - assert 'hello world' == _convert_value('hello world', truncate_length=1000) - - -def test_convert_bytes(): - assert 'hello world' == _convert_value(b'hello world') - assert 'hello...' == _convert_value(b'hello world', truncate_length=5) - assert 'hello world' == _convert_value(b'hello world', truncate_length=1000) - - -def test_convert_ordered_dict(): - assert OrderedDict([(1, 'a'), ('2', 'b')]) == _convert_value( - OrderedDict([(1, 'a'), ('2', 'b')])) - assert OrderedDict([(1, 'a'), ('2', 'b')]) == _convert_value( - OrderedDict([(1, 'a'), ('2', 'b')]), truncate_length=1) - assert OrderedDict([(1, 'a'), ('2', 'b')]) == _convert_value( - OrderedDict([(1, 'a'), ('2', 'b')]), truncate_length=1000) - assert OrderedDict([(1, 'a...'), ('234', 'b...')]) == _convert_value( - OrderedDict([(1, 'abc'), ('234', 'bcd')]), truncate_length=1) - - -def test_convert_dict(): - assert {1: 'a', '2': 'b'} == _convert_value({1: 'a', '2': 'b'}) - assert {1: 'a', '2': 'b'} == _convert_value({1: 'a', '2': 'b'}, truncate_length=1) - assert {1: 'a', '2': 'b'} == _convert_value({1: 'a', '2': 'b'}, truncate_length=1000) - assert {1: 'a...', '234': 'b...'} == _convert_value( - {1: 'abc', '234': 'bcd'}, truncate_length=1) diff --git a/ros2topic/test/test_primitives.py b/ros2topic/test/test_primitives.py deleted file mode 100644 index f77866f28..000000000 --- a/ros2topic/test/test_primitives.py +++ /dev/null @@ -1,44 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from argparse import Namespace - -from ros2topic.verb.echo import DEFAULT_TRUNCATE_LENGTH -from ros2topic.verb.echo import msg_to_csv -from ros2topic.verb.echo import msg_to_yaml -from ros2topic.verb.echo import register_yaml_representer -from test_msgs import message_fixtures - - -def test_primitives(): - register_yaml_representer() - # Smoke-test the formatters on a bunch of messages - msgs = [] - msgs.extend(message_fixtures.get_msg_bounded_array_nested()) - msgs.extend(message_fixtures.get_msg_bounded_array_primitives()) - msgs.extend(message_fixtures.get_msg_builtins()) - msgs.extend(message_fixtures.get_msg_dynamic_array_nested()) - msgs.extend(message_fixtures.get_msg_dynamic_array_primitives()) - msgs.extend(message_fixtures.get_msg_dynamic_array_primitives_nested()) - msgs.extend(message_fixtures.get_msg_empty()) - msgs.extend(message_fixtures.get_msg_nested()) - msgs.extend(message_fixtures.get_msg_primitives()) - msgs.extend(message_fixtures.get_msg_static_array_nested()) - msgs.extend(message_fixtures.get_msg_static_array_primitives()) - args = Namespace() - args.full_length = False - args.truncate_length = DEFAULT_TRUNCATE_LENGTH - for m in msgs: - msg_to_yaml(args, m) - msg_to_csv(args, m)