Skip to content

Commit

Permalink
Support field selection (#174)
Browse files Browse the repository at this point in the history 
* Add field selection for ROS 1 fields to mappings

Signed-off-by: Juan Rodriguez Hortala <[email protected]>

* Add field selection for ROS 2 fields to mappings

Signed-off-by: Juan Rodriguez Hortala <[email protected]>

* Add documentation for field selection

Signed-off-by: Juan Rodriguez Hortala <[email protected]>

* Fix linting issues with flake8 and pep257

Signed-off-by: Juan Rodriguez Hortala <[email protected]>

* rebase consolidation

Signed-off-by: Gonzalo de Pedro <[email protected]>

* Linter and review comments

Signed-off-by: Gonzalo de Pedro <[email protected]>

* Use isinstance instead of type()

Signed-off-by: Gonzalo de Pedro <[email protected]>
  • Loading branch information
@juanrh @dirk-thomas
juanrh authored and dirk-thomas committed May 16, 2019
1 parent 7f3756e commit 43e2fca
Show file tree
Hide file tree
Showing 3 changed files with 191 additions and 64 deletions.
6 changes: 5 additions & 1 deletion doc/index.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
100 changes: 54 additions & 46 deletions resource/interface_factories.cpp.em
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]@
Expand All @@ -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]@
Expand Down
149 changes: 132 additions & 17 deletions ros1_bridge/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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):
Expand All @@ -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)]

0 comments on commit 43e2fca

Please sign in to comment.