Fix any expansions in proto2ros

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

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)

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,27 +192,27 @@ 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"
     typed_field_message = {{ ros_type_name | as_ros_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

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

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<int32, Fragment> 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;
+}

proto2ros_tests/proto2ros_tests/manual_conversions.py

@@ -1,3 +1,5 @@
+import math
+
 import bosdyn.api.geometry_pb2
 import geometry_msgs.msg
 
@@ -43,21 +45,76 @@ 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)
     convert_geometry_msgs_quaternion_message_to_bosdyn_api_quaternion_proto(ros_msg.orientation, proto_msg.rotation)
 
 
 @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

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

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

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