diff --git a/docs/api/rosbags.rst b/docs/api/rosbags.rst index ef5b502d..b106356c 100644 --- a/docs/api/rosbags.rst +++ b/docs/api/rosbags.rst @@ -4,3 +4,4 @@ Rosbags namespace .. toctree:: :maxdepth: 4 + rosbags.typesys diff --git a/docs/api/rosbags.typesys.rst b/docs/api/rosbags.typesys.rst new file mode 100644 index 00000000..58aad884 --- /dev/null +++ b/docs/api/rosbags.typesys.rst @@ -0,0 +1,6 @@ +rosbags.typesys +=============== + +.. automodule:: rosbags.typesys + :members: + :show-inheritance: \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst index efe8bdfc..83f2482d 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -10,6 +10,7 @@ :maxdepth: 1 :hidden: + topics/typesys .. toctree:: diff --git a/docs/topics/typesys.rst b/docs/topics/typesys.rst new file mode 100644 index 00000000..acb98f8c --- /dev/null +++ b/docs/topics/typesys.rst @@ -0,0 +1,35 @@ +Type system +=========== + +Rosbags ships its own pure python typesystem :py:mod:`rosbags.typesys`. It uses parse trees to represent message definitions internally. It ships its own ``.idl`` and ``.msg`` definition parser to convert message definition files into the internal format. + +Out of the box it supports the message types defined by the standard ROS2 distribution. Message types can be parsed and added on the fly during runtime without an additional build step. + +Message instances +----------------- +The type system generates a dataclass for each message type. These dataclasses give direct read write access to all mutable fields of a message. Fields should be mutated with care as no type checking is applied during runtime. + +Extending the type system +------------------------- +Adding custom message types consists of two steps. First, message definitions are converted into parse trees using :py:func:`get_types_from_idl() ` or :py:func:`get_types_from_msg() `, and second the types are registered in the type system via :py:func:`register_types() `. The following example shows how to add messages type definitions from ``.msg`` and ``.idl`` files: + +.. code-block:: python + + from pathlib import Path + + from rosbags.typesys import get_types_from_idl, get_types_from_msg, register_types + + idl_text = Path('foo_msgs/msg/Foo.idl').read_text() + msg_text = Path('bar_msgs/msg/Bar.msg').read_text() + + # plain dictionary to hold message definitions + add_types = {} + + # add all definitions from one idl file + add_types.update(get_types_from_idl(idl_text)) + + # add definition from one msg file + add_types.update(get_types_from_msg(msg_text, 'bar_msgs/msg/Bar')) + + # make types available to rosbags serializers/deserializers + register_types(add_types) diff --git a/src/rosbags/typesys/__init__.py b/src/rosbags/typesys/__init__.py new file mode 100644 index 00000000..1dc19c22 --- /dev/null +++ b/src/rosbags/typesys/__init__.py @@ -0,0 +1,29 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Rosbags Type System. + +The type system manages ROS message types and ships all standard ROS2 +distribution message types by default. The system supports custom message +types through parsers that dynamically parse custom message definitons +from different source formats. + +Supported formats: + - IDL files (subset of the standard necessary for parsing ROS2 IDL) `[1]`_ + - MSG files `[2]`_ + +.. _[1]: https://www.omg.org/spec/IDL/About-IDL/ +.. _[2]: http://wiki.ros.org/msg + +""" + +from .base import TypesysError +from .idl import get_types_from_idl +from .msg import get_types_from_msg +from .register import register_types + +__all__ = [ + 'TypesysError', + 'get_types_from_idl', + 'get_types_from_msg', + 'register_types', +] diff --git a/src/rosbags/typesys/__main__.py b/src/rosbags/typesys/__main__.py new file mode 100644 index 00000000..4396fcaf --- /dev/null +++ b/src/rosbags/typesys/__main__.py @@ -0,0 +1,45 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Tool to update builtin types shipped with rosbags.""" + +from __future__ import annotations + +from os import walk +from pathlib import Path +from typing import TYPE_CHECKING + +from .idl import get_types_from_idl +from .msg import get_types_from_msg +from .register import generate_python_code, register_types + +if TYPE_CHECKING: + from .base import Typesdict + + +def main() -> None: # pragma: no cover + """Update builtin types. + + Discover message definitions in filesystem and generate types.py module. + + """ + typs: Typesdict = {} + selfdir = Path(__file__).parent + for root, dirnames, files in walk(selfdir.parents[2] / 'tools' / 'messages'): + if '.rosbags_ignore' in files: + dirnames.clear() + continue + for fname in files: + path = Path(root, fname) + if path.suffix == '.idl': + typs.update(get_types_from_idl(path.read_text())) + elif path.suffix == '.msg': + name = path.relative_to(path.parents[2]).with_suffix('') + if '/msg/' not in str(name): + name = name.parent / 'msg' / name.name + typs.update(get_types_from_msg(path.read_text(), str(name))) + register_types(typs) + (selfdir / 'types.py').write_text(generate_python_code(typs)) + + +if __name__ == '__main__': + main() diff --git a/src/rosbags/typesys/base.py b/src/rosbags/typesys/base.py new file mode 100644 index 00000000..d7d34377 --- /dev/null +++ b/src/rosbags/typesys/base.py @@ -0,0 +1,70 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Types and helpers used by message definition converters.""" + +from __future__ import annotations + +from enum import IntEnum, auto +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from typing import Any, Dict, List, Tuple + + from .peg import Visitor + + Fielddefs = List[Tuple[Any, Any]] + Typesdict = Dict[str, Fielddefs] + + +class TypesysError(Exception): + """Parser error.""" + + +class Nodetype(IntEnum): + """Parse tree node types. + + The first four match the Valtypes of final message definitions. + """ + + BASE = auto() + NAME = auto() + ARRAY = auto() + SEQUENCE = auto() + + LITERAL_STRING = auto() + LITERAL_NUMBER = auto() + LITERAL_BOOLEAN = auto() + LITERAL_CHAR = auto() + + MODULE = auto() + CONST = auto() + STRUCT = auto() + SDECLARATOR = auto() + ADECLARATOR = auto() + ANNOTATION = auto() + EXPRESSION_BINARY = auto() + EXPRESSION_UNARY = auto() + + +def parse_message_definition(visitor: Visitor, text: str) -> Typesdict: + """Parse message definition. + + Args: + visitor: Visitor instance to use. + text: Message definition. + + Returns: + Parsetree of message. + + Raises: + TypesysError: Message parsing failed. + + """ + try: + rule = visitor.RULES['specification'] + pos = rule.skip_ws(text, 0) + npos, trees = rule.parse(text, pos) + assert npos == len(text), f'Could not parse: {text!r}' + return visitor.visit(trees) + except Exception as err: # pylint: disable=broad-except + raise TypesysError(f'Could not parse: {text!r}') from err diff --git a/src/rosbags/typesys/idl.py b/src/rosbags/typesys/idl.py new file mode 100644 index 00000000..acc6bafa --- /dev/null +++ b/src/rosbags/typesys/idl.py @@ -0,0 +1,465 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""IDL Parser. + +Grammar, parse tree visitor and conversion functions for message definitions in +`IDL`_ format. + +.. _IDL: https://www.omg.org/spec/IDL/About-IDL/ + +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from .base import Nodetype, parse_message_definition +from .peg import Rule, Visitor, parse_grammar + +if TYPE_CHECKING: + from typing import Any + + from .base import Typesdict + +GRAMMAR_IDL = r""" +specification + = definition+ + +definition + = comment + / macro + / include + / module_dcl ';' + / const_dcl ';' + / type_dcl ';' + +comment + = r'[/][/][^\n]*' + +macro + = ifndef + / define + / endif + +ifndef + = '#ifndef' r'[a-zA-Z0-9_]+' + +define + = '#define' r'[a-zA-Z0-9_]+' + +endif + = '#endif' + +include + = '#include' include_filename + +include_filename + = '<' r'[^>]+' '>' + / '"' r'[^"]+' '"' + +module_dcl + = annotation* 'module' identifier '{' definition+ '}' + +const_dcl + = 'const' const_type identifier '=' expression + +type_dcl + = typedef_dcl + / constr_type_dcl + +typedef_dcl + = 'typedef' type_declarator + +type_declarator + = ( simple_type_spec + / template_type_spec + / constr_type_dcl + ) any_declarators + +simple_type_spec + = base_type_spec + / scoped_name + +template_type_spec + = sequence_type + / string_type + +sequence_type + = 'sequence' '<' type_spec ',' expression '>' + / 'sequence' '<' type_spec '>' + +type_spec + = template_type_spec + / simple_type_spec + +any_declarators + = any_declarator (',' any_declarator)* + +any_declarator + = array_declarator + / simple_declarator + +constr_type_dcl + = struct_dcl + +struct_dcl + = struct_def + +struct_def + = annotation* 'struct' identifier '{' member+ '}' + +member + = annotation* type_spec declarators ';' + +declarators + = declarator (',' declarator)* + +declarator + = array_declarator + / simple_declarator + +simple_declarator + = identifier + +array_declarator + = identifier fixed_array_size+ + +fixed_array_size + = '[' expression ']' + +annotation + = '@' scoped_name ('(' annotation_params ')')? + +annotation_params + = annotation_param (',' annotation_param)* + / expression + +annotation_param + = identifier '=' expression + +const_type + = base_type_spec + / string_type + / scoped_name + +base_type_spec + = integer_type + / float_type + / char_type + / boolean_type + / octet_type + +integer_type + = r'u?int(64|32|16|8)\b' + / r'(unsigned\s+)?((long\s+)?long|int|short)\b' + +float_type + = r'((long\s+)?double|float)\b' + +char_type + = r'char\b' + +boolean_type + = r'boolean\b' + +octet_type + = r'octet\b' + +string_type + = 'string' '<' expression '>' + / 'string' + +scoped_name + = identifier '::' scoped_name + / '::' scoped_name + / identifier + +identifier + = r'[a-zA-Z_][a-zA-Z_0-9]*' + +expression + = primary_expr binary_operator primary_expr + / primary_expr + / unary_operator primary_expr + +primary_expr + = literal + / scoped_name + / '(' expression ')' + +binary_operator + = '|' + / '^' + / '&' + / '<<' + / '>>' + / '+' + / '-' + / '*' + / '/' + / '%' + +unary_operator + = '+' + / '-' + / '~' + +literal + = boolean_literal + / float_literal + / integer_literal + / character_literal + / string_literals + +boolean_literal + = 'TRUE' + / 'FALSE' + +integer_literal + = hexadecimal_literal + / octal_literal + / decimal_literal + +decimal_literal + = r'[-+]?[1-9][0-9]+' + / r'[-+]?[0-9]' + +octal_literal + = r'[-+]?0[0-7]+' + +hexadecimal_literal + = r'[-+]?0[xX][a-fA-F0-9]+' + +float_literal + = r'[-+]?[0-9]*\.[0-9]+([eE][-+]?[0-9]+)?' + / r'[-+]?[0-9]*\.?[0-9]+([eE][-+]?[0-9]+)' + +character_literal + = '\'' r'[a-zA-Z0-9_]' '\'' + +string_literals + = string_literal+ + +string_literal + = '"' r'(\\"|[^"])*' '"' +""" + + +class VisitorIDL(Visitor): # pylint: disable=too-many-public-methods + """IDL file visitor.""" + + # pylint: disable=no-self-use + + RULES = parse_grammar(GRAMMAR_IDL) + + def visit_specification(self, children: Any) -> Typesdict: + """Process start symbol, return only children of modules.""" + children = [x[0] for x in children if x is not None] + modules = [y for t, x in children if t == Nodetype.MODULE for y in x] + return {x[1]: x[2] for x in modules if x[0] == Nodetype.STRUCT} + + def visit_comment(self, children: Any) -> Any: + """Process comment, suppress output.""" + + def visit_macro(self, children: Any) -> Any: + """Process macro, suppress output.""" + + def visit_include(self, children: Any) -> Any: + """Process include, suppress output.""" + + def visit_type_dcl(self, children: Any) -> Any: + """Process typedef, pass structs, suppress otherwise.""" + if children[0] == Nodetype.STRUCT: + return children + return None + + def visit_module_dcl(self, children: Any) -> Any: + """Process module declaration.""" + assert len(children) == 6 + assert children[2][0] == Nodetype.NAME + name = children[2][1] + + children = children[4] + consts = [] + structs = [] + modules = [] + for item in children: + if not item or item[0] is None: + continue + item = item[0] + if item[0] == Nodetype.CONST: + consts.append(item) + elif item[0] == Nodetype.STRUCT: + structs.append(item) + else: + assert item[0] == Nodetype.MODULE + modules.append(item) + + for _, module in modules: + consts += [x for x in module if x[0] == Nodetype.CONST] + structs += [x for x in module if x[0] == Nodetype.STRUCT] + + consts = [(x[0], f'{name}/{x[1][0]}', *x[1][1:]) for x in consts] + structs = [(x[0], f'{name}/{x[1]}', *x[2:]) for x in structs] + + return (Nodetype.MODULE, consts + structs) + + def visit_const_dcl(self, children: Any) -> Any: + """Process const declaration.""" + return (Nodetype.CONST, (children[1][1], *children[2:])) + + def visit_type_declarator(self, children: Any) -> Any: + """Process type declarator, register type mapping in instance typedef dictionary.""" + assert len(children) == 2 + base, declarators = children + if base[1] in self.typedefs: + base = self.typedefs[base[1]] + declarators = [children[1][0], *[x[1:][0] for x in children[1][1]]] + for declarator in declarators: + if declarator[0] == Nodetype.ADECLARATOR: + value = (Nodetype.ARRAY, declarator[2][1], base) + else: + value = base + self.typedefs[declarator[1][1]] = value + + def visit_sequence_type(self, children: Any) -> Any: + """Process sequence type specification.""" + assert len(children) in [4, 6] + if len(children) == 6: + assert children[4][0] == Nodetype.LITERAL_NUMBER + return (Nodetype.SEQUENCE, children[2]) + return (Nodetype.SEQUENCE, children[2]) + + def create_struct_field(self, parts: Any) -> Any: + """Create struct field and expand typedefs.""" + typename, params = parts[1:3] + params = [params[0], *[x[1:][0] for x in params[1]]] + + def resolve_name(name: Any) -> Any: + while name[0] == Nodetype.NAME and name[1] in self.typedefs: + name = self.typedefs[name[1]] + return name + + yield from ((resolve_name(typename), x[1]) for x in params if x) + + def visit_struct_dcl(self, children: Any) -> Any: + """Process struct declaration.""" + assert len(children) == 6 + assert children[2][0] == Nodetype.NAME + + fields = [y for x in children[4] for y in self.create_struct_field(x)] + return (Nodetype.STRUCT, children[2][1], fields) + + def visit_simple_declarator(self, children: Any) -> Any: + """Process simple declarator.""" + assert len(children) == 2 + return (Nodetype.SDECLARATOR, children) + + def visit_array_declarator(self, children: Any) -> Any: + """Process array declarator.""" + assert len(children) == 2 + return (Nodetype.ADECLARATOR, children[0], children[1][0][1]) + + def visit_annotation(self, children: Any) -> Any: + """Process annotation.""" + assert len(children) == 3 + assert children[1][0] == Nodetype.NAME + params = children[2][0][1] + params = [ + [z for z in y if z[0] != Rule.LIT] for y in [params[0], *[x[1:][0] for x in params[1]]] + ] + return (Nodetype.ANNOTATION, children[1][1], params) + + def visit_base_type_spec(self, children: Any) -> Any: + """Process base type specifier.""" + oname = children + name = { + 'boolean': 'bool', + 'double': 'float64', + 'float': 'float32', + 'octet': 'uint8', + }.get(oname, oname) + return (Nodetype.BASE, name) + + def visit_string_type(self, children: Any) -> Any: + """Prrocess string type specifier.""" + assert len(children) in [2, 4] + if len(children) == 4: + return (Nodetype.BASE, 'string', children[2]) + return (Nodetype.BASE, 'string') + + def visit_scoped_name(self, children: Any) -> Any: + """Process scoped name.""" + if len(children) == 2: + return (Nodetype.NAME, children[1]) + assert len(children) == 3 + assert children[1][1] == '::' + return (Nodetype.NAME, f'{children[0][1]}/{children[2][1]}') + + def visit_identifier(self, children: Any) -> Any: + """Process identifier.""" + return (Nodetype.NAME, children) + + def visit_expression(self, children: Any) -> Any: + """Process expression, literals are assumed to be integers only.""" + if children[0] in [ + Nodetype.LITERAL_STRING, + Nodetype.LITERAL_NUMBER, + Nodetype.LITERAL_BOOLEAN, + Nodetype.LITERAL_CHAR, + Nodetype.NAME, + ]: + return children + + assert len(children) in [2, 3] + if len(children) == 3: + assert isinstance(children[0][1], int) + assert isinstance(children[2][1], int) + return (Nodetype.EXPRESSION_BINARY, children[1], children[0][1], children[2]) + assert len(children) == 2 + assert isinstance(children[1][1], int), children + return (Nodetype.EXPRESSION_UNARY, children[0][1], children[1]) + + def visit_boolean_literal(self, children: Any) -> Any: + """Process boolean literal.""" + return (Nodetype.LITERAL_BOOLEAN, children[1] == 'TRUE') + + def visit_float_literal(self, children: Any) -> Any: + """Process float literal.""" + return (Nodetype.LITERAL_NUMBER, float(children)) + + def visit_decimal_literal(self, children: Any) -> Any: + """Process decimal integer literal.""" + return (Nodetype.LITERAL_NUMBER, int(children)) + + def visit_octal_literal(self, children: Any) -> Any: + """Process octal integer literal.""" + return (Nodetype.LITERAL_NUMBER, int(children, 8)) + + def visit_hexadecimal_literal(self, children: Any) -> Any: + """Process hexadecimal integer literal.""" + return (Nodetype.LITERAL_NUMBER, int(children, 16)) + + def visit_character_literal(self, children: Any) -> Any: + """Process char literal.""" + return (Nodetype.LITERAL_CHAR, children[1]) + + def visit_string_literals(self, children: Any) -> Any: + """Process string literal.""" + return ( + Nodetype.LITERAL_STRING, + ''.join(y for x in children for y in x if y and y[0] != Rule.LIT), + ) + + +def get_types_from_idl(text: str) -> Typesdict: + """Get types from idl message definition. + + Args: + text: Message definition. + + Returns: + List of message message names and parsetrees. + + """ + return parse_message_definition(VisitorIDL(), text) diff --git a/src/rosbags/typesys/msg.py b/src/rosbags/typesys/msg.py new file mode 100644 index 00000000..52943a43 --- /dev/null +++ b/src/rosbags/typesys/msg.py @@ -0,0 +1,215 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""MSG Parser. + +Grammar, parse tree visitor and conversion functions for message definitions in +`MSG`_ format. It also supports concatened message definitions as found in +Rosbag1 connection information. + +.. _MSG: http://wiki.ros.org/msg + +""" + +from __future__ import annotations + +from pathlib import Path +from typing import TYPE_CHECKING + +from .base import Nodetype, parse_message_definition +from .peg import Rule, Visitor, parse_grammar + +if TYPE_CHECKING: + from typing import Any, List + + from .base import Typesdict + +GRAMMAR_MSG = r""" +specification + = msgdef (msgsep msgdef)* + +msgdef + = r'MSG:\s' scoped_name definition+ + +msgsep + = r'================================================================================' + +definition + = comment + / const_dcl + / field_dcl + +comment + = r'#[^\n]*' + +const_dcl + = type_spec identifier '=' r'[^=][^\n]*' + +field_dcl + = type_spec identifier + +type_spec + = array_type_spec + / simple_type_spec + +array_type_spec + = simple_type_spec array_size + +simple_type_spec + = scoped_name + +array_size + = '[' integer_literal? ']' + +integer_literal + = r'[-+]?[1-9][0-9]+' + / r'[-+]?[0-9]' + +scoped_name + = identifier '/' scoped_name + / identifier + +identifier + = r'[a-zA-Z_][a-zA-Z_0-9]*' +""" + + +def normalize_msgtype(name: str) -> str: + """Normalize message typename. + + Args: + name: Message typename. + + Returns: + Normalized name. + + """ + path = Path(name) + if path.parent.name != 'msg': + return str(path.parent / 'msg' / path.name) + return name + + +def normalize_fieldtype(field: Any, names: List[str]): + """Normalize field typename. + + Args: + field: Field definition. + names: Valid message names. + + """ + dct = {Path(name).name: name for name in names} + namedef = field[0] + if namedef[0] == Nodetype.NAME: + name = namedef[1] + elif namedef[0] == Nodetype.SEQUENCE: + name = namedef[1][1] + else: + name = namedef[2][1] + + if name in VisitorMSG.BASETYPES: + inamedef = (Nodetype.BASE, name) + else: + if name in dct: + name = dct[name] + elif '/msg/' not in name: + ptype = Path(name) + name = str(ptype.parent / 'msg' / ptype.name) + inamedef = (Nodetype.NAME, name) + + if namedef[0] == Nodetype.NAME: + namedef = inamedef + elif namedef[0] == Nodetype.SEQUENCE: + namedef = (Nodetype.SEQUENCE, inamedef) + else: + namedef = (Nodetype.ARRAY, namedef[1], inamedef) + + field[0] = namedef + + +class VisitorMSG(Visitor): + """MSG file visitor.""" + + # pylint: disable=no-self-use + + RULES = parse_grammar(GRAMMAR_MSG) + + BASETYPES = { + 'bool', + 'int8', + 'int16', + 'int32', + 'int64', + 'uint8', + 'uint16', + 'uint32', + 'uint64', + 'float32', + 'float64', + 'string', + } + + def visit_comment(self, children: Any) -> Any: + """Process comment, suppress output.""" + + def visit_const_dcl(self, children: Any) -> Any: + """Process const declaration, suppress output.""" + + def visit_specification(self, children: Any) -> Typesdict: + """Process start symbol.""" + typelist = [children[0], *[x[1] for x in children[1]]] + typedict = dict(typelist) + names = list(typedict.keys()) + for _, fields in typedict.items(): + for field in fields: + normalize_fieldtype(field, names) + return typedict + + def visit_msgdef(self, children: Any) -> Any: + """Process single message definition.""" + assert len(children) == 3 + return normalize_msgtype(children[1][1]), [x for x in children[2] if x is not None] + + def visit_msgsep(self, children: Any) -> Any: + """Process message separator, suppress output.""" + + def visit_array_type_spec(self, children: Any) -> Any: + """Process array type specifier.""" + length = children[1][1] + if length: + return (Nodetype.ARRAY, int(length[0]), children[0]) + return (Nodetype.SEQUENCE, children[0]) + + def visit_simple_type_spec(self, children: Any) -> Any: + """Process simple type specifier.""" + dct = { + 'time': 'builtin_interfaces/msg/Time', + 'duration': 'builtin_interfaces/msg/Duration', + 'byte': 'uint8', + 'char': 'uint8', + } + return Nodetype.NAME, dct.get(children[1], children[1]) + + def visit_scoped_name(self, children: Any) -> Any: + """Process scoped name.""" + if len(children) == 2: + return children + assert len(children) == 3 + return (Nodetype.NAME, '/'.join(x[1] for x in children if x[0] != Rule.LIT)) + + def visit_identifier(self, children: Any) -> Any: + """Process identifier.""" + return (Nodetype.NAME, children) + + +def get_types_from_msg(text: str, name: str) -> Typesdict: + """Get type from msg message definition. + + Args: + text: Message definiton. + name: Message typename. + + Returns: + List with single message name and parsetree. + + """ + return parse_message_definition(VisitorMSG(), f'MSG: {name}\n{text}') diff --git a/src/rosbags/typesys/peg.py b/src/rosbags/typesys/peg.py new file mode 100644 index 00000000..95d1e731 --- /dev/null +++ b/src/rosbags/typesys/peg.py @@ -0,0 +1,247 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""PEG Parser. + +Parsing expression grammar inspired parser for simple EBNF-like notations. It +implements just enough features to support parsing of the different ROS message +definition formats. + +""" + +from __future__ import annotations + +import re +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from typing import Any, Dict, List, Optional, Tuple + + +class Rule: + """Rule base class.""" + + LIT = 'LITERAL' + WS = re.compile(r'\s+', re.M | re.S) + + def __init__(self, value: Any, rules: Dict[str, Rule], name: Optional[str] = None): + """Initialize. + + Args: + value: Value of this rule. + rules: Grammar containing all rules. + name: Name of this rule. + + """ + self.value = value + self.rules = rules + self.name = name + + def skip_ws(self, text: str, pos: int) -> int: + """Skip whitespace.""" + match = self.WS.match(text, pos) + return match.span()[1] if match else pos + + def make_node(self, data: Any) -> Any: + """Make node for parse tree.""" + if self.name: + return { + 'node': self.name, + 'data': data, + } + return data + + def parse(self, text: str, pos: int): + """Apply rule at position.""" + raise NotImplementedError # pragma: no cover + + +class RuleLiteral(Rule): + """Rule to match string literal.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + value: str = self.value[1:-1].replace('\\\'', '\'') + if text[pos:].startswith(value): + npos = pos + len(value) + npos = self.skip_ws(text, npos) + return npos, (self.LIT, value) + return -1, () + + +class RuleRegex(Rule): + """Rule to match regular expression.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + pattern = re.compile(self.value[2:-1], re.M | re.S) + match = pattern.match(text, pos) + if not match: + return -1, [] + npos = self.skip_ws(text, match.span()[1]) + return npos, self.make_node(match.group()) + + +class RuleToken(Rule): + """Rule to match token.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + token = self.rules[self.value] + npos, data = token.parse(text, pos) + if npos == -1: + return npos, data + return npos, self.make_node(data) + + +class RuleOneof(Rule): + """Rule to match first matching subrule.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + for value in self.value: + npos, data = value.parse(text, pos) + if npos != -1: + return npos, self.make_node(data) + return -1, [] + + +class RuleSequence(Rule): + """Rule to match a sequence of subrules.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + data = [] + npos = pos + for value in self.value: + npos, node = value.parse(text, npos) + if npos == -1: + return -1, [] + data.append(node) + return npos, self.make_node(data) + + +class RuleZeroPlus(Rule): + """Rule to match zero or more occurences of subrule.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + data: List[Any] = [] + lpos = pos + while True: + npos, node = self.value.parse(text, lpos) + if npos == -1: + return lpos, self.make_node(data) + data.append(node) + lpos = npos + + +class RuleOnePlus(Rule): + """Rule to match one or more occurences of subrule.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + npos, node = self.value.parse(text, pos) + if npos == -1: + return -1, [] + data = [node] + lpos = npos + while True: + npos, node = self.value.parse(text, lpos) + if npos == -1: + return lpos, self.make_node(data) + data.append(node) + lpos = npos + + +class RuleZeroOne(Rule): + """Rule to match zero or one occurence of subrule.""" + + def parse(self, text: str, pos: int) -> Tuple[int, Any]: + """Apply rule at position.""" + npos, node = self.value.parse(text, pos) + if npos == -1: + return pos, self.make_node([]) + return npos, self.make_node([node]) + + +class Visitor: # pylint: disable=too-few-public-methods + """Visitor transforming parse trees.""" + + RULES: Dict[str, Rule] = {} + + def __init__(self): + """Initialize.""" + self.typedefs = {} + + def visit(self, tree: Any) -> Any: + """Visit all nodes in parse tree.""" + if isinstance(tree, list): + return [self.visit(x) for x in tree] + + if not isinstance(tree, dict): + return tree + + tree['data'] = self.visit(tree['data']) + func = getattr(self, f'visit_{tree["node"]}', lambda x: x) + return func(tree['data']) + + +def split_token(tok: str) -> List[str]: + """Split repetition and grouping tokens.""" + return list(filter(None, re.split(r'(^\()|(\)(?=[*+?]?$))|([*+?]$)', tok))) + + +def collapse_tokens(toks: List[Optional[Rule]], rules: Dict[str, Rule]) -> Rule: + """Collapse linear list of tokens to oneof of sequences.""" + value: List[Rule] = [] + seq: List[Rule] = [] + for tok in toks: + if tok: + seq.append(tok) + else: + value.append(RuleSequence(seq, rules) if len(seq) > 1 else seq[0]) + seq = [] + value.append(RuleSequence(seq, rules) if len(seq) > 1 else seq[0]) + return RuleOneof(value, rules) if len(value) > 1 else value[0] + + +def parse_grammar(grammar: str) -> Dict[str, Rule]: + """Parse grammar into rule dictionary.""" + rules: Dict[str, Rule] = {} + for token in grammar.split('\n\n'): + lines = token.strip().split('\n') + name, *defs = lines + items = [z for x in defs for y in x.split(' ') if y for z in split_token(y) if z] + assert items + assert items[0] == '=' + items.pop(0) + stack: List[Optional[Rule]] = [] + parens: List[int] = [] + while items: + tok = items.pop(0) + if tok in ['*', '+', '?']: + stack[-1] = { + '*': RuleZeroPlus, + '+': RuleOnePlus, + '?': RuleZeroOne, + }[tok](stack[-1], rules) + elif tok == '/': + stack.append(None) + elif tok == '(': + parens.append(len(stack)) + elif tok == ')': + index = parens.pop() + rule = collapse_tokens(stack[index:], rules) + stack = stack[:index] + stack.append(rule) + elif len(tok) > 2 and tok[:2] == 'r\'': + stack.append(RuleRegex(tok, rules)) + elif tok[0] == '\'': + stack.append(RuleLiteral(tok, rules)) + else: + stack.append(RuleToken(tok, rules)) + + res = collapse_tokens(stack, rules) + res.name = name + rules[name] = res + return rules diff --git a/src/rosbags/typesys/register.py b/src/rosbags/typesys/register.py new file mode 100644 index 00000000..4a4b07af --- /dev/null +++ b/src/rosbags/typesys/register.py @@ -0,0 +1,112 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Code generators and registration functions for the extensible type system.""" + +from __future__ import annotations + +import json +import sys +from importlib.util import module_from_spec, spec_from_loader +from typing import TYPE_CHECKING + +from . import types +from .base import TypesysError + +if TYPE_CHECKING: + from .base import Typesdict + + +def generate_python_code(typs: Typesdict) -> str: + """Generate python code from types dictionary. + + Args: + typs: Dictionary mapping message typenames to parsetrees. + + Returns: + Code for importable python module. + + """ + lines = [ + '# Copyright 2020-2021 Ternaris.', + '# SPDX-License-Identifier: Apache-2.0', + '#', + '# THIS FILE IS GENERATED, DO NOT EDIT', + '"""ROS2 message types."""', + '', + '# flake8: noqa N801', + '# pylint: disable=invalid-name,too-many-instance-attributes,too-many-lines', + '', + 'from __future__ import annotations', + '', + 'from dataclasses import dataclass', + 'from typing import TYPE_CHECKING', + '', + 'if TYPE_CHECKING:', + ' from typing import Any', + '', + '', + ] + + for name, fields in typs.items(): + pyname = name.replace('/', '__') + lines += [ + '@dataclass', + f'class {pyname}:', + f' """Class for {name}."""', + '', + *[f' {fname[1]}: Any' for _, fname in fields], + ] + + lines += [ + '', + '', + ] + + lines += ['FIELDDEFS = {'] + for name, fields in typs.items(): + pyname = name.replace('/', '__') + lines += [ + f' \'{name}\': [', + *[ + f' ({repr(fname[1])}, {json.loads(json.dumps(ftype))}),' + for ftype, fname in fields + ], + ' ],', + ] + lines += [ + '}', + '', + ] + return '\n'.join(lines) + + +def register_types(typs: Typesdict) -> None: + """Register types in type system. + + Args: + typs: Dictionary mapping message typenames to parsetrees. + + Raises: + TypesysError: Type already present with different definition. + """ + code = generate_python_code(typs) + name = 'rosbags.usertypes' + spec = spec_from_loader(name, loader=None) + module = module_from_spec(spec) + sys.modules[name] = module + exec(code, module.__dict__) # pylint: disable=exec-used + fielddefs = module.FIELDDEFS # type: ignore + + for name, fields in fielddefs.items(): + if name == 'std_msgs/msg/Header': + continue + if have := types.FIELDDEFS.get(name): + have = [(x[0].lower(), x[1]) for x in have] + fields = [(x[0].lower(), x[1]) for x in fields] + if have != fields: + raise TypesysError(f'Type {name!r} is already present with different definition.') + + for name in fielddefs.keys() - types.FIELDDEFS.keys(): + pyname = name.replace('/', '__') + setattr(types, pyname, getattr(module, pyname)) + types.FIELDDEFS[name] = fielddefs[name] diff --git a/src/rosbags/typesys/types.py b/src/rosbags/typesys/types.py new file mode 100644 index 00000000..3c484174 --- /dev/null +++ b/src/rosbags/typesys/types.py @@ -0,0 +1,2420 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +# +# THIS FILE IS GENERATED, DO NOT EDIT +"""ROS2 message types.""" + +# flake8: noqa N801 +# pylint: disable=invalid-name,too-many-instance-attributes,too-many-lines + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from typing import Any + + +@dataclass +class builtin_interfaces__msg__Time: + """Class for builtin_interfaces/msg/Time.""" + + sec: Any + nanosec: Any + + +@dataclass +class builtin_interfaces__msg__Duration: + """Class for builtin_interfaces/msg/Duration.""" + + sec: Any + nanosec: Any + + +@dataclass +class diagnostic_msgs__msg__DiagnosticStatus: + """Class for diagnostic_msgs/msg/DiagnosticStatus.""" + + level: Any + name: Any + message: Any + hardware_id: Any + values: Any + + +@dataclass +class diagnostic_msgs__msg__DiagnosticArray: + """Class for diagnostic_msgs/msg/DiagnosticArray.""" + + header: Any + status: Any + + +@dataclass +class diagnostic_msgs__msg__KeyValue: + """Class for diagnostic_msgs/msg/KeyValue.""" + + key: Any + value: Any + + +@dataclass +class geometry_msgs__msg__AccelWithCovariance: + """Class for geometry_msgs/msg/AccelWithCovariance.""" + + accel: Any + covariance: Any + + +@dataclass +class geometry_msgs__msg__Point32: + """Class for geometry_msgs/msg/Point32.""" + + x: Any + y: Any + z: Any + + +@dataclass +class geometry_msgs__msg__Vector3: + """Class for geometry_msgs/msg/Vector3.""" + + x: Any + y: Any + z: Any + + +@dataclass +class geometry_msgs__msg__Inertia: + """Class for geometry_msgs/msg/Inertia.""" + + m: Any + com: Any + ixx: Any + ixy: Any + ixz: Any + iyy: Any + iyz: Any + izz: Any + + +@dataclass +class geometry_msgs__msg__PoseWithCovarianceStamped: + """Class for geometry_msgs/msg/PoseWithCovarianceStamped.""" + + header: Any + pose: Any + + +@dataclass +class geometry_msgs__msg__Twist: + """Class for geometry_msgs/msg/Twist.""" + + linear: Any + angular: Any + + +@dataclass +class geometry_msgs__msg__Pose: + """Class for geometry_msgs/msg/Pose.""" + + position: Any + orientation: Any + + +@dataclass +class geometry_msgs__msg__Point: + """Class for geometry_msgs/msg/Point.""" + + x: Any + y: Any + z: Any + + +@dataclass +class geometry_msgs__msg__Vector3Stamped: + """Class for geometry_msgs/msg/Vector3Stamped.""" + + header: Any + vector: Any + + +@dataclass +class geometry_msgs__msg__Transform: + """Class for geometry_msgs/msg/Transform.""" + + translation: Any + rotation: Any + + +@dataclass +class geometry_msgs__msg__PolygonStamped: + """Class for geometry_msgs/msg/PolygonStamped.""" + + header: Any + polygon: Any + + +@dataclass +class geometry_msgs__msg__Quaternion: + """Class for geometry_msgs/msg/Quaternion.""" + + x: Any + y: Any + z: Any + w: Any + + +@dataclass +class geometry_msgs__msg__Pose2D: + """Class for geometry_msgs/msg/Pose2D.""" + + x: Any + y: Any + theta: Any + + +@dataclass +class geometry_msgs__msg__InertiaStamped: + """Class for geometry_msgs/msg/InertiaStamped.""" + + header: Any + inertia: Any + + +@dataclass +class geometry_msgs__msg__TwistStamped: + """Class for geometry_msgs/msg/TwistStamped.""" + + header: Any + twist: Any + + +@dataclass +class geometry_msgs__msg__PoseStamped: + """Class for geometry_msgs/msg/PoseStamped.""" + + header: Any + pose: Any + + +@dataclass +class geometry_msgs__msg__PointStamped: + """Class for geometry_msgs/msg/PointStamped.""" + + header: Any + point: Any + + +@dataclass +class geometry_msgs__msg__Polygon: + """Class for geometry_msgs/msg/Polygon.""" + + points: Any + + +@dataclass +class geometry_msgs__msg__PoseArray: + """Class for geometry_msgs/msg/PoseArray.""" + + header: Any + poses: Any + + +@dataclass +class geometry_msgs__msg__AccelStamped: + """Class for geometry_msgs/msg/AccelStamped.""" + + header: Any + accel: Any + + +@dataclass +class geometry_msgs__msg__TwistWithCovarianceStamped: + """Class for geometry_msgs/msg/TwistWithCovarianceStamped.""" + + header: Any + twist: Any + + +@dataclass +class geometry_msgs__msg__QuaternionStamped: + """Class for geometry_msgs/msg/QuaternionStamped.""" + + header: Any + quaternion: Any + + +@dataclass +class geometry_msgs__msg__WrenchStamped: + """Class for geometry_msgs/msg/WrenchStamped.""" + + header: Any + wrench: Any + + +@dataclass +class geometry_msgs__msg__AccelWithCovarianceStamped: + """Class for geometry_msgs/msg/AccelWithCovarianceStamped.""" + + header: Any + accel: Any + + +@dataclass +class geometry_msgs__msg__PoseWithCovariance: + """Class for geometry_msgs/msg/PoseWithCovariance.""" + + pose: Any + covariance: Any + + +@dataclass +class geometry_msgs__msg__Wrench: + """Class for geometry_msgs/msg/Wrench.""" + + force: Any + torque: Any + + +@dataclass +class geometry_msgs__msg__TransformStamped: + """Class for geometry_msgs/msg/TransformStamped.""" + + header: Any + child_frame_id: Any + transform: Any + + +@dataclass +class geometry_msgs__msg__Accel: + """Class for geometry_msgs/msg/Accel.""" + + linear: Any + angular: Any + + +@dataclass +class geometry_msgs__msg__TwistWithCovariance: + """Class for geometry_msgs/msg/TwistWithCovariance.""" + + twist: Any + covariance: Any + + +@dataclass +class libstatistics_collector__msg__DummyMessage: + """Class for libstatistics_collector/msg/DummyMessage.""" + + header: Any + + +@dataclass +class lifecycle_msgs__msg__TransitionDescription: + """Class for lifecycle_msgs/msg/TransitionDescription.""" + + transition: Any + start_state: Any + goal_state: Any + + +@dataclass +class lifecycle_msgs__msg__State: + """Class for lifecycle_msgs/msg/State.""" + + id: Any + label: Any + + +@dataclass +class lifecycle_msgs__msg__TransitionEvent: + """Class for lifecycle_msgs/msg/TransitionEvent.""" + + timestamp: Any + transition: Any + start_state: Any + goal_state: Any + + +@dataclass +class lifecycle_msgs__msg__Transition: + """Class for lifecycle_msgs/msg/Transition.""" + + id: Any + label: Any + + +@dataclass +class nav_msgs__msg__MapMetaData: + """Class for nav_msgs/msg/MapMetaData.""" + + map_load_time: Any + resolution: Any + width: Any + height: Any + origin: Any + + +@dataclass +class nav_msgs__msg__GridCells: + """Class for nav_msgs/msg/GridCells.""" + + header: Any + cell_width: Any + cell_height: Any + cells: Any + + +@dataclass +class nav_msgs__msg__Odometry: + """Class for nav_msgs/msg/Odometry.""" + + header: Any + child_frame_id: Any + pose: Any + twist: Any + + +@dataclass +class nav_msgs__msg__Path: + """Class for nav_msgs/msg/Path.""" + + header: Any + poses: Any + + +@dataclass +class nav_msgs__msg__OccupancyGrid: + """Class for nav_msgs/msg/OccupancyGrid.""" + + header: Any + info: Any + data: Any + + +@dataclass +class rcl_interfaces__msg__ListParametersResult: + """Class for rcl_interfaces/msg/ListParametersResult.""" + + names: Any + prefixes: Any + + +@dataclass +class rcl_interfaces__msg__ParameterType: + """Class for rcl_interfaces/msg/ParameterType.""" + + structure_needs_at_least_one_member: Any + + +@dataclass +class rcl_interfaces__msg__ParameterEventDescriptors: + """Class for rcl_interfaces/msg/ParameterEventDescriptors.""" + + new_parameters: Any + changed_parameters: Any + deleted_parameters: Any + + +@dataclass +class rcl_interfaces__msg__ParameterEvent: + """Class for rcl_interfaces/msg/ParameterEvent.""" + + stamp: Any + node: Any + new_parameters: Any + changed_parameters: Any + deleted_parameters: Any + + +@dataclass +class rcl_interfaces__msg__IntegerRange: + """Class for rcl_interfaces/msg/IntegerRange.""" + + from_value: Any + to_value: Any + step: Any + + +@dataclass +class rcl_interfaces__msg__Parameter: + """Class for rcl_interfaces/msg/Parameter.""" + + name: Any + value: Any + + +@dataclass +class rcl_interfaces__msg__ParameterValue: + """Class for rcl_interfaces/msg/ParameterValue.""" + + type: Any + bool_value: Any + integer_value: Any + double_value: Any + string_value: Any + byte_array_value: Any + bool_array_value: Any + integer_array_value: Any + double_array_value: Any + string_array_value: Any + + +@dataclass +class rcl_interfaces__msg__FloatingPointRange: + """Class for rcl_interfaces/msg/FloatingPointRange.""" + + from_value: Any + to_value: Any + step: Any + + +@dataclass +class rcl_interfaces__msg__SetParametersResult: + """Class for rcl_interfaces/msg/SetParametersResult.""" + + successful: Any + reason: Any + + +@dataclass +class rcl_interfaces__msg__Log: + """Class for rcl_interfaces/msg/Log.""" + + stamp: Any + level: Any + name: Any + msg: Any + file: Any + function: Any + line: Any + + +@dataclass +class rcl_interfaces__msg__ParameterDescriptor: + """Class for rcl_interfaces/msg/ParameterDescriptor.""" + + name: Any + type: Any + description: Any + additional_constraints: Any + read_only: Any + floating_point_range: Any + integer_range: Any + + +@dataclass +class rmw_dds_common__msg__Gid: + """Class for rmw_dds_common/msg/Gid.""" + + data: Any + + +@dataclass +class rmw_dds_common__msg__NodeEntitiesInfo: + """Class for rmw_dds_common/msg/NodeEntitiesInfo.""" + + node_namespace: Any + node_name: Any + reader_gid_seq: Any + writer_gid_seq: Any + + +@dataclass +class rmw_dds_common__msg__ParticipantEntitiesInfo: + """Class for rmw_dds_common/msg/ParticipantEntitiesInfo.""" + + gid: Any + node_entities_info_seq: Any + + +@dataclass +class rosgraph_msgs__msg__Clock: + """Class for rosgraph_msgs/msg/Clock.""" + + clock: Any + + +@dataclass +class sensor_msgs__msg__Temperature: + """Class for sensor_msgs/msg/Temperature.""" + + header: Any + temperature: Any + variance: Any + + +@dataclass +class sensor_msgs__msg__Range: + """Class for sensor_msgs/msg/Range.""" + + header: Any + radiation_type: Any + field_of_view: Any + min_range: Any + max_range: Any + range: Any + + +@dataclass +class sensor_msgs__msg__RegionOfInterest: + """Class for sensor_msgs/msg/RegionOfInterest.""" + + x_offset: Any + y_offset: Any + height: Any + width: Any + do_rectify: Any + + +@dataclass +class sensor_msgs__msg__JoyFeedbackArray: + """Class for sensor_msgs/msg/JoyFeedbackArray.""" + + array: Any + + +@dataclass +class sensor_msgs__msg__TimeReference: + """Class for sensor_msgs/msg/TimeReference.""" + + header: Any + time_ref: Any + source: Any + + +@dataclass +class sensor_msgs__msg__CompressedImage: + """Class for sensor_msgs/msg/CompressedImage.""" + + header: Any + format: Any + data: Any + + +@dataclass +class sensor_msgs__msg__MultiEchoLaserScan: + """Class for sensor_msgs/msg/MultiEchoLaserScan.""" + + header: Any + angle_min: Any + angle_max: Any + angle_increment: Any + time_increment: Any + scan_time: Any + range_min: Any + range_max: Any + ranges: Any + intensities: Any + + +@dataclass +class sensor_msgs__msg__LaserEcho: + """Class for sensor_msgs/msg/LaserEcho.""" + + echoes: Any + + +@dataclass +class sensor_msgs__msg__ChannelFloat32: + """Class for sensor_msgs/msg/ChannelFloat32.""" + + name: Any + values: Any + + +@dataclass +class sensor_msgs__msg__CameraInfo: + """Class for sensor_msgs/msg/CameraInfo.""" + + header: Any + height: Any + width: Any + distortion_model: Any + d: Any + k: Any + r: Any + p: Any + binning_x: Any + binning_y: Any + roi: Any + + +@dataclass +class sensor_msgs__msg__RelativeHumidity: + """Class for sensor_msgs/msg/RelativeHumidity.""" + + header: Any + relative_humidity: Any + variance: Any + + +@dataclass +class sensor_msgs__msg__FluidPressure: + """Class for sensor_msgs/msg/FluidPressure.""" + + header: Any + fluid_pressure: Any + variance: Any + + +@dataclass +class sensor_msgs__msg__LaserScan: + """Class for sensor_msgs/msg/LaserScan.""" + + header: Any + angle_min: Any + angle_max: Any + angle_increment: Any + time_increment: Any + scan_time: Any + range_min: Any + range_max: Any + ranges: Any + intensities: Any + + +@dataclass +class sensor_msgs__msg__BatteryState: + """Class for sensor_msgs/msg/BatteryState.""" + + header: Any + voltage: Any + temperature: Any + current: Any + charge: Any + capacity: Any + design_capacity: Any + percentage: Any + power_supply_status: Any + power_supply_health: Any + power_supply_technology: Any + present: Any + cell_voltage: Any + cell_temperature: Any + location: Any + serial_number: Any + + +@dataclass +class sensor_msgs__msg__Image: + """Class for sensor_msgs/msg/Image.""" + + header: Any + height: Any + width: Any + encoding: Any + is_bigendian: Any + step: Any + data: Any + + +@dataclass +class sensor_msgs__msg__PointCloud: + """Class for sensor_msgs/msg/PointCloud.""" + + header: Any + points: Any + channels: Any + + +@dataclass +class sensor_msgs__msg__Imu: + """Class for sensor_msgs/msg/Imu.""" + + header: Any + orientation: Any + orientation_covariance: Any + angular_velocity: Any + angular_velocity_covariance: Any + linear_acceleration: Any + linear_acceleration_covariance: Any + + +@dataclass +class sensor_msgs__msg__NavSatStatus: + """Class for sensor_msgs/msg/NavSatStatus.""" + + status: Any + service: Any + + +@dataclass +class sensor_msgs__msg__Illuminance: + """Class for sensor_msgs/msg/Illuminance.""" + + header: Any + illuminance: Any + variance: Any + + +@dataclass +class sensor_msgs__msg__Joy: + """Class for sensor_msgs/msg/Joy.""" + + header: Any + axes: Any + buttons: Any + + +@dataclass +class sensor_msgs__msg__NavSatFix: + """Class for sensor_msgs/msg/NavSatFix.""" + + header: Any + status: Any + latitude: Any + longitude: Any + altitude: Any + position_covariance: Any + position_covariance_type: Any + + +@dataclass +class sensor_msgs__msg__MultiDOFJointState: + """Class for sensor_msgs/msg/MultiDOFJointState.""" + + header: Any + joint_names: Any + transforms: Any + twist: Any + wrench: Any + + +@dataclass +class sensor_msgs__msg__MagneticField: + """Class for sensor_msgs/msg/MagneticField.""" + + header: Any + magnetic_field: Any + magnetic_field_covariance: Any + + +@dataclass +class sensor_msgs__msg__JointState: + """Class for sensor_msgs/msg/JointState.""" + + header: Any + name: Any + position: Any + velocity: Any + effort: Any + + +@dataclass +class sensor_msgs__msg__PointField: + """Class for sensor_msgs/msg/PointField.""" + + name: Any + offset: Any + datatype: Any + count: Any + + +@dataclass +class sensor_msgs__msg__PointCloud2: + """Class for sensor_msgs/msg/PointCloud2.""" + + header: Any + height: Any + width: Any + fields: Any + is_bigendian: Any + point_step: Any + row_step: Any + data: Any + is_dense: Any + + +@dataclass +class sensor_msgs__msg__JoyFeedback: + """Class for sensor_msgs/msg/JoyFeedback.""" + + type: Any + id: Any + intensity: Any + + +@dataclass +class shape_msgs__msg__SolidPrimitive: + """Class for shape_msgs/msg/SolidPrimitive.""" + + type: Any + dimensions: Any + + +@dataclass +class shape_msgs__msg__Mesh: + """Class for shape_msgs/msg/Mesh.""" + + triangles: Any + vertices: Any + + +@dataclass +class shape_msgs__msg__Plane: + """Class for shape_msgs/msg/Plane.""" + + coef: Any + + +@dataclass +class shape_msgs__msg__MeshTriangle: + """Class for shape_msgs/msg/MeshTriangle.""" + + vertex_indices: Any + + +@dataclass +class statistics_msgs__msg__StatisticDataType: + """Class for statistics_msgs/msg/StatisticDataType.""" + + structure_needs_at_least_one_member: Any + + +@dataclass +class statistics_msgs__msg__StatisticDataPoint: + """Class for statistics_msgs/msg/StatisticDataPoint.""" + + data_type: Any + data: Any + + +@dataclass +class statistics_msgs__msg__MetricsMessage: + """Class for statistics_msgs/msg/MetricsMessage.""" + + measurement_source_name: Any + metrics_source: Any + unit: Any + window_start: Any + window_stop: Any + statistics: Any + + +@dataclass +class std_msgs__msg__UInt8: + """Class for std_msgs/msg/UInt8.""" + + data: Any + + +@dataclass +class std_msgs__msg__Float32MultiArray: + """Class for std_msgs/msg/Float32MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Int8: + """Class for std_msgs/msg/Int8.""" + + data: Any + + +@dataclass +class std_msgs__msg__Empty: + """Class for std_msgs/msg/Empty.""" + + structure_needs_at_least_one_member: Any + + +@dataclass +class std_msgs__msg__String: + """Class for std_msgs/msg/String.""" + + data: Any + + +@dataclass +class std_msgs__msg__MultiArrayDimension: + """Class for std_msgs/msg/MultiArrayDimension.""" + + label: Any + size: Any + stride: Any + + +@dataclass +class std_msgs__msg__UInt64: + """Class for std_msgs/msg/UInt64.""" + + data: Any + + +@dataclass +class std_msgs__msg__UInt16: + """Class for std_msgs/msg/UInt16.""" + + data: Any + + +@dataclass +class std_msgs__msg__Float32: + """Class for std_msgs/msg/Float32.""" + + data: Any + + +@dataclass +class std_msgs__msg__Int64: + """Class for std_msgs/msg/Int64.""" + + data: Any + + +@dataclass +class std_msgs__msg__Int16MultiArray: + """Class for std_msgs/msg/Int16MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Int16: + """Class for std_msgs/msg/Int16.""" + + data: Any + + +@dataclass +class std_msgs__msg__Float64MultiArray: + """Class for std_msgs/msg/Float64MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__MultiArrayLayout: + """Class for std_msgs/msg/MultiArrayLayout.""" + + dim: Any + data_offset: Any + + +@dataclass +class std_msgs__msg__UInt32MultiArray: + """Class for std_msgs/msg/UInt32MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Header: + """Class for std_msgs/msg/Header.""" + + stamp: Any + frame_id: Any + + +@dataclass +class std_msgs__msg__ByteMultiArray: + """Class for std_msgs/msg/ByteMultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Int8MultiArray: + """Class for std_msgs/msg/Int8MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Float64: + """Class for std_msgs/msg/Float64.""" + + data: Any + + +@dataclass +class std_msgs__msg__UInt8MultiArray: + """Class for std_msgs/msg/UInt8MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Byte: + """Class for std_msgs/msg/Byte.""" + + data: Any + + +@dataclass +class std_msgs__msg__Char: + """Class for std_msgs/msg/Char.""" + + data: Any + + +@dataclass +class std_msgs__msg__UInt64MultiArray: + """Class for std_msgs/msg/UInt64MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Int32MultiArray: + """Class for std_msgs/msg/Int32MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__ColorRGBA: + """Class for std_msgs/msg/ColorRGBA.""" + + r: Any + g: Any + b: Any + a: Any + + +@dataclass +class std_msgs__msg__Bool: + """Class for std_msgs/msg/Bool.""" + + data: Any + + +@dataclass +class std_msgs__msg__UInt32: + """Class for std_msgs/msg/UInt32.""" + + data: Any + + +@dataclass +class std_msgs__msg__Int64MultiArray: + """Class for std_msgs/msg/Int64MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class std_msgs__msg__Int32: + """Class for std_msgs/msg/Int32.""" + + data: Any + + +@dataclass +class std_msgs__msg__UInt16MultiArray: + """Class for std_msgs/msg/UInt16MultiArray.""" + + layout: Any + data: Any + + +@dataclass +class stereo_msgs__msg__DisparityImage: + """Class for stereo_msgs/msg/DisparityImage.""" + + header: Any + image: Any + f: Any + t: Any + valid_window: Any + min_disparity: Any + max_disparity: Any + delta_d: Any + + +@dataclass +class tf2_msgs__msg__TF2Error: + """Class for tf2_msgs/msg/TF2Error.""" + + error: Any + error_string: Any + + +@dataclass +class tf2_msgs__msg__TFMessage: + """Class for tf2_msgs/msg/TFMessage.""" + + transforms: Any + + +@dataclass +class trajectory_msgs__msg__MultiDOFJointTrajectory: + """Class for trajectory_msgs/msg/MultiDOFJointTrajectory.""" + + header: Any + joint_names: Any + points: Any + + +@dataclass +class trajectory_msgs__msg__JointTrajectory: + """Class for trajectory_msgs/msg/JointTrajectory.""" + + header: Any + joint_names: Any + points: Any + + +@dataclass +class trajectory_msgs__msg__JointTrajectoryPoint: + """Class for trajectory_msgs/msg/JointTrajectoryPoint.""" + + positions: Any + velocities: Any + accelerations: Any + effort: Any + time_from_start: Any + + +@dataclass +class trajectory_msgs__msg__MultiDOFJointTrajectoryPoint: + """Class for trajectory_msgs/msg/MultiDOFJointTrajectoryPoint.""" + + transforms: Any + velocities: Any + accelerations: Any + time_from_start: Any + + +@dataclass +class unique_identifier_msgs__msg__UUID: + """Class for unique_identifier_msgs/msg/UUID.""" + + uuid: Any + + +@dataclass +class visualization_msgs__msg__Marker: + """Class for visualization_msgs/msg/Marker.""" + + header: Any + ns: Any + id: Any + type: Any + action: Any + pose: Any + scale: Any + color: Any + lifetime: Any + frame_locked: Any + points: Any + colors: Any + text: Any + mesh_resource: Any + mesh_use_embedded_materials: Any + + +@dataclass +class visualization_msgs__msg__InteractiveMarkerInit: + """Class for visualization_msgs/msg/InteractiveMarkerInit.""" + + server_id: Any + seq_num: Any + markers: Any + + +@dataclass +class visualization_msgs__msg__MenuEntry: + """Class for visualization_msgs/msg/MenuEntry.""" + + id: Any + parent_id: Any + title: Any + command: Any + command_type: Any + + +@dataclass +class visualization_msgs__msg__MarkerArray: + """Class for visualization_msgs/msg/MarkerArray.""" + + markers: Any + + +@dataclass +class visualization_msgs__msg__InteractiveMarkerUpdate: + """Class for visualization_msgs/msg/InteractiveMarkerUpdate.""" + + server_id: Any + seq_num: Any + type: Any + markers: Any + poses: Any + erases: Any + + +@dataclass +class visualization_msgs__msg__InteractiveMarker: + """Class for visualization_msgs/msg/InteractiveMarker.""" + + header: Any + pose: Any + name: Any + description: Any + scale: Any + menu_entries: Any + controls: Any + + +@dataclass +class visualization_msgs__msg__InteractiveMarkerFeedback: + """Class for visualization_msgs/msg/InteractiveMarkerFeedback.""" + + header: Any + client_id: Any + marker_name: Any + control_name: Any + event_type: Any + pose: Any + menu_entry_id: Any + mouse_point: Any + mouse_point_valid: Any + + +@dataclass +class visualization_msgs__msg__ImageMarker: + """Class for visualization_msgs/msg/ImageMarker.""" + + header: Any + ns: Any + id: Any + type: Any + action: Any + position: Any + scale: Any + outline_color: Any + filled: Any + fill_color: Any + lifetime: Any + points: Any + outline_colors: Any + + +@dataclass +class visualization_msgs__msg__InteractiveMarkerControl: + """Class for visualization_msgs/msg/InteractiveMarkerControl.""" + + name: Any + orientation: Any + orientation_mode: Any + interaction_mode: Any + always_visible: Any + markers: Any + independent_marker_orientation: Any + description: Any + + +@dataclass +class visualization_msgs__msg__InteractiveMarkerPose: + """Class for visualization_msgs/msg/InteractiveMarkerPose.""" + + header: Any + pose: Any + name: Any + + +@dataclass +class autoware_auto_msgs__msg__BoundingBox: + """Class for autoware_auto_msgs/msg/BoundingBox.""" + + centroid: Any + size: Any + orientation: Any + velocity: Any + heading: Any + heading_rate: Any + corners: Any + variance: Any + value: Any + vehicle_label: Any + signal_label: Any + class_likelihood: Any + + +@dataclass +class autoware_auto_msgs__msg__HADMapBin: + """Class for autoware_auto_msgs/msg/HADMapBin.""" + + header: Any + map_format: Any + format_version: Any + map_version: Any + data: Any + + +@dataclass +class autoware_auto_msgs__msg__Quaternion32: + """Class for autoware_auto_msgs/msg/Quaternion32.""" + + x: Any + y: Any + z: Any + w: Any + + +@dataclass +class autoware_auto_msgs__msg__Trajectory: + """Class for autoware_auto_msgs/msg/Trajectory.""" + + header: Any + points: Any + + +@dataclass +class autoware_auto_msgs__msg__Route: + """Class for autoware_auto_msgs/msg/Route.""" + + header: Any + start_point: Any + goal_point: Any + primitives: Any + + +@dataclass +class autoware_auto_msgs__msg__MapPrimitive: + """Class for autoware_auto_msgs/msg/MapPrimitive.""" + + id: Any + primitive_type: Any + + +@dataclass +class autoware_auto_msgs__msg__TrajectoryPoint: + """Class for autoware_auto_msgs/msg/TrajectoryPoint.""" + + time_from_start: Any + x: Any + y: Any + heading: Any + longitudinal_velocity_mps: Any + lateral_velocity_mps: Any + acceleration_mps2: Any + heading_rate_rps: Any + front_wheel_angle_rad: Any + rear_wheel_angle_rad: Any + + +@dataclass +class autoware_auto_msgs__msg__Complex32: + """Class for autoware_auto_msgs/msg/Complex32.""" + + real: Any + imag: Any + + +@dataclass +class autoware_auto_msgs__msg__VehicleOdometry: + """Class for autoware_auto_msgs/msg/VehicleOdometry.""" + + stamp: Any + velocity_mps: Any + front_wheel_angle_rad: Any + rear_wheel_angle_rad: Any + + +@dataclass +class autoware_auto_msgs__msg__DiagnosticHeader: + """Class for autoware_auto_msgs/msg/DiagnosticHeader.""" + + name: Any + data_stamp: Any + computation_start: Any + runtime: Any + iterations: Any + + +@dataclass +class autoware_auto_msgs__msg__HighLevelControlCommand: + """Class for autoware_auto_msgs/msg/HighLevelControlCommand.""" + + stamp: Any + velocity_mps: Any + curvature: Any + + +@dataclass +class autoware_auto_msgs__msg__VehicleStateCommand: + """Class for autoware_auto_msgs/msg/VehicleStateCommand.""" + + stamp: Any + blinker: Any + headlight: Any + wiper: Any + gear: Any + mode: Any + hand_brake: Any + horn: Any + + +@dataclass +class autoware_auto_msgs__msg__PointClusters: + """Class for autoware_auto_msgs/msg/PointClusters.""" + + clusters: Any + + +@dataclass +class autoware_auto_msgs__msg__RawControlCommand: + """Class for autoware_auto_msgs/msg/RawControlCommand.""" + + stamp: Any + throttle: Any + brake: Any + front_steer: Any + rear_steer: Any + + +@dataclass +class autoware_auto_msgs__msg__VehicleStateReport: + """Class for autoware_auto_msgs/msg/VehicleStateReport.""" + + stamp: Any + fuel: Any + blinker: Any + headlight: Any + wiper: Any + gear: Any + mode: Any + hand_brake: Any + horn: Any + + +@dataclass +class autoware_auto_msgs__msg__ControlDiagnostic: + """Class for autoware_auto_msgs/msg/ControlDiagnostic.""" + + diag_header: Any + new_trajectory: Any + trajectory_source: Any + pose_source: Any + lateral_error_m: Any + longitudinal_error_m: Any + velocity_error_mps: Any + acceleration_error_mps2: Any + yaw_error_rad: Any + yaw_rate_error_rps: Any + + +@dataclass +class autoware_auto_msgs__msg__VehicleKinematicState: + """Class for autoware_auto_msgs/msg/VehicleKinematicState.""" + + header: Any + state: Any + delta: Any + + +@dataclass +class autoware_auto_msgs__msg__BoundingBoxArray: + """Class for autoware_auto_msgs/msg/BoundingBoxArray.""" + + header: Any + boxes: Any + + +@dataclass +class autoware_auto_msgs__msg__VehicleControlCommand: + """Class for autoware_auto_msgs/msg/VehicleControlCommand.""" + + stamp: Any + long_accel_mps2: Any + velocity_mps: Any + front_wheel_angle_rad: Any + rear_wheel_angle_rad: Any + + +FIELDDEFS = { + 'builtin_interfaces/msg/Time': [ + ('sec', [1, 'int32']), + ('nanosec', [1, 'uint32']), + ], + 'builtin_interfaces/msg/Duration': [ + ('sec', [1, 'int32']), + ('nanosec', [1, 'uint32']), + ], + 'diagnostic_msgs/msg/DiagnosticStatus': [ + ('level', [1, 'uint8']), + ('name', [1, 'string']), + ('message', [1, 'string']), + ('hardware_id', [1, 'string']), + ('values', [4, [2, 'diagnostic_msgs/msg/KeyValue']]), + ], + 'diagnostic_msgs/msg/DiagnosticArray': [ + ('header', [2, 'std_msgs/msg/Header']), + ('status', [4, [2, 'diagnostic_msgs/msg/DiagnosticStatus']]), + ], + 'diagnostic_msgs/msg/KeyValue': [ + ('key', [1, 'string']), + ('value', [1, 'string']), + ], + 'geometry_msgs/msg/AccelWithCovariance': [ + ('accel', [2, 'geometry_msgs/msg/Accel']), + ('covariance', [3, 36, [1, 'float64']]), + ], + 'geometry_msgs/msg/Point32': [ + ('x', [1, 'float32']), + ('y', [1, 'float32']), + ('z', [1, 'float32']), + ], + 'geometry_msgs/msg/Vector3': [ + ('x', [1, 'float64']), + ('y', [1, 'float64']), + ('z', [1, 'float64']), + ], + 'geometry_msgs/msg/Inertia': [ + ('m', [1, 'float64']), + ('com', [2, 'geometry_msgs/msg/Vector3']), + ('ixx', [1, 'float64']), + ('ixy', [1, 'float64']), + ('ixz', [1, 'float64']), + ('iyy', [1, 'float64']), + ('iyz', [1, 'float64']), + ('izz', [1, 'float64']), + ], + 'geometry_msgs/msg/PoseWithCovarianceStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('pose', [2, 'geometry_msgs/msg/PoseWithCovariance']), + ], + 'geometry_msgs/msg/Twist': [ + ('linear', [2, 'geometry_msgs/msg/Vector3']), + ('angular', [2, 'geometry_msgs/msg/Vector3']), + ], + 'geometry_msgs/msg/Pose': [ + ('position', [2, 'geometry_msgs/msg/Point']), + ('orientation', [2, 'geometry_msgs/msg/Quaternion']), + ], + 'geometry_msgs/msg/Point': [ + ('x', [1, 'float64']), + ('y', [1, 'float64']), + ('z', [1, 'float64']), + ], + 'geometry_msgs/msg/Vector3Stamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('vector', [2, 'geometry_msgs/msg/Vector3']), + ], + 'geometry_msgs/msg/Transform': [ + ('translation', [2, 'geometry_msgs/msg/Vector3']), + ('rotation', [2, 'geometry_msgs/msg/Quaternion']), + ], + 'geometry_msgs/msg/PolygonStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('polygon', [2, 'geometry_msgs/msg/Polygon']), + ], + 'geometry_msgs/msg/Quaternion': [ + ('x', [1, 'float64']), + ('y', [1, 'float64']), + ('z', [1, 'float64']), + ('w', [1, 'float64']), + ], + 'geometry_msgs/msg/Pose2D': [ + ('x', [1, 'float64']), + ('y', [1, 'float64']), + ('theta', [1, 'float64']), + ], + 'geometry_msgs/msg/InertiaStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('inertia', [2, 'geometry_msgs/msg/Inertia']), + ], + 'geometry_msgs/msg/TwistStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('twist', [2, 'geometry_msgs/msg/Twist']), + ], + 'geometry_msgs/msg/PoseStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('pose', [2, 'geometry_msgs/msg/Pose']), + ], + 'geometry_msgs/msg/PointStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('point', [2, 'geometry_msgs/msg/Point']), + ], + 'geometry_msgs/msg/Polygon': [ + ('points', [4, [2, 'geometry_msgs/msg/Point32']]), + ], + 'geometry_msgs/msg/PoseArray': [ + ('header', [2, 'std_msgs/msg/Header']), + ('poses', [4, [2, 'geometry_msgs/msg/Pose']]), + ], + 'geometry_msgs/msg/AccelStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('accel', [2, 'geometry_msgs/msg/Accel']), + ], + 'geometry_msgs/msg/TwistWithCovarianceStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('twist', [2, 'geometry_msgs/msg/TwistWithCovariance']), + ], + 'geometry_msgs/msg/QuaternionStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('quaternion', [2, 'geometry_msgs/msg/Quaternion']), + ], + 'geometry_msgs/msg/WrenchStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('wrench', [2, 'geometry_msgs/msg/Wrench']), + ], + 'geometry_msgs/msg/AccelWithCovarianceStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('accel', [2, 'geometry_msgs/msg/AccelWithCovariance']), + ], + 'geometry_msgs/msg/PoseWithCovariance': [ + ('pose', [2, 'geometry_msgs/msg/Pose']), + ('covariance', [3, 36, [1, 'float64']]), + ], + 'geometry_msgs/msg/Wrench': [ + ('force', [2, 'geometry_msgs/msg/Vector3']), + ('torque', [2, 'geometry_msgs/msg/Vector3']), + ], + 'geometry_msgs/msg/TransformStamped': [ + ('header', [2, 'std_msgs/msg/Header']), + ('child_frame_id', [1, 'string']), + ('transform', [2, 'geometry_msgs/msg/Transform']), + ], + 'geometry_msgs/msg/Accel': [ + ('linear', [2, 'geometry_msgs/msg/Vector3']), + ('angular', [2, 'geometry_msgs/msg/Vector3']), + ], + 'geometry_msgs/msg/TwistWithCovariance': [ + ('twist', [2, 'geometry_msgs/msg/Twist']), + ('covariance', [3, 36, [1, 'float64']]), + ], + 'libstatistics_collector/msg/DummyMessage': [ + ('header', [2, 'std_msgs/msg/Header']), + ], + 'lifecycle_msgs/msg/TransitionDescription': [ + ('transition', [2, 'lifecycle_msgs/msg/Transition']), + ('start_state', [2, 'lifecycle_msgs/msg/State']), + ('goal_state', [2, 'lifecycle_msgs/msg/State']), + ], + 'lifecycle_msgs/msg/State': [ + ('id', [1, 'uint8']), + ('label', [1, 'string']), + ], + 'lifecycle_msgs/msg/TransitionEvent': [ + ('timestamp', [1, 'uint64']), + ('transition', [2, 'lifecycle_msgs/msg/Transition']), + ('start_state', [2, 'lifecycle_msgs/msg/State']), + ('goal_state', [2, 'lifecycle_msgs/msg/State']), + ], + 'lifecycle_msgs/msg/Transition': [ + ('id', [1, 'uint8']), + ('label', [1, 'string']), + ], + 'nav_msgs/msg/MapMetaData': [ + ('map_load_time', [2, 'builtin_interfaces/msg/Time']), + ('resolution', [1, 'float32']), + ('width', [1, 'uint32']), + ('height', [1, 'uint32']), + ('origin', [2, 'geometry_msgs/msg/Pose']), + ], + 'nav_msgs/msg/GridCells': [ + ('header', [2, 'std_msgs/msg/Header']), + ('cell_width', [1, 'float32']), + ('cell_height', [1, 'float32']), + ('cells', [4, [2, 'geometry_msgs/msg/Point']]), + ], + 'nav_msgs/msg/Odometry': [ + ('header', [2, 'std_msgs/msg/Header']), + ('child_frame_id', [1, 'string']), + ('pose', [2, 'geometry_msgs/msg/PoseWithCovariance']), + ('twist', [2, 'geometry_msgs/msg/TwistWithCovariance']), + ], + 'nav_msgs/msg/Path': [ + ('header', [2, 'std_msgs/msg/Header']), + ('poses', [4, [2, 'geometry_msgs/msg/PoseStamped']]), + ], + 'nav_msgs/msg/OccupancyGrid': [ + ('header', [2, 'std_msgs/msg/Header']), + ('info', [2, 'nav_msgs/msg/MapMetaData']), + ('data', [4, [1, 'int8']]), + ], + 'rcl_interfaces/msg/ListParametersResult': [ + ('names', [4, [1, 'string']]), + ('prefixes', [4, [1, 'string']]), + ], + 'rcl_interfaces/msg/ParameterType': [ + ('structure_needs_at_least_one_member', [1, 'uint8']), + ], + 'rcl_interfaces/msg/ParameterEventDescriptors': [ + ('new_parameters', [4, [2, 'rcl_interfaces/msg/ParameterDescriptor']]), + ('changed_parameters', [4, [2, 'rcl_interfaces/msg/ParameterDescriptor']]), + ('deleted_parameters', [4, [2, 'rcl_interfaces/msg/ParameterDescriptor']]), + ], + 'rcl_interfaces/msg/ParameterEvent': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('node', [1, 'string']), + ('new_parameters', [4, [2, 'rcl_interfaces/msg/Parameter']]), + ('changed_parameters', [4, [2, 'rcl_interfaces/msg/Parameter']]), + ('deleted_parameters', [4, [2, 'rcl_interfaces/msg/Parameter']]), + ], + 'rcl_interfaces/msg/IntegerRange': [ + ('from_value', [1, 'int64']), + ('to_value', [1, 'int64']), + ('step', [1, 'uint64']), + ], + 'rcl_interfaces/msg/Parameter': [ + ('name', [1, 'string']), + ('value', [2, 'rcl_interfaces/msg/ParameterValue']), + ], + 'rcl_interfaces/msg/ParameterValue': [ + ('type', [1, 'uint8']), + ('bool_value', [1, 'bool']), + ('integer_value', [1, 'int64']), + ('double_value', [1, 'float64']), + ('string_value', [1, 'string']), + ('byte_array_value', [4, [1, 'uint8']]), + ('bool_array_value', [4, [1, 'bool']]), + ('integer_array_value', [4, [1, 'int64']]), + ('double_array_value', [4, [1, 'float64']]), + ('string_array_value', [4, [1, 'string']]), + ], + 'rcl_interfaces/msg/FloatingPointRange': [ + ('from_value', [1, 'float64']), + ('to_value', [1, 'float64']), + ('step', [1, 'float64']), + ], + 'rcl_interfaces/msg/SetParametersResult': [ + ('successful', [1, 'bool']), + ('reason', [1, 'string']), + ], + 'rcl_interfaces/msg/Log': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('level', [1, 'uint8']), + ('name', [1, 'string']), + ('msg', [1, 'string']), + ('file', [1, 'string']), + ('function', [1, 'string']), + ('line', [1, 'uint32']), + ], + 'rcl_interfaces/msg/ParameterDescriptor': [ + ('name', [1, 'string']), + ('type', [1, 'uint8']), + ('description', [1, 'string']), + ('additional_constraints', [1, 'string']), + ('read_only', [1, 'bool']), + ('floating_point_range', [4, [2, 'rcl_interfaces/msg/FloatingPointRange']]), + ('integer_range', [4, [2, 'rcl_interfaces/msg/IntegerRange']]), + ], + 'rmw_dds_common/msg/Gid': [ + ('data', [3, 24, [1, 'uint8']]), + ], + 'rmw_dds_common/msg/NodeEntitiesInfo': [ + ('node_namespace', [1, 'string', [6, 256]]), + ('node_name', [1, 'string', [6, 256]]), + ('reader_gid_seq', [4, [2, 'rmw_dds_common/msg/Gid']]), + ('writer_gid_seq', [4, [2, 'rmw_dds_common/msg/Gid']]), + ], + 'rmw_dds_common/msg/ParticipantEntitiesInfo': [ + ('gid', [2, 'rmw_dds_common/msg/Gid']), + ('node_entities_info_seq', [4, [2, 'rmw_dds_common/msg/NodeEntitiesInfo']]), + ], + 'rosgraph_msgs/msg/Clock': [ + ('clock', [2, 'builtin_interfaces/msg/Time']), + ], + 'sensor_msgs/msg/Temperature': [ + ('header', [2, 'std_msgs/msg/Header']), + ('temperature', [1, 'float64']), + ('variance', [1, 'float64']), + ], + 'sensor_msgs/msg/Range': [ + ('header', [2, 'std_msgs/msg/Header']), + ('radiation_type', [1, 'uint8']), + ('field_of_view', [1, 'float32']), + ('min_range', [1, 'float32']), + ('max_range', [1, 'float32']), + ('range', [1, 'float32']), + ], + 'sensor_msgs/msg/RegionOfInterest': [ + ('x_offset', [1, 'uint32']), + ('y_offset', [1, 'uint32']), + ('height', [1, 'uint32']), + ('width', [1, 'uint32']), + ('do_rectify', [1, 'bool']), + ], + 'sensor_msgs/msg/JoyFeedbackArray': [ + ('array', [4, [2, 'sensor_msgs/msg/JoyFeedback']]), + ], + 'sensor_msgs/msg/TimeReference': [ + ('header', [2, 'std_msgs/msg/Header']), + ('time_ref', [2, 'builtin_interfaces/msg/Time']), + ('source', [1, 'string']), + ], + 'sensor_msgs/msg/CompressedImage': [ + ('header', [2, 'std_msgs/msg/Header']), + ('format', [1, 'string']), + ('data', [4, [1, 'uint8']]), + ], + 'sensor_msgs/msg/MultiEchoLaserScan': [ + ('header', [2, 'std_msgs/msg/Header']), + ('angle_min', [1, 'float32']), + ('angle_max', [1, 'float32']), + ('angle_increment', [1, 'float32']), + ('time_increment', [1, 'float32']), + ('scan_time', [1, 'float32']), + ('range_min', [1, 'float32']), + ('range_max', [1, 'float32']), + ('ranges', [4, [2, 'sensor_msgs/msg/LaserEcho']]), + ('intensities', [4, [2, 'sensor_msgs/msg/LaserEcho']]), + ], + 'sensor_msgs/msg/LaserEcho': [ + ('echoes', [4, [1, 'float32']]), + ], + 'sensor_msgs/msg/ChannelFloat32': [ + ('name', [1, 'string']), + ('values', [4, [1, 'float32']]), + ], + 'sensor_msgs/msg/CameraInfo': [ + ('header', [2, 'std_msgs/msg/Header']), + ('height', [1, 'uint32']), + ('width', [1, 'uint32']), + ('distortion_model', [1, 'string']), + ('d', [4, [1, 'float64']]), + ('k', [3, 9, [1, 'float64']]), + ('r', [3, 9, [1, 'float64']]), + ('p', [3, 12, [1, 'float64']]), + ('binning_x', [1, 'uint32']), + ('binning_y', [1, 'uint32']), + ('roi', [2, 'sensor_msgs/msg/RegionOfInterest']), + ], + 'sensor_msgs/msg/RelativeHumidity': [ + ('header', [2, 'std_msgs/msg/Header']), + ('relative_humidity', [1, 'float64']), + ('variance', [1, 'float64']), + ], + 'sensor_msgs/msg/FluidPressure': [ + ('header', [2, 'std_msgs/msg/Header']), + ('fluid_pressure', [1, 'float64']), + ('variance', [1, 'float64']), + ], + 'sensor_msgs/msg/LaserScan': [ + ('header', [2, 'std_msgs/msg/Header']), + ('angle_min', [1, 'float32']), + ('angle_max', [1, 'float32']), + ('angle_increment', [1, 'float32']), + ('time_increment', [1, 'float32']), + ('scan_time', [1, 'float32']), + ('range_min', [1, 'float32']), + ('range_max', [1, 'float32']), + ('ranges', [4, [1, 'float32']]), + ('intensities', [4, [1, 'float32']]), + ], + 'sensor_msgs/msg/BatteryState': [ + ('header', [2, 'std_msgs/msg/Header']), + ('voltage', [1, 'float32']), + ('temperature', [1, 'float32']), + ('current', [1, 'float32']), + ('charge', [1, 'float32']), + ('capacity', [1, 'float32']), + ('design_capacity', [1, 'float32']), + ('percentage', [1, 'float32']), + ('power_supply_status', [1, 'uint8']), + ('power_supply_health', [1, 'uint8']), + ('power_supply_technology', [1, 'uint8']), + ('present', [1, 'bool']), + ('cell_voltage', [4, [1, 'float32']]), + ('cell_temperature', [4, [1, 'float32']]), + ('location', [1, 'string']), + ('serial_number', [1, 'string']), + ], + 'sensor_msgs/msg/Image': [ + ('header', [2, 'std_msgs/msg/Header']), + ('height', [1, 'uint32']), + ('width', [1, 'uint32']), + ('encoding', [1, 'string']), + ('is_bigendian', [1, 'uint8']), + ('step', [1, 'uint32']), + ('data', [4, [1, 'uint8']]), + ], + 'sensor_msgs/msg/PointCloud': [ + ('header', [2, 'std_msgs/msg/Header']), + ('points', [4, [2, 'geometry_msgs/msg/Point32']]), + ('channels', [4, [2, 'sensor_msgs/msg/ChannelFloat32']]), + ], + 'sensor_msgs/msg/Imu': [ + ('header', [2, 'std_msgs/msg/Header']), + ('orientation', [2, 'geometry_msgs/msg/Quaternion']), + ('orientation_covariance', [3, 9, [1, 'float64']]), + ('angular_velocity', [2, 'geometry_msgs/msg/Vector3']), + ('angular_velocity_covariance', [3, 9, [1, 'float64']]), + ('linear_acceleration', [2, 'geometry_msgs/msg/Vector3']), + ('linear_acceleration_covariance', [3, 9, [1, 'float64']]), + ], + 'sensor_msgs/msg/NavSatStatus': [ + ('status', [1, 'int8']), + ('service', [1, 'uint16']), + ], + 'sensor_msgs/msg/Illuminance': [ + ('header', [2, 'std_msgs/msg/Header']), + ('illuminance', [1, 'float64']), + ('variance', [1, 'float64']), + ], + 'sensor_msgs/msg/Joy': [ + ('header', [2, 'std_msgs/msg/Header']), + ('axes', [4, [1, 'float32']]), + ('buttons', [4, [1, 'int32']]), + ], + 'sensor_msgs/msg/NavSatFix': [ + ('header', [2, 'std_msgs/msg/Header']), + ('status', [2, 'sensor_msgs/msg/NavSatStatus']), + ('latitude', [1, 'float64']), + ('longitude', [1, 'float64']), + ('altitude', [1, 'float64']), + ('position_covariance', [3, 9, [1, 'float64']]), + ('position_covariance_type', [1, 'uint8']), + ], + 'sensor_msgs/msg/MultiDOFJointState': [ + ('header', [2, 'std_msgs/msg/Header']), + ('joint_names', [4, [1, 'string']]), + ('transforms', [4, [2, 'geometry_msgs/msg/Transform']]), + ('twist', [4, [2, 'geometry_msgs/msg/Twist']]), + ('wrench', [4, [2, 'geometry_msgs/msg/Wrench']]), + ], + 'sensor_msgs/msg/MagneticField': [ + ('header', [2, 'std_msgs/msg/Header']), + ('magnetic_field', [2, 'geometry_msgs/msg/Vector3']), + ('magnetic_field_covariance', [3, 9, [1, 'float64']]), + ], + 'sensor_msgs/msg/JointState': [ + ('header', [2, 'std_msgs/msg/Header']), + ('name', [4, [1, 'string']]), + ('position', [4, [1, 'float64']]), + ('velocity', [4, [1, 'float64']]), + ('effort', [4, [1, 'float64']]), + ], + 'sensor_msgs/msg/PointField': [ + ('name', [1, 'string']), + ('offset', [1, 'uint32']), + ('datatype', [1, 'uint8']), + ('count', [1, 'uint32']), + ], + 'sensor_msgs/msg/PointCloud2': [ + ('header', [2, 'std_msgs/msg/Header']), + ('height', [1, 'uint32']), + ('width', [1, 'uint32']), + ('fields', [4, [2, 'sensor_msgs/msg/PointField']]), + ('is_bigendian', [1, 'bool']), + ('point_step', [1, 'uint32']), + ('row_step', [1, 'uint32']), + ('data', [4, [1, 'uint8']]), + ('is_dense', [1, 'bool']), + ], + 'sensor_msgs/msg/JoyFeedback': [ + ('type', [1, 'uint8']), + ('id', [1, 'uint8']), + ('intensity', [1, 'float32']), + ], + 'shape_msgs/msg/SolidPrimitive': [ + ('type', [1, 'uint8']), + ('dimensions', [4, [1, 'float64']]), + ], + 'shape_msgs/msg/Mesh': [ + ('triangles', [4, [2, 'shape_msgs/msg/MeshTriangle']]), + ('vertices', [4, [2, 'geometry_msgs/msg/Point']]), + ], + 'shape_msgs/msg/Plane': [ + ('coef', [3, 4, [1, 'float64']]), + ], + 'shape_msgs/msg/MeshTriangle': [ + ('vertex_indices', [3, 3, [1, 'uint32']]), + ], + 'statistics_msgs/msg/StatisticDataType': [ + ('structure_needs_at_least_one_member', [1, 'uint8']), + ], + 'statistics_msgs/msg/StatisticDataPoint': [ + ('data_type', [1, 'uint8']), + ('data', [1, 'float64']), + ], + 'statistics_msgs/msg/MetricsMessage': [ + ('measurement_source_name', [1, 'string']), + ('metrics_source', [1, 'string']), + ('unit', [1, 'string']), + ('window_start', [2, 'builtin_interfaces/msg/Time']), + ('window_stop', [2, 'builtin_interfaces/msg/Time']), + ('statistics', [4, [2, 'statistics_msgs/msg/StatisticDataPoint']]), + ], + 'std_msgs/msg/UInt8': [ + ('data', [1, 'uint8']), + ], + 'std_msgs/msg/Float32MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'float32']]), + ], + 'std_msgs/msg/Int8': [ + ('data', [1, 'int8']), + ], + 'std_msgs/msg/Empty': [ + ('structure_needs_at_least_one_member', [1, 'uint8']), + ], + 'std_msgs/msg/String': [ + ('data', [1, 'string']), + ], + 'std_msgs/msg/MultiArrayDimension': [ + ('label', [1, 'string']), + ('size', [1, 'uint32']), + ('stride', [1, 'uint32']), + ], + 'std_msgs/msg/UInt64': [ + ('data', [1, 'uint64']), + ], + 'std_msgs/msg/UInt16': [ + ('data', [1, 'uint16']), + ], + 'std_msgs/msg/Float32': [ + ('data', [1, 'float32']), + ], + 'std_msgs/msg/Int64': [ + ('data', [1, 'int64']), + ], + 'std_msgs/msg/Int16MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'int16']]), + ], + 'std_msgs/msg/Int16': [ + ('data', [1, 'int16']), + ], + 'std_msgs/msg/Float64MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'float64']]), + ], + 'std_msgs/msg/MultiArrayLayout': [ + ('dim', [4, [2, 'std_msgs/msg/MultiArrayDimension']]), + ('data_offset', [1, 'uint32']), + ], + 'std_msgs/msg/UInt32MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'uint32']]), + ], + 'std_msgs/msg/Header': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('frame_id', [1, 'string']), + ], + 'std_msgs/msg/ByteMultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'uint8']]), + ], + 'std_msgs/msg/Int8MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'int8']]), + ], + 'std_msgs/msg/Float64': [ + ('data', [1, 'float64']), + ], + 'std_msgs/msg/UInt8MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'uint8']]), + ], + 'std_msgs/msg/Byte': [ + ('data', [1, 'uint8']), + ], + 'std_msgs/msg/Char': [ + ('data', [1, 'uint8']), + ], + 'std_msgs/msg/UInt64MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'uint64']]), + ], + 'std_msgs/msg/Int32MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'int32']]), + ], + 'std_msgs/msg/ColorRGBA': [ + ('r', [1, 'float32']), + ('g', [1, 'float32']), + ('b', [1, 'float32']), + ('a', [1, 'float32']), + ], + 'std_msgs/msg/Bool': [ + ('data', [1, 'bool']), + ], + 'std_msgs/msg/UInt32': [ + ('data', [1, 'uint32']), + ], + 'std_msgs/msg/Int64MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'int64']]), + ], + 'std_msgs/msg/Int32': [ + ('data', [1, 'int32']), + ], + 'std_msgs/msg/UInt16MultiArray': [ + ('layout', [2, 'std_msgs/msg/MultiArrayLayout']), + ('data', [4, [1, 'uint16']]), + ], + 'stereo_msgs/msg/DisparityImage': [ + ('header', [2, 'std_msgs/msg/Header']), + ('image', [2, 'sensor_msgs/msg/Image']), + ('f', [1, 'float32']), + ('t', [1, 'float32']), + ('valid_window', [2, 'sensor_msgs/msg/RegionOfInterest']), + ('min_disparity', [1, 'float32']), + ('max_disparity', [1, 'float32']), + ('delta_d', [1, 'float32']), + ], + 'tf2_msgs/msg/TF2Error': [ + ('error', [1, 'uint8']), + ('error_string', [1, 'string']), + ], + 'tf2_msgs/msg/TFMessage': [ + ('transforms', [4, [2, 'geometry_msgs/msg/TransformStamped']]), + ], + 'trajectory_msgs/msg/MultiDOFJointTrajectory': [ + ('header', [2, 'std_msgs/msg/Header']), + ('joint_names', [4, [1, 'string']]), + ('points', [4, [2, 'trajectory_msgs/msg/MultiDOFJointTrajectoryPoint']]), + ], + 'trajectory_msgs/msg/JointTrajectory': [ + ('header', [2, 'std_msgs/msg/Header']), + ('joint_names', [4, [1, 'string']]), + ('points', [4, [2, 'trajectory_msgs/msg/JointTrajectoryPoint']]), + ], + 'trajectory_msgs/msg/JointTrajectoryPoint': [ + ('positions', [4, [1, 'float64']]), + ('velocities', [4, [1, 'float64']]), + ('accelerations', [4, [1, 'float64']]), + ('effort', [4, [1, 'float64']]), + ('time_from_start', [2, 'builtin_interfaces/msg/Duration']), + ], + 'trajectory_msgs/msg/MultiDOFJointTrajectoryPoint': [ + ('transforms', [4, [2, 'geometry_msgs/msg/Transform']]), + ('velocities', [4, [2, 'geometry_msgs/msg/Twist']]), + ('accelerations', [4, [2, 'geometry_msgs/msg/Twist']]), + ('time_from_start', [2, 'builtin_interfaces/msg/Duration']), + ], + 'unique_identifier_msgs/msg/UUID': [ + ('uuid', [3, 16, [1, 'uint8']]), + ], + 'visualization_msgs/msg/Marker': [ + ('header', [2, 'std_msgs/msg/Header']), + ('ns', [1, 'string']), + ('id', [1, 'int32']), + ('type', [1, 'int32']), + ('action', [1, 'int32']), + ('pose', [2, 'geometry_msgs/msg/Pose']), + ('scale', [2, 'geometry_msgs/msg/Vector3']), + ('color', [2, 'std_msgs/msg/ColorRGBA']), + ('lifetime', [2, 'builtin_interfaces/msg/Duration']), + ('frame_locked', [1, 'bool']), + ('points', [4, [2, 'geometry_msgs/msg/Point']]), + ('colors', [4, [2, 'std_msgs/msg/ColorRGBA']]), + ('text', [1, 'string']), + ('mesh_resource', [1, 'string']), + ('mesh_use_embedded_materials', [1, 'bool']), + ], + 'visualization_msgs/msg/InteractiveMarkerInit': [ + ('server_id', [1, 'string']), + ('seq_num', [1, 'uint64']), + ('markers', [4, [2, 'visualization_msgs/msg/InteractiveMarker']]), + ], + 'visualization_msgs/msg/MenuEntry': [ + ('id', [1, 'uint32']), + ('parent_id', [1, 'uint32']), + ('title', [1, 'string']), + ('command', [1, 'string']), + ('command_type', [1, 'uint8']), + ], + 'visualization_msgs/msg/MarkerArray': [ + ('markers', [4, [2, 'visualization_msgs/msg/Marker']]), + ], + 'visualization_msgs/msg/InteractiveMarkerUpdate': [ + ('server_id', [1, 'string']), + ('seq_num', [1, 'uint64']), + ('type', [1, 'uint8']), + ('markers', [4, [2, 'visualization_msgs/msg/InteractiveMarker']]), + ('poses', [4, [2, 'visualization_msgs/msg/InteractiveMarkerPose']]), + ('erases', [4, [1, 'string']]), + ], + 'visualization_msgs/msg/InteractiveMarker': [ + ('header', [2, 'std_msgs/msg/Header']), + ('pose', [2, 'geometry_msgs/msg/Pose']), + ('name', [1, 'string']), + ('description', [1, 'string']), + ('scale', [1, 'float32']), + ('menu_entries', [4, [2, 'visualization_msgs/msg/MenuEntry']]), + ('controls', [4, [2, 'visualization_msgs/msg/InteractiveMarkerControl']]), + ], + 'visualization_msgs/msg/InteractiveMarkerFeedback': [ + ('header', [2, 'std_msgs/msg/Header']), + ('client_id', [1, 'string']), + ('marker_name', [1, 'string']), + ('control_name', [1, 'string']), + ('event_type', [1, 'uint8']), + ('pose', [2, 'geometry_msgs/msg/Pose']), + ('menu_entry_id', [1, 'uint32']), + ('mouse_point', [2, 'geometry_msgs/msg/Point']), + ('mouse_point_valid', [1, 'bool']), + ], + 'visualization_msgs/msg/ImageMarker': [ + ('header', [2, 'std_msgs/msg/Header']), + ('ns', [1, 'string']), + ('id', [1, 'int32']), + ('type', [1, 'int32']), + ('action', [1, 'int32']), + ('position', [2, 'geometry_msgs/msg/Point']), + ('scale', [1, 'float32']), + ('outline_color', [2, 'std_msgs/msg/ColorRGBA']), + ('filled', [1, 'uint8']), + ('fill_color', [2, 'std_msgs/msg/ColorRGBA']), + ('lifetime', [2, 'builtin_interfaces/msg/Duration']), + ('points', [4, [2, 'geometry_msgs/msg/Point']]), + ('outline_colors', [4, [2, 'std_msgs/msg/ColorRGBA']]), + ], + 'visualization_msgs/msg/InteractiveMarkerControl': [ + ('name', [1, 'string']), + ('orientation', [2, 'geometry_msgs/msg/Quaternion']), + ('orientation_mode', [1, 'uint8']), + ('interaction_mode', [1, 'uint8']), + ('always_visible', [1, 'bool']), + ('markers', [4, [2, 'visualization_msgs/msg/Marker']]), + ('independent_marker_orientation', [1, 'bool']), + ('description', [1, 'string']), + ], + 'visualization_msgs/msg/InteractiveMarkerPose': [ + ('header', [2, 'std_msgs/msg/Header']), + ('pose', [2, 'geometry_msgs/msg/Pose']), + ('name', [1, 'string']), + ], + 'autoware_auto_msgs/msg/BoundingBox': [ + ('centroid', [2, 'geometry_msgs/msg/Point32']), + ('size', [2, 'geometry_msgs/msg/Point32']), + ('orientation', [2, 'autoware_auto_msgs/msg/Quaternion32']), + ('velocity', [1, 'float32']), + ('heading', [1, 'float32']), + ('heading_rate', [1, 'float32']), + ('corners', [3, 4, [2, 'geometry_msgs/msg/Point32']]), + ('variance', [3, 8, [1, 'float32']]), + ('value', [1, 'float32']), + ('vehicle_label', [1, 'uint8']), + ('signal_label', [1, 'uint8']), + ('class_likelihood', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/HADMapBin': [ + ('header', [2, 'std_msgs/msg/Header']), + ('map_format', [1, 'uint8']), + ('format_version', [1, 'string']), + ('map_version', [1, 'string']), + ('data', [4, [1, 'uint8']]), + ], + 'autoware_auto_msgs/msg/Quaternion32': [ + ('x', [1, 'float32']), + ('y', [1, 'float32']), + ('z', [1, 'float32']), + ('w', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/Trajectory': [ + ('header', [2, 'std_msgs/msg/Header']), + ('points', [4, [2, 'autoware_auto_msgs/msg/TrajectoryPoint']]), + ], + 'autoware_auto_msgs/msg/Route': [ + ('header', [2, 'std_msgs/msg/Header']), + ('start_point', [2, 'autoware_auto_msgs/msg/TrajectoryPoint']), + ('goal_point', [2, 'autoware_auto_msgs/msg/TrajectoryPoint']), + ('primitives', [4, [2, 'autoware_auto_msgs/msg/MapPrimitive']]), + ], + 'autoware_auto_msgs/msg/MapPrimitive': [ + ('id', [1, 'int64']), + ('primitive_type', [1, 'string']), + ], + 'autoware_auto_msgs/msg/TrajectoryPoint': [ + ('time_from_start', [2, 'builtin_interfaces/msg/Duration']), + ('x', [1, 'float32']), + ('y', [1, 'float32']), + ('heading', [2, 'autoware_auto_msgs/msg/Complex32']), + ('longitudinal_velocity_mps', [1, 'float32']), + ('lateral_velocity_mps', [1, 'float32']), + ('acceleration_mps2', [1, 'float32']), + ('heading_rate_rps', [1, 'float32']), + ('front_wheel_angle_rad', [1, 'float32']), + ('rear_wheel_angle_rad', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/Complex32': [ + ('real', [1, 'float32']), + ('imag', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/VehicleOdometry': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('velocity_mps', [1, 'float32']), + ('front_wheel_angle_rad', [1, 'float32']), + ('rear_wheel_angle_rad', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/DiagnosticHeader': [ + ('name', [1, 'string', [6, 256]]), + ('data_stamp', [2, 'builtin_interfaces/msg/Time']), + ('computation_start', [2, 'builtin_interfaces/msg/Time']), + ('runtime', [2, 'builtin_interfaces/msg/Duration']), + ('iterations', [1, 'uint32']), + ], + 'autoware_auto_msgs/msg/HighLevelControlCommand': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('velocity_mps', [1, 'float32']), + ('curvature', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/VehicleStateCommand': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('blinker', [1, 'uint8']), + ('headlight', [1, 'uint8']), + ('wiper', [1, 'uint8']), + ('gear', [1, 'uint8']), + ('mode', [1, 'uint8']), + ('hand_brake', [1, 'bool']), + ('horn', [1, 'bool']), + ], + 'autoware_auto_msgs/msg/PointClusters': [ + ('clusters', [4, [2, 'sensor_msgs/msg/PointCloud2']]), + ], + 'autoware_auto_msgs/msg/RawControlCommand': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('throttle', [1, 'uint32']), + ('brake', [1, 'uint32']), + ('front_steer', [1, 'int32']), + ('rear_steer', [1, 'int32']), + ], + 'autoware_auto_msgs/msg/VehicleStateReport': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('fuel', [1, 'uint8']), + ('blinker', [1, 'uint8']), + ('headlight', [1, 'uint8']), + ('wiper', [1, 'uint8']), + ('gear', [1, 'uint8']), + ('mode', [1, 'uint8']), + ('hand_brake', [1, 'bool']), + ('horn', [1, 'bool']), + ], + 'autoware_auto_msgs/msg/ControlDiagnostic': [ + ('diag_header', [2, 'autoware_auto_msgs/msg/DiagnosticHeader']), + ('new_trajectory', [1, 'bool']), + ('trajectory_source', [1, 'string', [6, 256]]), + ('pose_source', [1, 'string', [6, 256]]), + ('lateral_error_m', [1, 'float32']), + ('longitudinal_error_m', [1, 'float32']), + ('velocity_error_mps', [1, 'float32']), + ('acceleration_error_mps2', [1, 'float32']), + ('yaw_error_rad', [1, 'float32']), + ('yaw_rate_error_rps', [1, 'float32']), + ], + 'autoware_auto_msgs/msg/VehicleKinematicState': [ + ('header', [2, 'std_msgs/msg/Header']), + ('state', [2, 'autoware_auto_msgs/msg/TrajectoryPoint']), + ('delta', [2, 'geometry_msgs/msg/Transform']), + ], + 'autoware_auto_msgs/msg/BoundingBoxArray': [ + ('header', [2, 'std_msgs/msg/Header']), + ('boxes', [4, [2, 'autoware_auto_msgs/msg/BoundingBox']]), + ], + 'autoware_auto_msgs/msg/VehicleControlCommand': [ + ('stamp', [2, 'builtin_interfaces/msg/Time']), + ('long_accel_mps2', [1, 'float32']), + ('velocity_mps', [1, 'float32']), + ('front_wheel_angle_rad', [1, 'float32']), + ('rear_wheel_angle_rad', [1, 'float32']), + ], +} diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 00000000..5a703b86 --- /dev/null +++ b/tests/__init__.py @@ -0,0 +1,3 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Rosbag tests.""" diff --git a/tests/test_parse.py b/tests/test_parse.py new file mode 100644 index 00000000..014149ce --- /dev/null +++ b/tests/test_parse.py @@ -0,0 +1,156 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Message definition parser tests.""" + +import pytest + +from rosbags.typesys import TypesysError, get_types_from_idl, get_types_from_msg, register_types +from rosbags.typesys.base import Nodetype +from rosbags.typesys.types import FIELDDEFS + +MSG = """ +# comment + +int32 global=42 + +std_msgs/Header header +std_msgs/msg/Bool bool +test_msgs/Bar sibling +float64 base +float64[] seq1 +float64[] seq2 +float64[4] array +""" + +MULTI_MSG = """ +std_msgs/Header header +byte b +char c +Other[] o + +================================================================================ +MSG: std_msgs/Header +time time + +================================================================================ +MSG: test_msgs/Other +uint64[3] Header +""" + +IDL_LANG = """ +// assign different literals and expressions + +#ifndef FOO +#define FOO + +#include +#include "local" + +const bool g_bool = TRUE; +const int8 g_int1 = 7; +const int8 g_int2 = 07; +const int8 g_int3 = 0x7; +const float64 g_float1 = 1.1; +const float64 g_float2 = 1e10; +const char g_char = 'c'; +const string g_string1 = ""; +const string<128> g_string2 = "str" "ing"; + +module Foo { + const int64 g_expr1 = ~1; + const int64 g_expr2 = 2 * 4; +}; + +#endif +""" + +IDL = """ +// comment in file +module test_msgs { + // comment in module + typedef std_msgs::msg::Bool Bool; + + module msg { + // comment in submodule + typedef Bool Balias; + typedef test_msgs::msg::Bar Bar; + typedef double d4[4]; + + @comment(type="text", text="ignore") + struct Foo { + std_msgs::msg::Header header; + Balias bool; + Bar sibling; + double x; + sequence seq1; + sequence seq2; + d4 array; + }; + }; +}; +""" + + +def test_parse_msg(): + """Test msg parser.""" + with pytest.raises(TypesysError, match='Could not parse'): + get_types_from_msg('', 'test_msgs/msg/Foo') + ret = get_types_from_msg(MSG, 'test_msgs/msg/Foo') + assert 'test_msgs/msg/Foo' in ret + fields = ret['test_msgs/msg/Foo'] + assert fields[0][0][1] == 'std_msgs/msg/Header' + assert fields[0][1][1] == 'header' + assert fields[1][0][1] == 'std_msgs/msg/Bool' + assert fields[1][1][1] == 'bool' + assert fields[2][0][1] == 'test_msgs/msg/Bar' + assert fields[2][1][1] == 'sibling' + assert fields[3][0][0] == Nodetype.BASE + assert fields[4][0][0] == Nodetype.SEQUENCE + assert fields[5][0][0] == Nodetype.SEQUENCE + assert fields[6][0][0] == Nodetype.ARRAY + + +def test_parse_multi_msg(): + """Test multi msg parser.""" + ret = get_types_from_msg(MULTI_MSG, 'test_msgs/msg/Foo') + assert len(ret) == 3 + assert 'test_msgs/msg/Foo' in ret + assert 'std_msgs/msg/Header' in ret + assert 'test_msgs/msg/Other' in ret + assert ret['test_msgs/msg/Foo'][0][0][1] == 'std_msgs/msg/Header' + assert ret['test_msgs/msg/Foo'][1][0][1] == 'uint8' + assert ret['test_msgs/msg/Foo'][2][0][1] == 'uint8' + + +def test_parse_idl(): + """Test idl parser.""" + ret = get_types_from_idl(IDL_LANG) + assert ret == {} + + ret = get_types_from_idl(IDL) + assert 'test_msgs/msg/Foo' in ret + fields = ret['test_msgs/msg/Foo'] + assert fields[0][0][1] == 'std_msgs/msg/Header' + assert fields[0][1][1] == 'header' + assert fields[1][0][1] == 'std_msgs/msg/Bool' + assert fields[1][1][1] == 'bool' + assert fields[2][0][1] == 'test_msgs/msg/Bar' + assert fields[2][1][1] == 'sibling' + assert fields[3][0][0] == Nodetype.BASE + assert fields[4][0][0] == Nodetype.SEQUENCE + assert fields[5][0][0] == Nodetype.SEQUENCE + assert fields[6][0][0] == Nodetype.ARRAY + + +def test_register_types(): + """Test type registeration.""" + assert 'foo' not in FIELDDEFS + register_types({}) + register_types({'foo': [[(1, 'bool'), (2, 'b')]]}) + assert 'foo' in FIELDDEFS + + register_types({'std_msgs/msg/Header': []}) + assert len(FIELDDEFS['std_msgs/msg/Header']) == 2 + + with pytest.raises(TypesysError, match='different definition'): + register_types({'foo': [[(1, 'bool'), (2, 'x')]]})