diff --git a/doc/index.rst b/doc/index.rst index 2f9b5332..594cb4c7 100644 --- a/doc/index.rst +++ b/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. diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index 4b6f9318..28a3e8ac 100644 --- a/resource/interface_factories.cpp.em +++ b/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]@ diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 970e7d3c..7841c4b2 100644 --- a/ros1_bridge/__init__.py +++ b/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)]