diff --git a/proto2ros/proto2ros/conversions/basic.py b/proto2ros/proto2ros/conversions/basic.py index 90b8cd6..44e193b 100644 --- a/proto2ros/proto2ros/conversions/basic.py +++ b/proto2ros/proto2ros/conversions/basic.py @@ -26,7 +26,7 @@ def convert_proto2ros_any_proto_message_to_some_proto(ros_msg: proto2ros.msg.Any ValueError: if the given ROS message cannot be unpacked onto the given Protobuf message. """ proto_msg.Clear() - wrapper = google.protobuf.Any() + wrapper = google.protobuf.any_pb2.Any() wrapper.type_url = ros_msg.type_url wrapper.value = ros_msg.value.tobytes() if not wrapper.Unpack(proto_msg): @@ -36,12 +36,18 @@ def convert_proto2ros_any_proto_message_to_some_proto(ros_msg: proto2ros.msg.Any @convert.register(object, proto2ros.msg.AnyProto) def convert_some_proto_to_proto2ros_any_proto_message(proto_msg: Any, ros_msg: proto2ros.msg.AnyProto) -> None: """Packs any Protobuf message into a proto2ros/AnyProto ROS message.""" - wrapper = google.protobuf.Any() + wrapper = google.protobuf.any_pb2.Any() wrapper.Pack(proto_msg) ros_msg.type_url = wrapper.type_url ros_msg.value = wrapper.value +@convert.register(proto2ros.msg.AnyProto, proto2ros.msg.AnyProto) +def _(proto_msg: proto2ros.msg.AnyProto, ros_msg: proto2ros.msg.AnyProto) -> None: + # address multipledispatch ambiguous resolution concerns + raise RuntimeError("invalid overload") + + @convert.register(proto2ros.msg.AnyProto, google.protobuf.any_pb2.Any) def convert_proto2ros_any_proto_message_to_google_protobuf_any_proto( ros_msg: proto2ros.msg.AnyProto, proto_msg: google.protobuf.any_pb2.Any diff --git a/proto2ros/proto2ros/equivalences.py b/proto2ros/proto2ros/equivalences.py index a89a21c..2cecdb4 100644 --- a/proto2ros/proto2ros/equivalences.py +++ b/proto2ros/proto2ros/equivalences.py @@ -15,7 +15,7 @@ import functools import math from collections.abc import Sequence -from typing import Any, Iterable, List, Optional, Union +from typing import Any, Iterable, List, Optional, Set, Union import inflection from google.protobuf.descriptor_pb2 import ( @@ -155,7 +155,7 @@ def translate_type_name(name: str, config: Configuration) -> str: proto_type_name = name[1:] if proto_type_name == "google.protobuf.Any": - return "proto2ros/Any" + return "proto2ros/AnyProto" if proto_type_name in config.message_mapping: return config.message_mapping[proto_type_name] @@ -217,6 +217,31 @@ def translate_type(name: str, repeated: bool, config: Configuration) -> Type: return Type(ros_type_name) +def translate_any_type(any_expansion: Union[Set[str], str], repeated: bool, config: Configuration) -> Type: + """ + Translates a ``google.protobuf.Any`` type to its ROS equivalent given an any expansion. + + Args: + any_expansion: a Protobuf message type set that the given ``google.protobuf.Any`` + is expected to pack. A single Protobuf message type may also be specified in lieu + of a single element set. All Protobuf message types must be fully qualified. + repeated: whether the Protobuf type applies to a repeated field. + config: a suitable configuration for the procedure. + + Returns: + a ROS message type. + """ + if config.allow_any_casts and isinstance(any_expansion, str): + # Type name is expected to be fully qualified, thus the leading dot. See + # https://github.com/protocolbuffers/protobuf/blob/main/src/google/protobuf/descriptor.proto#L265-L270 + # for further reference. + return translate_type(f".{any_expansion}", repeated, config) + ros_type_name = "proto2ros/Any" + if repeated: + ros_type_name += "[]" + return Type(ros_type_name) + + def translate_field( descriptor: FieldDescriptorProto, source: FileDescriptorProto, @@ -239,36 +264,32 @@ def translate_field( ValueError: when the given field is of an unsupported or unknown type. ValueError: when an any expansion is specified for a fully typed field. """ - any_expansion = config.any_expansions.get(protofqn(source, location)) - if any_expansion and descriptor.type_name != ".google.protobuf.Any": - raise ValueError(f"any expansion specified for '{descriptor.name}' field of {descriptor.type_name} type") - if descriptor.type in PRIMITIVE_TYPE_NAMES: - type_name = PRIMITIVE_TYPE_NAMES[descriptor.type] - elif descriptor.type in COMPOSITE_TYPES: - if any_expansion: - if config.allow_any_casts and isinstance(any_expansion, str): - # Type name is expected to be fully qualified, thus the leading dot. See - # https://github.com/protocolbuffers/protobuf/blob/main/src/google/protobuf/descriptor.proto#L265-L270 - # for further reference. - type_name = f".{any_expansion}" - else: - type_name = descriptor.type_name - else: - type_name = descriptor.type_name - else: - raise ValueError(f"unsupported field type: {descriptor.type}") repeated = descriptor.label == FieldDescriptorProto.LABEL_REPEATED + any_expansion = config.any_expansions.get(protofqn(source, location)) + if any_expansion: + if descriptor.type_name != ".google.protobuf.Any": + raise ValueError(f"any expansion specified for '{descriptor.name}' field of {descriptor.type_name} type") + type_name = descriptor.type_name - field_type = translate_type(type_name, repeated, config) + field_type = translate_any_type(any_expansion, repeated, config) + else: + if descriptor.type in PRIMITIVE_TYPE_NAMES: + type_name = PRIMITIVE_TYPE_NAMES[descriptor.type] + elif descriptor.type in COMPOSITE_TYPES: + type_name = descriptor.type_name + else: + raise ValueError(f"unsupported field type: {descriptor.type}") + field_type = translate_type(type_name, repeated, config) field = Field(field_type, to_ros_field_name(descriptor.name)) if any_expansion: if not config.allow_any_casts or not isinstance(any_expansion, str): # Annotate field with paired Protobuf and ROS message type names, # so that any expansions can be resolved in conversion code. field.annotations["type-casts"] = [ - (proto_type, translate_type_name(proto_type, config)) for proto_type in any_expansion + (proto_type, translate_type_name(f".{proto_type}", config)) for proto_type in any_expansion ] else: + type_name = f".{any_expansion}" field.annotations["type-casted"] = True if source.syntax == "proto3": field.annotations["optional"] = descriptor.proto3_optional or ( @@ -280,7 +301,8 @@ def translate_field( else: raise ValueError(f"unknown proto syntax: {source.syntax}") field.annotations["proto-name"] = descriptor.name - if to_ros_base_type(field_type) == "proto2ros/AnyProto": + ros_type_name = to_ros_base_type(field_type) + if type_name != ".google.protobuf.Any" and ros_type_name == "proto2ros/AnyProto": type_name = "some" field.annotations["proto-type"] = type_name.strip(".") leading_comments = extract_leading_comments(location) diff --git a/proto2ros/proto2ros/output/templates/conversions.py.jinja b/proto2ros/proto2ros/output/templates/conversions.py.jinja index 55047ee..2331c6b 100644 --- a/proto2ros/proto2ros/output/templates/conversions.py.jinja +++ b/proto2ros/proto2ros/output/templates/conversions.py.jinja @@ -86,8 +86,8 @@ typed_field_message = rclpy.serialization.deserialize_message({{ source }}.value convert_{{ spec.type | as_ros_base_type | as_python_identifier }}_message_to_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto(typed_field_message, {{ destination }}) {%- elif spec.annotations.get("type-casted") -%} {#- ROS message must be converted and packed for assignment. -#} -typed_proto_message = {{ proto_type_name | as_pb2_python_type }}() -convert_{{ spec.type | as_ros_base_type | as_python_idenfier }}_message_to_{{ proto_type_name | as_python_identifier }}_proto({{ source }}, typed_proto_message) +typed_proto_message = {{ spec.annotations["proto-type"] | as_pb2_python_type }}() +convert_{{ spec.type | as_ros_base_type | as_python_identifier }}_message_to_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto({{ source }}, typed_proto_message) {{ destination }}.Pack(typed_proto_message) {%- elif spec.annotations.get("type-casts") -%} {#- ROS message must be deserialized according to type, then converted, then packed for assignment. -#} @@ -192,17 +192,17 @@ convert_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto_to_{{ {{ destination }}.type_name = "{{ spec.type | as_ros_base_type }}" {%- elif spec.annotations.get("type-casted") -%} {#- Protobuf message must be unpacked for conversion. -#} -typed_proto_message = {{ field_spec.annotations["proto-type"] | as_pb2_python_type }}() +typed_proto_message = {{ spec.annotations["proto-type"] | as_pb2_python_type }}() {{ source }}.Unpack(typed_proto_message) convert_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto_to_{{ spec.type | as_ros_base_type | as_python_identifier }}_message(typed_proto_message, {{ destination }}) {%- elif spec.annotations.get("type-casts") -%} {#- Protobuf message must be unpacked according to type, then converted, then serialized for assignment. -#} {%- for proto_type_name, ros_type_name in spec.annotations["type-casts"] -%} {%- if loop.first -%} -if {{ source }}.Is({{ proto_type_name | as_pb2_python_type }}): - {%- else -%} -elif {{ source }}.Is({{ proto_type_name | as_pb2_python_type }}): - {%- endif -%} +if {{ source }}.Is({{ proto_type_name | as_pb2_python_type }}.DESCRIPTOR): + {%- else %} +elif {{ source }}.Is({{ proto_type_name | as_pb2_python_type }}.DESCRIPTOR): + {%- endif %} typed_proto_message = {{ proto_type_name | as_pb2_python_type }}() ok = {{ source }}.Unpack(typed_proto_message) assert ok, "Failed to unpack any protobuf, internal error" @@ -210,9 +210,9 @@ elif {{ source }}.Is({{ proto_type_name | as_pb2_python_type }}): convert_{{ proto_type_name | as_python_identifier }}_proto_to_{{ ros_type_name | as_python_identifier }}_message(typed_proto_message, typed_field_message) {{ destination }}.value = rclpy.serialization.serialize_message(typed_field_message) {{ destination }}.type_name = "{{ ros_type_name }}" - {%- endfor -%} + {%- endfor %} else: - raise ValueError("unknown protobuf message type in {{ spec.name }} member: %s" %s {{ source }}.type_url) + raise ValueError("unknown protobuf message type in {{ spec.name }} member: %s" % {{ source }}.type_url) {%- elif type_spec and type_spec.annotations.get("tagged") -%} {#- Handle one-of field case i.e. determine and convert the Protobuf message member that is set. -#} match {{ source.rpartition(".")[0] }}.WhichOneof("{{ spec.name }}"): @@ -238,7 +238,7 @@ convert_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto_to_{{ def convert_{{ spec.base_type | string | as_python_identifier }}_message_to_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto( ros_msg: {{ spec.base_type | string | as_ros_python_type }}, proto_msg: {{ spec.annotations["proto-type"] | as_pb2_python_type }} ) -> None: - """Converts from {{ spec.base_type }} ROS messages to {{ spec.annotations["proto-type"] }} Protobuf messages.""" + """Convert from {{ spec.base_type }} ROS messages to {{ spec.annotations["proto-type"] }} Protobuf messages.""" {%- if spec.fields %} proto_msg.Clear() {%- for field_spec in spec.fields if field_spec.name != "has_field" %} @@ -250,10 +250,11 @@ def convert_{{ spec.base_type | string | as_python_identifier }}_message_to_{{ s {%- else %} {{ ros_to_proto_field_code(source, destination, field_spec) | indent(4) }} {%- endif -%} - {% endfor %} + {%- endfor -%} {% else -%}{#- Handle empty message. #} pass - {% endif %} + {%- endif %} + convert_{{ spec.base_type | string | as_python_identifier }}_to_proto = \ convert_{{ spec.base_type | string | as_python_identifier }}_message_to_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto @@ -263,7 +264,7 @@ convert_{{ spec.base_type | string | as_python_identifier }}_to_proto = \ def convert_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto_to_{{ spec.base_type | string | as_python_identifier }}_message( proto_msg: {{ spec.annotations["proto-type"] | as_pb2_python_type }}, ros_msg: {{ spec.base_type | string | as_ros_python_type }} ) -> None: - """Converts from {{ spec.annotations["proto-type"] }} Protobuf messages to {{ spec.base_type }} ROS messages.""" + """Convert from {{ spec.annotations["proto-type"] }} Protobuf messages to {{ spec.base_type }} ROS messages.""" {%- if spec.fields %} {%- if spec.annotations["has-optionals"] %} ros_msg.has_field = 0 @@ -278,10 +279,11 @@ def convert_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto_t {%- else %} {{ proto_to_ros_field_code(source, destination, field_spec) | indent(4) }} {%- endif -%} - {% endfor %} + {%- endfor -%} {% else -%}{#- Handle empty message. #} pass - {% endif %} + {%- endif %} + convert_proto_to_{{ spec.base_type | string | as_python_identifier }} = \ convert_{{ spec.annotations["proto-type"] | as_python_identifier }}_proto_to_{{ spec.base_type | string | as_python_identifier }}_message diff --git a/proto2ros_tests/config/overlay.yaml b/proto2ros_tests/config/overlay.yaml index 64f83e6..0ef176b 100644 --- a/proto2ros_tests/config/overlay.yaml +++ b/proto2ros_tests/config/overlay.yaml @@ -1,6 +1,12 @@ +any_expansions: + proto2ros_tests.Goal.target: bosdyn.api.SE2Pose + proto2ros_tests.Goal.roi: [bosdyn.api.Polygon, bosdyn.api.Circle] message_mapping: bosdyn.api.Vec3: geometry_msgs/Vector3 bosdyn.api.Quaternion: geometry_msgs/Quaternion + bosdyn.api.SE2Pose: geometry_msgs/Pose + bosdyn.api.Polygon: geometry_msgs/Polygon + bosdyn.api.Circle: geometry_msgs/Vector3 bosdyn.api.SE3Pose: geometry_msgs/Pose bosdyn.api.SE3Velocity: geometry_msgs/Twist bosdyn.api.Wrench: geometry_msgs/Wrench diff --git a/proto2ros_tests/proto/test.proto b/proto2ros_tests/proto/test.proto index 7e22bf2..72c02f2 100644 --- a/proto2ros_tests/proto/test.proto +++ b/proto2ros_tests/proto/test.proto @@ -6,6 +6,9 @@ package proto2ros_tests; option java_outer_classname = "Proto2ROSTests"; +import "google/protobuf/any.proto"; +import "google/protobuf/type.proto"; + import "bosdyn/api/geometry.proto"; enum Direction { @@ -195,3 +198,19 @@ message Map { } map submaps = 1; } + +// Protobuf type query request message. +message RTTIQueryRequest { + google.protobuf.Any msg = 1; +} + +// Protobuf type query result message. +message RTTIQueryResult { + google.protobuf.Type type = 1; +} + +// Generalized goal. +message Goal { + google.protobuf.Any target = 1; + google.protobuf.Any roi = 2; +} diff --git a/proto2ros_tests/proto2ros_tests/manual_conversions.py b/proto2ros_tests/proto2ros_tests/manual_conversions.py index d3f33e9..841f02b 100644 --- a/proto2ros_tests/proto2ros_tests/manual_conversions.py +++ b/proto2ros_tests/proto2ros_tests/manual_conversions.py @@ -1,3 +1,5 @@ +import math + import bosdyn.api.geometry_pb2 import geometry_msgs.msg @@ -43,7 +45,7 @@ def convert_bosdyn_api_quaternion_proto_to_geometry_msgs_quaternion_message( @convert.register(geometry_msgs.msg.Pose, bosdyn.api.geometry_pb2.SE3Pose) -def convert_geometry_msgs_pose_message_to_bosdyn_api_pose_proto( +def convert_geometry_msgs_pose_message_to_bosdyn_api_se3_pose_proto( ros_msg: geometry_msgs.msg.Pose, proto_msg: bosdyn.api.geometry_pb2.SE3Pose ) -> None: convert_geometry_msgs_vector3_message_to_bosdyn_api_vec3_proto(ros_msg.position, proto_msg.position) @@ -51,13 +53,68 @@ def convert_geometry_msgs_pose_message_to_bosdyn_api_pose_proto( @convert.register(bosdyn.api.geometry_pb2.SE3Pose, geometry_msgs.msg.Pose) -def convert_bosdyn_api_pose_proto_to_geometry_msgs_pose_message( +def convert_bosdyn_api_se3_pose_proto_to_geometry_msgs_pose_message( proto_msg: bosdyn.api.geometry_pb2.SE3Pose, ros_msg: geometry_msgs.msg.Pose ) -> None: convert_bosdyn_api_vec3_proto_to_geometry_msgs_vector3_message(proto_msg.position, ros_msg.position) convert_bosdyn_api_quaternion_proto_to_geometry_msgs_quaternion_message(proto_msg.rotation, ros_msg.orientation) +@convert.register(geometry_msgs.msg.Pose, bosdyn.api.geometry_pb2.SE2Pose) +def convert_geometry_msgs_pose_message_to_bosdyn_api_se2_pose_proto( + ros_msg: geometry_msgs.msg.Pose, proto_msg: bosdyn.api.geometry_pb2.SE2Pose +) -> None: + proto_msg.position.x = ros_msg.position.x + proto_msg.position.y = ros_msg.position.y + proto_msg.angle = 2.0 * math.acos(ros_msg.orientation.w) + + +@convert.register(bosdyn.api.geometry_pb2.SE2Pose, geometry_msgs.msg.Pose) +def convert_bosdyn_api_se2_pose_proto_to_geometry_msgs_pose_message( + proto_msg: bosdyn.api.geometry_pb2.SE2Pose, ros_msg: geometry_msgs.msg.Pose +) -> None: + ros_msg.position.x = proto_msg.position.x + ros_msg.position.y = proto_msg.position.y + ros_msg.orientation.w = math.cos(proto_msg.angle / 2.0) + ros_msg.orientation.z = math.sin(proto_msg.angle / 2.0) + + +@convert.register(geometry_msgs.msg.Polygon, bosdyn.api.geometry_pb2.Polygon) +def convert_geometry_msgs_polygon_message_to_bosdyn_api_polygon_proto( + ros_msg: geometry_msgs.msg.Polygon, proto_msg: bosdyn.api.geometry_pb2.Polygon +) -> None: + proto_msg.Clear() + for point in ros_msg.points: + vertex = proto_msg.vertexes.add() + vertex.x = point.x + vertex.y = point.y + + +@convert.register(bosdyn.api.geometry_pb2.Polygon, geometry_msgs.msg.Polygon) +def convert_bosdyn_api_polygon_proto_to_geometry_msgs_polygon_message( + proto_msg: bosdyn.api.geometry_pb2.Polygon, ros_msg: geometry_msgs.msg.Polygon +) -> None: + ros_msg.points = [geometry_msgs.msg.Point32(x=vertex.x, y=vertex.y, z=0.0) for vertex in proto_msg.vertexes] + + +@convert.register(geometry_msgs.msg.Vector3, bosdyn.api.geometry_pb2.Circle) +def convert_geometry_msgs_vector3_message_to_bosdyn_api_circle_proto( + ros_msg: geometry_msgs.msg.Vector3, proto_msg: bosdyn.api.geometry_pb2.Circle +) -> None: + proto_msg.center_pt.x = ros_msg.x + proto_msg.center_pt.y = ros_msg.y + proto_msg.radius = ros_msg.z + + +@convert.register(bosdyn.api.geometry_pb2.Circle, geometry_msgs.msg.Vector3) +def convert_bosdyn_api_circle_proto_to_geometry_msgs_vector3_message( + proto_msg: bosdyn.api.geometry_pb2.Circle, ros_msg: geometry_msgs.msg.Vector3 +) -> None: + ros_msg.x = proto_msg.center_pt.x + ros_msg.y = proto_msg.center_pt.y + ros_msg.z = proto_msg.radius + + @convert.register(geometry_msgs.msg.Twist, bosdyn.api.geometry_pb2.SE3Velocity) def convert_geometry_msgs_twist_message_to_bosdyn_api_se3_velocity_proto( ros_msg: geometry_msgs.msg.Twist, proto_msg: bosdyn.api.geometry_pb2.SE3Velocity diff --git a/proto2ros_tests/test/generated/Goal.msg b/proto2ros_tests/test/generated/Goal.msg new file mode 100644 index 0000000..b42f910 --- /dev/null +++ b/proto2ros_tests/test/generated/Goal.msg @@ -0,0 +1,8 @@ +# Generalized goal. + +uint8 TARGET_FIELD_SET=1 +uint8 ROI_FIELD_SET=2 + +geometry_msgs/Pose target +proto2ros/Any roi +uint8 has_field 255 diff --git a/proto2ros_tests/test/generated/RTTIQueryRequest.msg b/proto2ros_tests/test/generated/RTTIQueryRequest.msg new file mode 100644 index 0000000..f762e1e --- /dev/null +++ b/proto2ros_tests/test/generated/RTTIQueryRequest.msg @@ -0,0 +1,6 @@ +# Protobuf type query request message. + +uint8 MSG_FIELD_SET=1 + +proto2ros/AnyProto msg +uint8 has_field 255 diff --git a/proto2ros_tests/test/generated/RTTIQueryResult.msg b/proto2ros_tests/test/generated/RTTIQueryResult.msg new file mode 100644 index 0000000..8244044 --- /dev/null +++ b/proto2ros_tests/test/generated/RTTIQueryResult.msg @@ -0,0 +1,6 @@ +# Protobuf type query result message. + +uint8 TYPE_FIELD_SET=1 + +proto2ros/AnyProto type +uint8 has_field 255 diff --git a/proto2ros_tests/test/test_proto2ros.py b/proto2ros_tests/test/test_proto2ros.py index 706a3ab..5ad262e 100644 --- a/proto2ros_tests/test/test_proto2ros.py +++ b/proto2ros_tests/test/test_proto2ros.py @@ -1,11 +1,16 @@ # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. import filecmp +import math import os import pathlib +import bosdyn.api.geometry_pb2 +import geometry_msgs.msg +import google.protobuf.type_pb2 import test_pb2 +import proto2ros.msg import proto2ros_tests.msg from proto2ros_tests.conversions import convert @@ -239,3 +244,89 @@ def test_messages_with_submessage_map_field() -> None: assert other_proto_fragment.height == proto_fragment.height assert other_proto_fragment.width == proto_fragment.width assert other_proto_fragment.grid == proto_fragment.grid + + +def test_messages_with_any_fields() -> None: + proto_matrix = test_pb2.Matrix() + proto_matrix.rows = 1 + proto_matrix.cols = 1 + proto_matrix.data.append(0) + + proto_request = test_pb2.RTTIQueryRequest() + proto_request.msg.Pack(proto_matrix) + + ros_request = proto2ros_tests.msg.RTTIQueryRequest() + convert(proto_request, ros_request) + assert isinstance(ros_request.msg, proto2ros.msg.AnyProto) + + unpacked_proto_matrix = test_pb2.Matrix() + convert(ros_request.msg, unpacked_proto_matrix) + assert unpacked_proto_matrix.rows == proto_matrix.rows + assert unpacked_proto_matrix.cols == proto_matrix.cols + assert unpacked_proto_matrix.data == proto_matrix.data + + other_proto_request = test_pb2.RTTIQueryRequest() + convert(ros_request, other_proto_request) + + other_unpacked_proto_matrix = test_pb2.Matrix() + other_proto_request.msg.Unpack(other_unpacked_proto_matrix) + assert other_unpacked_proto_matrix.rows == proto_matrix.rows + assert other_unpacked_proto_matrix.cols == proto_matrix.cols + assert other_unpacked_proto_matrix.data == proto_matrix.data + + +def test_messages_with_unknown_type_fields() -> None: + proto_result = test_pb2.RTTIQueryResult() + proto_result.type.name = "SomeMessage" + + ros_result = proto2ros_tests.msg.RTTIQueryResult() + convert(proto_result, ros_result) + assert isinstance(ros_result.type, proto2ros.msg.AnyProto) + + unpacked_proto_type = google.protobuf.type_pb2.Type() + convert(ros_result.type, unpacked_proto_type) + assert unpacked_proto_type.name == proto_result.type.name + + other_proto_result = test_pb2.RTTIQueryResult() + convert(ros_result, other_proto_result) + assert other_proto_result.type.name == proto_result.type.name + + +def test_messages_with_expanded_any_fields() -> None: + proto_target = bosdyn.api.geometry_pb2.SE2Pose() + proto_target.position.x = 1.0 + proto_target.position.y = -1.0 + proto_target.angle = math.pi + + proto_roi = bosdyn.api.geometry_pb2.Polygon() + for dx in (-0.5, 0.5): + for dy in (-0.5, 0.5): + vertex = proto_roi.vertexes.add() + vertex.x = proto_target.position.x + dx + vertex.y = proto_target.position.y + dy + + proto_goal = test_pb2.Goal() + proto_goal.target.Pack(proto_target) + proto_goal.roi.Pack(proto_roi) + + ros_goal = proto2ros_tests.msg.Goal() + convert(proto_goal, ros_goal) + assert isinstance(ros_goal.target, geometry_msgs.msg.Pose) + assert isinstance(ros_goal.roi, proto2ros.msg.Any) + assert ros_goal.target.position.x == proto_target.position.x + assert ros_goal.target.position.y == proto_target.position.y + assert ros_goal.roi.type_name == "geometry_msgs/Polygon" + + other_proto_goal = test_pb2.Goal() + convert(ros_goal, other_proto_goal) + other_proto_target = bosdyn.api.geometry_pb2.SE2Pose() + other_proto_goal.target.Unpack(other_proto_target) + assert proto_target.position.x == other_proto_target.position.x + assert proto_target.position.y == other_proto_target.position.y + assert proto_target.angle == other_proto_target.angle + + other_proto_roi = bosdyn.api.geometry_pb2.Polygon() + other_proto_goal.roi.Unpack(other_proto_roi) + assert len(other_proto_roi.vertexes) == 4 + for a, b in zip(proto_roi.vertexes, other_proto_roi.vertexes): + assert a.x == b.x and a.y == b.y