Support field selection

doc/index.rst

@@ -40,7 +40,11 @@ Each mapping rule can have one of three types:
 
 3. A field mapping rule is defined by the attributes of a message mapping rule and:
 
-   - a dictionary ``fields_1_to_2`` mapping ROS 1 field names to ROS 2 field names.
+   - a dictionary ``fields_1_to_2`` mapping ROS 1 field selections to ROS 2 field selections.
+     A field selection is a sequence of field names separated by `.`, that specifies the path to a field starting from a message.
+     For example starting from the message `rosgraph_msgs/Log` the field selection `header.stamp` specifies a
+     path that goes through the field `header` of `rosgraph_msgs/Log`, that has a message of type `std_msgs/Header`,
+     and ending up in the field `stamp` of `std_msgs/Header`, that has type `time`.
      All fields must be listed explicitly - not listed fields are not mapped implicitly when their names match.
 
 Each mapping rule file contains a list of mapping rules.

resource/interface_factories.cpp.em

@@ -114,59 +114,63 @@ Factory<
   (void)ros1_msg;
   (void)ros2_msg;
 @[  end if]@
-@[  for ros1_field, ros2_field in m.fields_1_to_2.items()]@
-@[    if not ros2_field.type.is_array]@
+@[  for ros1_fields, ros2_fields in m.fields_1_to_2.items()]@
+@{
+ros1_field_selection = '.'.join((str(field.name) for field in ros1_fields))
+ros2_field_selection = '.'.join((str(field.name) for field in ros2_fields))
+}
+@[    if not ros2_fields[-1].type.is_array]@
   // convert non-array field
-@[      if not ros2_field.type.pkg_name]@
+@[      if not ros2_fields[-1].type.pkg_name]@
   // convert primitive field
-  ros2_msg.@(ros2_field.name) = ros1_msg.@(ros1_field.name);
-@[      elif ros2_field.type.pkg_name == 'builtin_interfaces']@
+  ros2_msg.@(ros2_field_selection) = ros1_msg.@(ros1_field_selection);
+@[      elif ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@
   // convert builtin field
-  ros1_bridge::convert_1_to_2(ros1_msg.@(ros1_field.name), ros2_msg.@(ros2_field.name));
+  ros1_bridge::convert_1_to_2(ros1_msg.@(ros1_field_selection), ros2_msg.@(ros2_field_selection));
 @[      else]@
   // convert sub message field
   Factory<
-    @(ros1_field.pkg_name)::@(ros1_field.msg_name),
-    @(ros2_field.type.pkg_name)::msg::@(ros2_field.type.type)
+    @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name),
+    @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type)
   >::convert_1_to_2(
-    ros1_msg.@(ros1_field.name), ros2_msg.@(ros2_field.name));
+    ros1_msg.@(ros1_field_selection), ros2_msg.@(ros2_field_selection));
 @[      end if]@
 @[    else]@
   // convert array field
-@[      if not ros2_field.type.array_size or ros2_field.type.is_upper_bound]@
+@[      if not ros2_fields[-1].type.array_size or ros2_fields[-1].type.is_upper_bound]@
   // ensure array size
-@[        if ros2_field.type.is_upper_bound]@
+@[        if ros2_fields[-1].type.is_upper_bound]@
   // check boundary
-  assert(ros1_msg.@(ros1_field.name).size() <= @(ros2_field.type.array_size));
+  assert(ros1_msg.@(ros1_field_selection).size() <= @(ros2_fields[-1].type.array_size));
 @[        end if]@
   // dynamic arrays must be resized
-  ros2_msg.@(ros2_field.name).resize(ros1_msg.@(ros1_field.name).size());
+  ros2_msg.@(ros2_field_selection).resize(ros1_msg.@(ros1_field_selection).size());
 @[      end if]@
-@[      if not ros2_field.type.pkg_name]@
+@[      if not ros2_fields[-1].type.pkg_name]@
   // convert primitive array elements
   std::copy(
-    ros1_msg.@(ros1_field.name).begin(),
-    ros1_msg.@(ros1_field.name).end(),
-    ros2_msg.@(ros2_field.name).begin());
+    ros1_msg.@(ros1_field_selection).begin(),
+    ros1_msg.@(ros1_field_selection).end(),
+    ros2_msg.@(ros2_field_selection).begin());
 @[      else]@
   // copy element wise since the type is different
   {
-    auto ros1_it = ros1_msg.@(ros1_field.name).begin();
-    auto ros2_it = ros2_msg.@(ros2_field.name).begin();
+    auto ros1_it = ros1_msg.@(ros1_field_selection).begin();
+    auto ros2_it = ros2_msg.@(ros2_field_selection).begin();
     for (
       ;
-      ros1_it != ros1_msg.@(ros1_field.name).end() &&
-      ros2_it != ros2_msg.@(ros2_field.name).end();
+      ros1_it != ros1_msg.@(ros1_field_selection).end() &&
+      ros2_it != ros2_msg.@(ros2_field_selection).end();
       ++ros1_it, ++ros2_it
     )
     {
       // convert sub message element
-@[        if ros2_field.type.pkg_name == 'builtin_interfaces']@
+@[        if ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@
       ros1_bridge::convert_1_to_2(*ros1_it, *ros2_it);
 @[        else]@
       Factory<
-        @(ros1_field.pkg_name)::@(ros1_field.msg_name),
-        @(ros2_field.type.pkg_name)::msg::@(ros2_field.type.type)
+        @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name),
+        @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type)
       >::convert_1_to_2(
         *ros1_it, *ros2_it);
 @[        end if]@
@@ -190,55 +194,59 @@ Factory<
   (void)ros2_msg;
   (void)ros1_msg;
 @[  end if]@
-@[  for ros2_field, ros1_field in m.fields_2_to_1.items()]@
-@[    if not ros2_field.type.is_array]@
+@[  for ros2_fields, ros1_fields in m.fields_2_to_1.items()]@
+@{
+ros2_field_selection = '.'.join((str(field.name) for field in ros2_fields))
+ros1_field_selection = '.'.join((str(field.name) for field in ros1_fields))
+}
+@[    if not ros2_fields[-1].type.is_array]@
   // convert non-array field
-@[      if not ros2_field.type.pkg_name]@
+@[      if not ros2_fields[-1].type.pkg_name]@
   // convert primitive field
-  ros1_msg.@(ros1_field.name) = ros2_msg.@(ros2_field.name);
-@[      elif ros2_field.type.pkg_name == 'builtin_interfaces']@
+  ros1_msg.@(ros1_field_selection) = ros2_msg.@(ros2_field_selection);
+@[      elif ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@
   // convert builtin field
-  ros1_bridge::convert_2_to_1(ros2_msg.@(ros2_field.name), ros1_msg.@(ros1_field.name));
+  ros1_bridge::convert_2_to_1(ros2_msg.@(ros2_field_selection), ros1_msg.@(ros1_field_selection));
 @[      else]@
   // convert sub message field
   Factory<
-    @(ros1_field.pkg_name)::@(ros1_field.msg_name),
-    @(ros2_field.type.pkg_name)::msg::@(ros2_field.type.type)
+    @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name),
+    @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type)
   >::convert_2_to_1(
-    ros2_msg.@(ros2_field.name), ros1_msg.@(ros1_field.name));
+    ros2_msg.@(ros2_field_selection), ros1_msg.@(ros1_field_selection));
 @[      end if]@
 @[    else]@
   // convert array field
-@[      if not ros2_field.type.array_size or ros2_field.type.is_upper_bound]@
+@[      if not ros2_fields[-1].type.array_size or ros2_fields[-1].type.is_upper_bound]@
   // ensure array size
   // dynamic arrays must be resized
-  ros1_msg.@(ros1_field.name).resize(ros2_msg.@(ros2_field.name).size());
+  ros1_msg.@(ros1_field_selection).resize(ros2_msg.@(ros2_field_selection).size());
 @[      end if]@
-@[      if not ros2_field.type.pkg_name]@
+@[      if not ros2_fields[-1].type.pkg_name]@
   // convert primitive array elements
   std::copy(
-    ros2_msg.@(ros2_field.name).begin(),
-    ros2_msg.@(ros2_field.name).end(),
-    ros1_msg.@(ros1_field.name).begin());
+    ros2_msg.@(ros2_field_selection).begin(),
+    ros2_msg.@(ros2_field_selection).end(),
+    ros1_msg.@(ros1_field_selection).begin());
 @[      else]@
   // copy element wise since the type is different
   {
-    auto ros2_it = ros2_msg.@(ros2_field.name).begin();
-    auto ros1_it = ros1_msg.@(ros1_field.name).begin();
+    auto ros2_it = ros2_msg.@(ros2_field_selection).begin();
+    auto ros1_it = ros1_msg.@(ros1_field_selection).begin();
     for (
       ;
-      ros2_it != ros2_msg.@(ros2_field.name).end() &&
-      ros1_it != ros1_msg.@(ros1_field.name).end();
+      ros2_it != ros2_msg.@(ros2_field_selection).end() &&
+      ros1_it != ros1_msg.@(ros1_field_selection).end();
       ++ros2_it, ++ros1_it
     )
     {
       // convert sub message element
-@[        if ros2_field.type.pkg_name == 'builtin_interfaces']@
+@[        if ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@
       ros1_bridge::convert_2_to_1(*ros2_it, *ros1_it);
 @[        else]@
       Factory<
-        @(ros1_field.pkg_name)::@(ros1_field.msg_name),
-        @(ros2_field.type.pkg_name)::msg::@(ros2_field.type.type)
+        @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name),
+        @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type)
       >::convert_2_to_1(
         *ros2_it, *ros1_it);
 @[        end if]@

ros1_bridge/__init__.py

@@ -167,8 +167,13 @@ def generate_messages(rospack=None):
         if ros1_msg and ros2_msg:
             mappings.append(Mapping(ros1_msg[0], ros2_msg[0]))
 
+    msg_idx = MessageIndex()
     for ros1_msg, ros2_msg in message_pairs:
-        mapping = determine_field_mapping(ros1_msg, ros2_msg, mapping_rules)
+        msg_idx.ros1_put(ros1_msg)
+        msg_idx.ros2_put(ros2_msg)
+
+    for ros1_msg, ros2_msg in message_pairs:
+        mapping = determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx)
         if mapping:
             mappings.append(mapping)
 
@@ -620,7 +625,62 @@ def update_ros1_field_information(ros1_field, package_name):
         ros1_field.msg_name = parts[1]
 
 
-def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules):
+def get_ros1_selected_fields(ros1_field_selection, parent_ros1_spec, msg_idx):
+    """
+    Get a tuple of fields corresponding to a field selection on a ROS 1 message.
+
+    :param ros1_field_selection: a string with message field names separated by `.`
+    :param parent_ros1_spec: a genmsg.MsgSpec for a message that contains the first field
+    in ros1_field_selection
+    :type msg_idx: MessageIndex
+
+    :return: a tuple of genmsg.msgs.Field objets with additional attributes `pkg_name`
+    and `msg_name` as defined by `update_ros1_field_information`, corresponding to
+    traversing `parent_ros1_spec` recursively following `ros1_field_selection`
+
+    :throws: IndexError in case some expected field is not found while traversing
+    `parent_ros1_spec` recursively following `ros1_field_selection`
+    """
+    selected_fields = []
+
+    def consume_field(field):
+        update_ros1_field_information(field, parent_ros1_spec.package)
+        selected_fields.append(field)
+
+    fields = ros1_field_selection.split('.')
+    current_field = [f for f in parent_ros1_spec.parsed_fields() if f.name == fields[0]][0]
+    consume_field(current_field)
+    for field in fields[1:]:
+        parent_ros1_spec = load_ros1_message(msg_idx.ros1_get_from_field(current_field))
+        current_field = [f for f in parent_ros1_spec.parsed_fields() if f.name == field][0]
+        consume_field(current_field)
+
+    return tuple(selected_fields)
+
+
+def get_ros2_selected_fields(ros2_field_selection, parent_ros2_spec, msg_idx):
+    selected_fields = []
+    fields = ros2_field_selection.split('.')
+    current_field = [f for f in parent_ros2_spec.fields if f.name == fields[0]][0]
+    selected_fields.append(current_field)
+    for field in fields[1:]:
+        parent_ros2_spec = load_ros2_message(msg_idx.ros2_get_from_field(current_field))
+        current_field = [f for f in parent_ros2_spec.fields if f.name == field][0]
+        selected_fields.append(current_field)
+    return tuple(selected_fields)
+
+
+def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx):
+    """
+    Return the first mapping object for ros1_msg and ros2_msg found in mapping_rules.
+
+    If not found in mapping_rules otherwise defined implicitly, or None if no mapping is found.
+
+    :type ros1_msg: Message
+    :type ros2_msg: Message
+    :type mapping_rules: list of MessageMappingRule
+    :type msg_idx: MessageIndex
+    """
     ros1_spec = load_ros1_message(ros1_msg)
     if not ros1_spec:
         return None
@@ -641,29 +701,28 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules):
                 rule.ros2_message_name != ros2_msg.message_name:
             continue
 
-        for ros1_field_name, ros2_field_name in rule.fields_1_to_2.items():
+        for ros1_field_selection, ros2_field_selection in rule.fields_1_to_2.items():
             try:
-                ros1_field = \
-                    [f for f in ros1_spec.parsed_fields() if f.name == ros1_field_name][0]
+                ros1_selected_fields = \
+                    get_ros1_selected_fields(ros1_field_selection, ros1_spec, msg_idx)
             except IndexError:
                 print(
-                    "A manual mapping refers to an invalid field '%s' " % ros1_field_name +
+                    "A manual mapping refers to an invalid field '%s' " % ros1_field_selection +
                     "in the ROS 1 message '%s/%s'" %
                     (rule.ros1_package_name, rule.ros1_message_name),
                     file=sys.stderr)
                 continue
             try:
-                ros2_field = \
-                    [f for f in ros2_spec.fields if f.name == ros2_field_name][0]
+                ros2_selected_fields = \
+                    get_ros2_selected_fields(ros2_field_selection, ros2_spec, msg_idx)
             except IndexError:
                 print(
-                    "A manual mapping refers to an invalid field '%s' " % ros2_field_name +
+                    "A manual mapping refers to an invalid field '%s' " % ros2_field_selection +
                     "in the ROS 2 message '%s/%s'" %
                     (rule.ros2_package_name, rule.ros2_message_name),
                     file=sys.stderr)
                 continue
-            update_ros1_field_information(ros1_field, ros1_msg.package_name)
-            mapping.add_field_pair(ros1_field, ros2_field)
+            mapping.add_field_pair(ros1_selected_fields, ros2_selected_fields)
         return mapping
 
     # apply name based mapping of fields
@@ -765,12 +824,26 @@ def __init__(self, ros1_msg, ros2_msg):
         self.fields_2_to_1 = OrderedDict()
         self.depends_on_ros2_messages = set()
 
-    def add_field_pair(self, ros1_field, ros2_field):
-        self.fields_1_to_2[ros1_field] = ros2_field
-        self.fields_2_to_1[ros2_field] = ros1_field
-        if ros2_field.type.pkg_name and ros2_field.type.pkg_name != 'builtin_interfaces':
-            self.depends_on_ros2_messages.add(
-                Message(ros2_field.type.pkg_name, ros2_field.type.type))
+    def add_field_pair(self, ros1_fields, ros2_fields):
+        """
+        Add a new mapping for a pair of ROS 1 and ROS 2 messages.
+
+        :type ros1_fields: either a genmsg.msgs.Field object with additional attributes `pkg_name`
+        and `msg_name` as defined by `update_ros1_field_information`, or a tuple of objects of
+        that type
+        :type ros2_field: either a rosidl_adapter.parser.Field object, or a tuple objects of
+        that type
+        """
+        if not isinstance(ros1_fields, tuple):
+            ros1_fields = (ros1_fields,)
+        if not isinstance(ros2_fields, tuple):
+            ros2_fields = (ros2_fields, )
+        self.fields_1_to_2[ros1_fields] = ros2_fields
+        self.fields_2_to_1[ros2_fields] = ros1_fields
+        for ros2_field in ros2_fields:
+            if ros2_field.type.pkg_name and ros2_field.type.pkg_name != 'builtin_interfaces':
+                self.depends_on_ros2_messages.add(
+                    Message(ros2_field.type.pkg_name, ros2_field.type.type))
 
 
 def camel_case_to_lower_case_underscore(value):
@@ -781,3 +854,45 @@ def camel_case_to_lower_case_underscore(value):
     # which is preseded by a lower case letter or number
     value = re.sub('([a-z0-9])([A-Z])', '\\1_\\2', value)
     return value.lower()
+
+
+class MessageIndex:
+    """
+    Index from package and message names to Message objects.
+
+    Maintains 2 indices from (package_name, message_name) to Message,
+    one for ROS 1 messages and another for ROS 2 messages
+    """
+
+    def __init__(self):
+        self._ros1_idx = {}
+        self._ros2_idx = {}
+
+    def ros1_put(self, msg):
+        """Add msg to the ROS1 index."""
+        self._ros1_idx[(msg.package_name, msg.message_name)] = msg
+
+    def ros2_put(self, msg):
+        """Add msg to the ROS2 index."""
+        self._ros2_idx[(msg.package_name, msg.message_name)] = msg
+
+    def ros1_get_from_field(self, field):
+        """
+        Get Message from ROS 1 index.
+
+        :type field: genmsg.msgs.Field with additional fields `pkg_name`
+        and `msg_name` as added by `update_ros1_field_information`
+        :return: the message indexed for the fields `pkg_name` and
+        `msg_name` of `field`
+        """
+        return self._ros1_idx[(field.pkg_name, field.msg_name)]
+
+    def ros2_get_from_field(self, field):
+        """
+        Get Message from ROS 2 index.
+
+        :type field: rosidl_adapter.parser.Field
+        :return: the message indexed for the fields `type.pkg_name` and
+        `type.type` of `field`
+        """
+        return self._ros2_idx[(field.type.pkg_name, field.type.type)]