diff --git a/README.md b/README.md index 05726b6..7d7c645 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ A package to bridge actions between ROS1 and ROS2. - + +Please clone `dashing-devel` branch in case you are working with ROS Dashing. + **NOTE:** - Currently supports forwarding goals from ROS1 (melodic) action client to ROS2 (dashing) action server - As an example, implemented interfaces for the action bridge for FibonacciAction diff --git a/resources/interface_factories.cpp.em b/resources/interface_factories.cpp.em new file mode 100644 index 0000000..cba923e --- /dev/null +++ b/resources/interface_factories.cpp.em @@ -0,0 +1,130 @@ +// Copyright 2019 Fraunhofer IPA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +@ +@{ +from ros1_bridge import camel_case_to_lower_case_underscore +}@ + +//need a find the proper path for the factories +#include "@(ros2_package_name)_factories.hpp" +#include + +#include "rclcpp/rclcpp.hpp" + +// include builtin interfaces +#include + + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunused-parameter" +#endif +// include ROS 1 actions +@[for action in mapped_actions]@ +#include <@(action["ros1_package"])/@(action["ros1_name"]).h> +hello +@[end for]@ +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +// include ROS 2 actions +@[for action in mapped_actions]@ +#include <@(action["ros2_package"])/action/@(camel_case_to_lower_case_underscore(action["ros2_name"])).hpp> +@[end for]@ + + +std::unique_ptr +get_action_factory_@(ros2_package_name)__@(interface_type)__@(interface.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name) +{ +@[if not mapped_actions]@ + (void)ros_id; + (void)package_name; + (void)action_name; +@[end if]@ +@[for action in mapped_actions]@ + if ( + ( + ros_id == "ros1" && + package_name == "@(action["ros1_package"])" && + action_name == "@(action["ros1_name"])" + ) || ( + ros_id == "ros2" && + package_name == "@(action["ros2_package"])" && + action_name == "action/@(action["ros2_name"])" + ) + ) { + return std::unique_ptr(new ActionFactory< + @(action["ros1_package"])::@(action["ros1_name"]), + @(action["ros2_package"])::srv::@(action["ros2_name"]) + >); + } +@[end for]@ + return nullptr; +} +@ + +@[for action in mapped_actions]@ +@[ for frm, to in [("1", "2"), ("2", "1")]]@ +@[ for type in ["Goal", "Result", "Feedback"]]@ +template <> +void ActionFactory< +@(action["ros1_package"])::@(action["ros1_name"])Action, +@(action["ros2_package"])::action::@(action["ros2_name"]) +>::translate_@(type.lower())_@(frm)_to_@(to)( +@[ if type == "Goal"]@ + const ROS@(frm)Goal &@(type.lower())@(frm), + ROS@(to)Goal &@(type.lower())@(to)) +@[ else]@ + const ROS@(to)Goal &@(type.lower())@(to), + ROS@(frm)Goal &@(type.lower())@(frm)) +@[ end if]@ +{ +@[ for field in action["fields"][type.lower()]]@ +@[ if field["array"]]@ + @(type.lower())@(to).@(field["ros" + frm]["name"]).resize(@(type.lower())@(frm).@(field["ros" + to]["name"]).size()); + auto @(field["ros" + frm]["name"])@(frm)_it = @(type.lower())@(frm).@(field["ros" + frm]["name"]).begin(); + auto @(field["ros" + to]["name"])@(to)_it = @(type.lower())@(to).@(field["ros" + to]["name"]).begin(); + while ( + @(field["ros" + frm]["name"])@(frm)_it != @(type.lower())@(frm).@(field["ros" + frm]["name"]).end() && + @(field["ros" + to]["name"])@(to)_it != @(type.lower())@(to).@(field["ros" + to]["name"]).end() + ) { + auto & @(field["ros" + frm]["name"])@(frm) = *(@(field["ros" + frm]["name"])@(frm)_it++); + auto & @(field["ros" + to]["name"])@(to) = *(@(field["ros" + to]["name"])@(to)_it++); +@[ else]@ + auto & @(field["ros" + frm]["name"])@(frm) = @(type.lower())@(frm).@(field["ros" + frm]["name"]); + auto & @(field["ros" + to]["name"])@(to) = @(type.lower())@(to).@(field["ros" + to]["name"]); +@[ end if]@ +@[ if field["basic"]]@ + @(field["ros" + to]["name"])@(to) = @(field["ros" + frm]["name"])@(frm); +@[ else]@ + Factory<@(field["ros" + frm]["cpptype"]),@(field["ros" + to]["cpptype"])>::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); +@[end if]@ +@[ if field["array"]]@ + } +@[ end if]@ + +@[ end for]@ +} + +@[ end for]@ + +@[ end for]@ + +@[end for]@ + + + diff --git a/scripts/generator.py b/scripts/generator.py new file mode 100755 index 0000000..58335f3 --- /dev/null +++ b/scripts/generator.py @@ -0,0 +1,441 @@ +import os +import sys +from collections import OrderedDict +try: + from cStringIO import StringIO # Python 2.x +except ImportError: + from io import StringIO # Python 3.x + +# ROS 1 imports +import ament_index_python +import genmsg.msg_loader +from genmsg.base import COMMENTCHAR, IODELIM +import rosmsg +import rospkg +from catkin_pkg.package import parse_package +import yaml +from init import Message, Mapping, MappingRule +# from rosidl_cmake import expand_template +import rosidl_adapter.parser +from rosidl_cmake import expand_template + + +# genmsg/actions.py? +class ActionSpec(object): + + def __init__(self, goal, result, feedback, text, full_name='', short_name='', package=''): + + alt_package, alt_short_name = genmsg.package_resource_name(full_name) + if not package: + package = alt_package + if not short_name: + short_name = alt_short_name + + self.goal = goal + self.result = result + self.feedback = feedback + self.text = text + self.full_name = full_name + self.short_name = short_name + self.package = package + + def __eq__(self, other): + if not other or not isinstance(other, ActionSpec): + return False + return self.goal == other.goal and \ + self.result == other.result and \ + self.feedback == other.feedback and \ + self.text == other.text and \ + self.full_name == other.full_name and \ + self.short_name == other.short_name and \ + self.package == other.package + + def __ne__(self, other): + if not other or not isinstance(other, ActionSpec): + return True + return not self.__eq__(other) + + def __repr__(self): + return "ActionSpec[%s, %s, %s]" % (repr(self.goal), repr(self.result), repr(self.feedback)) + + +# genmsg.msg_loader +def load_action_from_string(msg_context, text, full_name): + """ + Load :class:`ActionSpec` from the .action file. + + :param msg_context: :class:`MsgContext` instance to load goal/reresult/feedback messages into. + :param text: .msg text , ``str`` + :param package_name: context to use for msg type name, i.e. the package name, + or '' to use local naming convention. ``str`` + :returns: :class:`ActionSpec` instance + :raises :exc:`InvalidMsgSpec` If syntax errors or other problems are detected in file + """ + text_goal = StringIO() + text_result = StringIO() + text_feedback = StringIO() + count = 0 + accum = text_goal + for l in text.split('\n'): + l = l.split(COMMENTCHAR)[0].strip() # strip comments + if l.startswith(IODELIM): # lenient, by request + if count == 0: + accum = text_result + count = 1 + else: + accum = text_feedback + else: + accum.write(l+'\n') + + # create separate MsgSpec objects for each half of file + msg_goal = genmsg.msg_loader.load_msg_from_string( + msg_context, text_goal.getvalue(), '%sGoal' % (full_name)) + msg_result = genmsg.msg_loader.load_msg_from_string( + msg_context, text_result.getvalue(), '%sResult' % (full_name)) + msg_feedback = genmsg.msg_loader.load_msg_from_string( + msg_context, text_feedback.getvalue(), '%sFeedback' % (full_name)) + return ActionSpec(msg_goal, msg_result, msg_feedback, text, full_name) + + +# genmsg.msg_loader +def load_action_from_file(msg_context, file_path, full_name): + """ + Convert the .action representation in the file to a :class:`MsgSpec` instance. + NOTE: this will register the message in the *msg_context*. + + :param file_path: path of file to load from, ``str`` + :returns: :class:`MsgSpec` instance + :raises: :exc:`InvalidMsgSpec`: if syntax errors or other problems are detected in file + """ + with open(file_path, 'r') as f: + text = f.read() + + spec = load_action_from_string(msg_context, text, full_name) + # msg_context.set_file('%sRequest' % (full_name), file_path) + # msg_context.set_file('%sResponse' % (full_name), file_path) + return spec + + +# __init__.py +def load_ros1_action(ros1_action): + msg_context = genmsg.MsgContext.create_default() + message_path = os.path.join( + ros1_action.prefix_path, ros1_action.message_name + '.action') + try: + spec = load_action_from_file( + msg_context, message_path, '%s/%s' % (ros1_action.package_name, ros1_action.message_name)) + except genmsg.InvalidMsgSpec: + return None + return spec + + +# __init__.py +def load_ros2_action(ros2_action): + actiom_path = os.path.join( + ros2_action.prefix_path, 'share', ros2_action.package_name, 'action', + ros2_action.message_name + '.action') + try: + spec = rosidl_adapter.parser.parse_action_file( + ros2_action.package_name, actiom_path) + except rosidl_adapter.parser.InvalidSpecification: + print("Invalid spec") + return None + return spec + + +# rosmsg.py +def iterate_action_packages(rospack): + subdir = 'action' + pkgs = rospack.list() + for p in pkgs: + package_paths = rosmsg._get_package_paths(p, rospack) + for package_path in package_paths: + d = os.path.join(package_path, subdir) + if os.path.isdir(d): + yield p, d + + +# __init__.py +def get_ros1_actions(rospack=None): + if not rospack: + rospack = rospkg.RosPack() + actions = [] + # actual function: rosmsg.iterate_packages(rospack, rosmsg.MODE_MSG)) + pkgs = sorted(x for x in iterate_action_packages(rospack)) + for package_name, path in pkgs: + for action_name in rosmsg._list_types(path, 'msg', ".action"): + actions.append(Message(package_name, action_name, path)) + return actions + + +def get_ros2_actions(): + pkgs = [] + actions = [] + rules = [] + resource_type = 'rosidl_interfaces' + resources = ament_index_python.get_resources(resource_type) + for package_name, prefix_path in resources.items(): + pkgs.append(package_name) + resource, _ = ament_index_python.get_resource( + resource_type, package_name) + interfaces = resource.splitlines() + action_names = { + i[7:-7] + for i in interfaces + if i.startswith('action/') and i[-7:] in ('.idl', '.action')} + for action_name in sorted(action_names): + actions.append(Message(package_name, action_name, prefix_path)) + package_path = os.path.join(prefix_path, 'share', package_name) + pkg = parse_package(package_path) + for export in pkg.exports: + if export.tagname != 'ros1_bridge': + continue + if 'mapping_rules' not in export.attributes: + continue + rule_file = os.path.join( + package_path, export.attributes['mapping_rules']) + with open(rule_file, 'r') as h: + content = yaml.safe_load(h) + if not isinstance(content, list): + print( + "The content of the mapping rules in '%s' is not a list" % rule_file, + file=sys.stderr) + continue + for data in content: + if all(n not in data for n in ('ros1_message_name', 'ros2_message_name', 'ros1_service_name', 'ros2_service_name')): + try: + rules.append(ActionMappingRule(data, package_name)) + except Exception as e: + print('%s' % str(e), file=sys.stderr) + return pkgs, actions, rules + + +class ActionMappingRule(MappingRule): + __slots__ = [ + 'ros1_action_name', + 'ros2_action_name', + 'goal_fields_1_to_2', + 'result_fields_1_to_2', + 'feedback_fields_1_to_2', + ] + + def __init__(self, data, expected_package_name): + super().__init__(data, expected_package_name) + self.ros1_action_name = None + self.ros2_action_name = None + self.goal_fields_1_to_2 = None + self.result_fields_1_to_2 = None + self.feedback_fields_1_to_2 = None + if all(n in data for n in ('ros1_action_name', 'ros2_action_name')): + self.ros1_action_name = data['ros1_action_name'] + self.ros2_action_name = data['ros2_action_name'] + expected_keys = 4 + if 'goal_fields_1_to_2' in data: + self.goal_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['goal_fields_1_to_2'].items(): + self.goal_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + if 'result_fields_1_to_2' in data: + self.result_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['result_fields_1_to_2'].items(): + self.result_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + if 'feedback_fields_1_to_2' in data: + self.feedback_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['feedback_fields_1_to_2'].items(): + self.feedback_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + elif len(data) > expected_keys: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + elif len(data) > 2: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + + def __str__(self): + return 'ActionMappingRule(%s, %s)' % (self.ros1_package_name, self.ros2_package_name) + + +def determine_common_actions( + ros1_actions, ros2_actions, mapping_rules, message_string_pairs=None +): + if message_string_pairs is None: + message_string_pairs = set() + + pairs = [] + actions = [] + for ros1_action in ros1_actions: + for ros2_action in ros2_actions: + if ros1_action.package_name == ros2_action.package_name: + if ros1_action.message_name == ros2_action.message_name: + pairs.append((ros1_action, ros2_action)) + + for rule in mapping_rules: + for ros1_action in ros1_actions: + for ros2_action in ros2_actions: + if rule.ros1_package_name == ros1_action.package_name and \ + rule.ros2_package_name == ros2_action.package_name: + if rule.ros1_service_name is None and rule.ros2_service_name is None: + if ros1_action.message_name == ros2_action.message_name: + pairs.append((ros1_action, ros2_action)) + else: + if ( + rule.ros1_service_name == ros1_action.message_name and + rule.ros2_service_name == ros2_action.message_name + ): + pairs.append((ros1_action, ros2_action)) + + for pair in pairs: + ros1_spec = load_ros1_action(pair[0]) + ros2_spec = load_ros2_action(pair[1]) + ros1_fields = { + 'goal': ros1_spec.goal.fields(), + 'result': ros1_spec.result.fields(), + 'feedback': ros1_spec.feedback.fields() + } + ros2_fields = { + 'goal': ros2_spec.goal.fields, + 'result': ros2_spec.result.fields, + 'feedback': ros2_spec.feedback.fields + } + output = { + 'goal': [], + 'result': [], + 'feedback': [] + } + match = True + for direction in ['goal', 'result', 'feedback']: + if len(ros1_fields[direction]) != len(ros2_fields[direction]): + match = False + break + for i, ros1_field in enumerate(ros1_fields[direction]): + ros1_type = ros1_field[0] + ros2_type = str(ros2_fields[direction][i].type) + ros1_name = ros1_field[1] + ros2_name = ros2_fields[direction][i].name + if ros1_type != ros2_type or ros1_name != ros2_name: + # if the message types have a custom mapping their names + # might not be equal, therefore check the message pairs + # the check for 'builtin_interfaces' should be removed once merged with __init__.py + # It seems to handle it already + if (ros1_type, ros2_type) not in message_string_pairs and not ros2_type.startswith("builtin_interfaces"): + print("custom: " + ros1_type + " " + ros2_type + + " : " + ros1_name + " " + ros2_name) + match = False + break + output[direction].append({ + 'basic': False if '/' in ros1_type else True, + 'array': True if '[]' in ros1_type else False, + 'ros1': { + 'name': ros1_name, + 'type': ros1_type.rstrip('[]'), + 'cpptype': ros1_type.rstrip('[]').replace('/', '::') + }, + 'ros2': { + 'name': ros2_name, + 'type': ros2_type.rstrip('[]'), + 'cpptype': ros2_type.rstrip('[]').replace('/', '::msg::') + } + }) + if match: + actions.append({ + 'ros1_name': pair[0].message_name, + 'ros2_name': pair[1].message_name, + 'ros1_package': pair[0].package_name, + 'ros2_package': pair[1].package_name, + 'fields': output + }) + return actions + + +# __init__.py +def generate_actions(rospack=None, message_string_pairs=None): + # TODO: find all ROS1 and ROS2 actions + ros1_actions = get_ros1_actions(rospack) + ros2_pkgs, ros2_actions, mapping_rules = get_ros2_actions() + + actions = determine_common_actions( + ros1_actions, ros2_actions, mapping_rules, message_string_pairs=message_string_pairs) + return { + 'actions': actions, + 'ros2_package_names_actions': ros2_pkgs, + 'all_ros2_actions': ros2_actions, + } + + +# __init__.py +def generate_cpp(output_path, template_dir): + rospack = rospkg.RosPack() + ros1_time = Message("std_msgs", "Time", + "/opt/ros/melodic/share/std_msgs/msg") + ros1_dur = Message("std_msgs", "Duration", + "/opt/ros/melodic/share/std_msgs/msg") + ros2_time = Message("builtin_interfaces", "Time", + "/opt/ros/dashing") + ros2_dur = Message("builtin_interfaces", "Duration", + "/opt/ros/dashing") + + ros1_msgs = [ros1_time, ros1_dur] + ros2_msgs = [ros2_time, ros2_dur] + + mappings = [] + # add custom mapping for builtin_interfaces + for msg_name in ('Duration', 'Time'): + ros1_msg = [ + m for m in ros1_msgs + if m.package_name == 'std_msgs' and m.message_name == msg_name] + print(ros1_msg) + ros2_msg = [ + m for m in ros2_msgs + if m.package_name == 'builtin_interfaces' and m.message_name == msg_name] + print(ros2_msg) + if ros1_msg and ros2_msg: + # print(Mapping(ros1_msg[0], ros2_msg[0])) + mappings.append(Mapping(ros1_msg[0], ros2_msg[0])) + + message_string_pairs = { + ( + '%s/%s' % (m.ros1_msg.package_name, m.ros1_msg.message_name), + '%s/%s' % (m.ros2_msg.package_name, m.ros2_msg.message_name)) + for m in mappings} + print(message_string_pairs) + + data = generate_actions(rospack, message_string_pairs) + + unique_package_names = set( + data['ros2_package_names_actions']) + # skip builtin_interfaces since there is a custom implementation + unique_package_names -= {'builtin_interfaces'} + data['ros2_package_names'] = list(unique_package_names) + + for ros2_package_name in data['ros2_package_names']: + for interface_type, interfaces in zip( + ['action'], [data['all_ros2_actions']] + ): + for interface in interfaces: + if interface.package_name != ros2_package_name: + continue + data_idl_cpp = { + 'ros2_package_name': ros2_package_name, + 'interface_type': interface_type, + 'interface': interface, + 'mapped_actions': [ + s for s in data['actions'] + if s['ros2_package'] == ros2_package_name and + s['ros2_name'] == interface.message_name], + } + print(data_idl_cpp['mapped_actions']) + template_file = os.path.join( + template_dir, 'interface_factories.cpp.em') + output_file = os.path.join( + output_path, '%s__%s__%s__factories.cpp' % + (ros2_package_name, interface_type, interface.message_name)) + print(output_file) + output_file = "generated/" + output_file + if not os.path.exists(output_file): + open(output_file, 'w').close() + expand_template(template_file, data_idl_cpp, output_file) + + +if __name__ == "__main__": + generate_cpp("", "") diff --git a/scripts/init.py b/scripts/init.py new file mode 100644 index 0000000..13af46b --- /dev/null +++ b/scripts/init.py @@ -0,0 +1,962 @@ +# Copyright 2015-2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from collections import OrderedDict +import os +import re +import sys + +import ament_index_python +from catkin_pkg.package import parse_package +# ROS 1 imports +import genmsg +import genmsg.msg_loader + +import rosidl_adapter.parser +from rosidl_cmake import expand_template + +import yaml + +# import rospkg which is required by rosmsg +# and likely only available for Python 2 +try: + import rospkg +except ImportError: + from importlib.machinery import SourceFileLoader + import subprocess + for python_executable in ['python2', 'python2.7']: + try: + rospkg_path = subprocess.check_output( + [python_executable, '-c', 'import rospkg; print(rospkg.__file__)']) + except (subprocess.CalledProcessError, FileNotFoundError): + continue + rospkg_path = rospkg_path.decode().strip() + if rospkg_path.endswith('.pyc'): + rospkg_path = rospkg_path[:-1] + rospkg = SourceFileLoader('rospkg', rospkg_path).load_module() + if not rospkg: + raise + +# more ROS 1 imports +# since ROS 2 should be sourced after ROS 1 it will overlay +# e.g. the message packages, to prevent that from happening (and ROS 1 code to +# fail) ROS 1 paths are moved to the front of the search path +rpp = os.environ.get('ROS_PACKAGE_PATH', '').split(os.pathsep) +for package_path in reversed([p for p in rpp if p]): + if not package_path.endswith(os.sep + 'share'): + continue + ros1_basepath = os.path.dirname(package_path) + for sys_path in sys.path: + if sys_path.startswith(os.path.join(ros1_basepath, '')): + sys.path.remove(sys_path) + sys.path.insert(0, sys_path) +import rosmsg # noqa + + +def generate_cpp(output_path, template_dir): + rospack = rospkg.RosPack() + data = generate_messages(rospack) + message_string_pairs = { + ( + '%s/%s' % (m.ros1_msg.package_name, m.ros1_msg.message_name), + '%s/%s' % (m.ros2_msg.package_name, m.ros2_msg.message_name)) + for m in data['mappings']} + print(data['mappings']) + data.update( + generate_services(rospack, message_string_pairs=message_string_pairs)) + return + template_file = os.path.join(template_dir, 'get_mappings.cpp.em') + output_file = os.path.join(output_path, 'get_mappings.cpp') + data_for_template = { + 'mappings': data['mappings'], 'services': data['services']} + + #expand_template(template_file, data_for_template, output_file) + + unique_package_names = set( + data['ros2_package_names_msg'] + data['ros2_package_names_srv']) + # skip builtin_interfaces since there is a custom implementation + unique_package_names -= {'builtin_interfaces'} + data['ros2_package_names'] = list(unique_package_names) + + template_file = os.path.join(template_dir, 'get_factory.cpp.em') + output_file = os.path.join(output_path, 'get_factory.cpp') + #expand_template(template_file, data, output_file) + + for ros2_package_name in data['ros2_package_names']: + data_pkg_hpp = { + 'ros2_package_name': ros2_package_name, + # include directives and template types + 'mapped_ros1_msgs': [ + m.ros1_msg for m in data['mappings'] + if m.ros2_msg.package_name == ros2_package_name], + 'mapped_ros2_msgs': [ + m.ros2_msg for m in data['mappings'] + if m.ros2_msg.package_name == ros2_package_name], + # forward declaration of factory functions + 'ros2_msg_types': [ + m for m in data['all_ros2_msgs'] + if m.package_name == ros2_package_name], + 'ros2_srv_types': [ + s for s in data['all_ros2_srvs'] + if s.package_name == ros2_package_name], + # forward declaration of template specializations + 'mappings': [ + m for m in data['mappings'] + if m.ros2_msg.package_name == ros2_package_name], + } + template_file = os.path.join(template_dir, 'pkg_factories.hpp.em') + output_file = os.path.join( + output_path, '%s_factories.hpp' % ros2_package_name) + #expand_template(template_file, data_pkg_hpp, output_file) + + data_pkg_cpp = { + 'ros2_package_name': ros2_package_name, + # call interface specific factory functions + 'ros2_msg_types': data_pkg_hpp['ros2_msg_types'], + 'ros2_srv_types': data_pkg_hpp['ros2_srv_types'], + } + template_file = os.path.join(template_dir, 'pkg_factories.cpp.em') + output_file = os.path.join( + output_path, '%s_factories.cpp' % ros2_package_name) + #expand_template(template_file, data_pkg_cpp, output_file) + + interface_type, interfaces = zip( + ['msg', 'srv'], [data['all_ros2_msgs'], data['all_ros2_srvs']] + ) + # print(interface_type[1][1]) + + for interface_type, interfaces in zip( + ['msg', 'srv'], [data['all_ros2_msgs'], data['all_ros2_srvs']] + ): + for interface in interfaces: + # print(interface) + if interface.package_name != ros2_package_name: + continue + data_idl_cpp = { + 'ros2_package_name': ros2_package_name, + 'interface_type': interface_type, + 'interface': interface, + 'mapped_msgs': [ + m for m in data['mappings'] + if m.ros2_msg.package_name == ros2_package_name and + m.ros2_msg.message_name == interface.message_name], + 'mapped_services': [ + s for s in data['services'] + if s['ros2_package'] == ros2_package_name and + s['ros2_name'] == interface.message_name], + } + template_file = os.path.join( + template_dir, 'interface_factories.cpp.em') + output_file = os.path.join( + output_path, '%s__%s__%s__factories.cpp' % + (ros2_package_name, interface_type, interface.message_name)) + expand_template(template_file, data_idl_cpp, output_file) + + +def generate_messages(rospack=None): + ros1_msgs = get_ros1_messages(rospack=rospack) + ros2_package_names, ros2_msgs, mapping_rules = get_ros2_messages() + + package_pairs = determine_package_pairs( + ros1_msgs, ros2_msgs, mapping_rules) + message_pairs = determine_message_pairs( + ros1_msgs, ros2_msgs, package_pairs, mapping_rules) + + mappings = [] + # add custom mapping for builtin_interfaces + for msg_name in ('Duration', 'Time'): + ros1_msg = [ + m for m in ros1_msgs + if m.package_name == 'std_msgs' and m.message_name == msg_name] + print(ros1_msg) + ros2_msg = [ + m for m in ros2_msgs + if m.package_name == 'builtin_interfaces' and m.message_name == msg_name] + print(ros2_msg) + if ros1_msg and ros2_msg: + #print(Mapping(ros1_msg[0], ros2_msg[0])) + mappings.append(Mapping(ros1_msg[0], ros2_msg[0])) + + print(mappings) + + msg_idx = MessageIndex() + for ros1_msg, ros2_msg in message_pairs: + 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) + + # order mappings topologically to allow template specialization + ordered_mappings = [] + while mappings: + # pick first mapping without unsatisfied dependencies + for m in mappings: + if not m.depends_on_ros2_messages: + break + else: + break + # move mapping to ordered list + mappings.remove(m) + ordered_mappings.append(m) + ros2_msg = m.ros2_msg + # update unsatisfied dependencies of remaining mappings + for m in mappings: + if ros2_msg in m.depends_on_ros2_messages: + m.depends_on_ros2_messages.remove(ros2_msg) + + if mappings: + print('%d mappings can not be generated due to missing dependencies:' % len(mappings), + file=sys.stderr) + for m in mappings: + print('- %s <-> %s:' % + ('%s/%s' % (m.ros1_msg.package_name, m.ros1_msg.message_name), + '%s/%s' % (m.ros2_msg.package_name, m.ros2_msg.message_name)), file=sys.stderr) + for d in m.depends_on_ros2_messages: + print(' -', '%s/%s' % + (d.package_name, d.message_name), file=sys.stderr) + print(file=sys.stderr) + + return { + 'ros1_msgs': [m.ros1_msg for m in ordered_mappings], + 'ros2_msgs': [m.ros2_msg for m in ordered_mappings], + 'mappings': ordered_mappings, + 'ros2_package_names_msg': ros2_package_names, + 'all_ros2_msgs': ros2_msgs, + } + + +def generate_services(rospack=None, message_string_pairs=None): + ros1_srvs = get_ros1_services(rospack=rospack) + ros2_pkgs, ros2_srvs, mapping_rules = get_ros2_services() + services = determine_common_services( + ros1_srvs, ros2_srvs, mapping_rules, + message_string_pairs=message_string_pairs) + return { + 'services': services, + 'ros2_package_names_srv': ros2_pkgs, + 'all_ros2_srvs': ros2_srvs, + } + + +def get_ros1_messages(rospack=None): + if not rospack: + rospack = rospkg.RosPack() + msgs = [] + pkgs = sorted(x for x in rosmsg.iterate_packages(rospack, rosmsg.MODE_MSG)) + for package_name, path in pkgs: + for message_name in rosmsg._list_types(path, 'msg', rosmsg.MODE_MSG): + msgs.append(Message(package_name, message_name, path)) + return msgs + + +def get_ros2_messages(): + pkgs = [] + msgs = [] + rules = [] + # get messages from packages + resource_type = 'rosidl_interfaces' + resources = ament_index_python.get_resources(resource_type) + for package_name, prefix_path in resources.items(): + pkgs.append(package_name) + resource, _ = ament_index_python.get_resource( + resource_type, package_name) + interfaces = resource.splitlines() + message_names = { + i[4:-4] + for i in interfaces + if i.startswith('msg/') and i[-4:] in ('.idl', '.msg')} + + for message_name in sorted(message_names): + msgs.append(Message(package_name, message_name, prefix_path)) + # check package manifest for mapping rules + package_path = os.path.join(prefix_path, 'share', package_name) + pkg = parse_package(package_path) + for export in pkg.exports: + if export.tagname != 'ros1_bridge': + continue + if 'mapping_rules' not in export.attributes: + continue + rule_file = os.path.join( + package_path, export.attributes['mapping_rules']) + with open(rule_file, 'r') as h: + content = yaml.safe_load(h) + if not isinstance(content, list): + print( + "The content of the mapping rules in '%s' is not a list" % rule_file, + file=sys.stderr) + continue + for data in content: + if all(n not in data for n in ('ros1_service_name', 'ros2_service_name')): + try: + rules.append(MessageMappingRule(data, package_name)) + except Exception as e: + print('%s' % str(e), file=sys.stderr) + return pkgs, msgs, rules + + +def get_ros1_services(rospack=None): + if not rospack: + rospack = rospkg.RosPack() + srvs = [] + pkgs = sorted(x for x in rosmsg.iterate_packages(rospack, rosmsg.MODE_SRV)) + for package_name, path in pkgs: + for message_name in rosmsg._list_types(path, 'srv', rosmsg.MODE_SRV): + srvs.append(Message(package_name, message_name, path)) + return srvs + + +def get_ros2_services(): + pkgs = [] + srvs = [] + rules = [] + resource_type = 'rosidl_interfaces' + resources = ament_index_python.get_resources(resource_type) + for package_name, prefix_path in resources.items(): + pkgs.append(package_name) + resource, _ = ament_index_python.get_resource( + resource_type, package_name) + interfaces = resource.splitlines() + service_names = { + i[4:-4] + for i in interfaces + if i.startswith('srv/') and i[-4:] in ('.idl', '.srv')} + + for service_name in sorted(service_names): + srvs.append(Message(package_name, service_name, prefix_path)) + # check package manifest for mapping rules + package_path = os.path.join(prefix_path, 'share', package_name) + pkg = parse_package(package_path) + for export in pkg.exports: + if export.tagname != 'ros1_bridge': + continue + if 'mapping_rules' not in export.attributes: + continue + rule_file = os.path.join( + package_path, export.attributes['mapping_rules']) + with open(rule_file, 'r') as h: + content = yaml.safe_load(h) + if not isinstance(content, list): + print( + "The content of the mapping rules in '%s' is not a list" % rule_file, + file=sys.stderr) + continue + for data in content: + if all(n not in data for n in ('ros1_message_name', 'ros2_message_name')): + try: + rules.append(ServiceMappingRule(data, package_name)) + except Exception as e: + print('%s' % str(e), file=sys.stderr) + return pkgs, srvs, rules + + +class Message: + __slots__ = [ + 'package_name', + 'message_name', + 'prefix_path' + ] + + def __init__(self, package_name, message_name, prefix_path=None): + self.package_name = package_name + self.message_name = message_name + self.prefix_path = prefix_path + + def __eq__(self, other): + return self.package_name == other.package_name and \ + self.message_name == other.message_name + + def __hash__(self): + return hash('%s/%s' % (self.package_name, self.message_name)) + + def __str__(self): + return self.prefix_path + ':' + self.package_name + ':' + self.message_name + + def __repr__(self): + return self.__str__() + + +class MappingRule: + __slots__ = [ + 'ros1_package_name', + 'ros2_package_name', + 'package_mapping' + ] + + def __init__(self, data, expected_package_name): + if all(n in data for n in ('ros1_package_name', 'ros2_package_name')): + if data['ros2_package_name'] != expected_package_name: + raise Exception( + ('Ignoring rule which affects a different ROS 2 package (%s) ' + 'then the one it is defined in (%s)') % + (data['ros2_package_name'], expected_package_name)) + self.ros1_package_name = data['ros1_package_name'] + self.ros2_package_name = data['ros2_package_name'] + self.package_mapping = (len(data) == 2) + else: + raise Exception( + 'Ignoring a rule without a ros1_package_name and/or ros2_package_name') + + def is_package_mapping(self): + return self.package_mapping + + def __repr__(self): + return self.__str__() + + +class MessageMappingRule(MappingRule): + __slots__ = [ + 'ros1_message_name', + 'ros2_message_name', + 'fields_1_to_2', + ] + + def __init__(self, data, expected_package_name): + super().__init__(data, expected_package_name) + self.ros1_message_name = None + self.ros2_message_name = None + self.fields_1_to_2 = None + if all(n in data for n in ('ros1_message_name', 'ros2_message_name')): + self.ros1_message_name = data['ros1_message_name'] + self.ros2_message_name = data['ros2_message_name'] + if 'fields_1_to_2' in data: + self.fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['fields_1_to_2'].items(): + self.fields_1_to_2[ros1_field_name] = ros2_field_name + elif len(data) > 4: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + elif len(data) > 2: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + + def is_message_mapping(self): + return self.ros1_message_name is not None + + def is_field_mapping(self): + return self.fields_1_to_2 is not None + + def __str__(self): + return 'MessageMappingRule(%s <-> %s)' % (self.ros1_package_name, self.ros2_package_name) + + +class ServiceMappingRule(MappingRule): + __slots__ = [ + 'ros1_service_name', + 'ros2_service_name', + 'request_fields_1_to_2', + 'response_fields_1_to_2', + ] + + def __init__(self, data, expected_package_name): + super().__init__(data, expected_package_name) + self.ros1_service_name = None + self.ros2_service_name = None + self.request_fields_1_to_2 = None + self.response_fields_1_to_2 = None + if all(n in data for n in ('ros1_service_name', 'ros2_service_name')): + self.ros1_service_name = data['ros1_service_name'] + self.ros2_service_name = data['ros2_service_name'] + expected_keys = 4 + if 'request_fields_1_to_2' in data: + self.request_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['request_fields_1_to_2'].items(): + self.request_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + if 'response_fields_1_to_2' in data: + self.response_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['response_fields_1_to_2'].items(): + self.response_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + elif len(data) > expected_keys: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + elif len(data) > 2: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + + def __str__(self): + return 'ServiceMappingRule(%s <-> %s)' % (self.ros1_package_name, self.ros2_package_name) + + +def determine_package_pairs(ros1_msgs, ros2_msgs, mapping_rules): + pairs = [] + # determine package names considered equal between ROS 1 and ROS 2 + ros1_suffix = '_msgs' + ros2_suffixes = ['_msgs', '_interfaces'] + ros1_package_names = {m.package_name for m in ros1_msgs} + ros2_package_names = {m.package_name for m in ros2_msgs} + for ros1_package_name in ros1_package_names: + if not ros1_package_name.endswith(ros1_suffix): + continue + ros1_package_basename = ros1_package_name[:-len(ros1_suffix)] + + for ros2_package_name in ros2_package_names: + for ros2_suffix in ros2_suffixes: + if ros2_package_name.endswith(ros2_suffix): + break + else: + continue + ros2_package_basename = ros2_package_name[:-len(ros2_suffix)] + if ros1_package_basename != ros2_package_basename: + continue + pairs.append((ros1_package_name, ros2_package_name)) + + # add manual package mapping rules + for rule in mapping_rules: + if not rule.is_package_mapping(): + continue + if rule.ros1_package_name not in ros1_package_names: + continue + if rule.ros2_package_name not in ros2_package_names: + continue + pair = (rule.ros1_package_name, rule.ros2_package_name) + if pair not in pairs: + pairs.append(pair) + + return pairs + + +def determine_message_pairs(ros1_msgs, ros2_msgs, package_pairs, mapping_rules): + pairs = [] + # determine message names considered equal between ROS 1 and ROS 2 + for ros1_msg in ros1_msgs: + for ros2_msg in ros2_msgs: + package_pair = (ros1_msg.package_name, ros2_msg.package_name) + if package_pair not in package_pairs: + continue + if ros1_msg.message_name != ros2_msg.message_name: + continue + pairs.append((ros1_msg, ros2_msg)) + + # add manual message mapping rules + for rule in mapping_rules: + if not rule.is_message_mapping(): + continue + for ros1_msg in ros1_msgs: + if rule.ros1_package_name == ros1_msg.package_name and \ + rule.ros1_message_name == ros1_msg.message_name: + break + else: + # skip unknown messages + continue + for ros2_msg in ros2_msgs: + if rule.ros2_package_name == ros2_msg.package_name and \ + rule.ros2_message_name == ros2_msg.message_name: + break + else: + # skip unknown messages + continue + + pair = (ros1_msg, ros2_msg) + if pair not in pairs: + pairs.append(pair) + + return pairs + + +def determine_common_services( + ros1_srvs, ros2_srvs, mapping_rules, message_string_pairs=None +): + if message_string_pairs is None: + message_string_pairs = set() + + pairs = [] + services = [] + for ros1_srv in ros1_srvs: + for ros2_srv in ros2_srvs: + if ros1_srv.package_name == ros2_srv.package_name: + if ros1_srv.message_name == ros2_srv.message_name: + pairs.append((ros1_srv, ros2_srv)) + + for rule in mapping_rules: + for ros1_srv in ros1_srvs: + for ros2_srv in ros2_srvs: + if rule.ros1_package_name == ros1_srv.package_name and \ + rule.ros2_package_name == ros2_srv.package_name: + if rule.ros1_service_name is None and rule.ros2_service_name is None: + if ros1_srv.message_name == ros2_srv.message_name: + pairs.append((ros1_srv, ros2_srv)) + else: + if ( + rule.ros1_service_name == ros1_srv.message_name and + rule.ros2_service_name == ros2_srv.message_name + ): + pairs.append((ros1_srv, ros2_srv)) + + for pair in pairs: + ros1_spec = load_ros1_service(pair[0]) + ros2_spec = load_ros2_service(pair[1]) + ros1_fields = { + 'request': ros1_spec.request.fields(), + 'response': ros1_spec.response.fields() + } + ros2_fields = { + 'request': ros2_spec.request.fields, + 'response': ros2_spec.response.fields + } + output = { + 'request': [], + 'response': [] + } + match = True + for direction in ['request', 'response']: + if len(ros1_fields[direction]) != len(ros2_fields[direction]): + match = False + break + for i, ros1_field in enumerate(ros1_fields[direction]): + ros1_type = ros1_field[0] + ros2_type = str(ros2_fields[direction][i].type) + ros1_name = ros1_field[1] + ros2_name = ros2_fields[direction][i].name + if ros1_type != ros2_type or ros1_name != ros2_name: + # if the message types have a custom mapping their names + # might not be equal, therefore check the message pairs + if (ros1_type, ros2_type) not in message_string_pairs: + print(ros1_type + " " + ros2_type + + " : " + ros1_name + " " + ros2_name) + match = False + break + output[direction].append({ + 'basic': False if '/' in ros1_type else True, + 'array': True if '[]' in ros1_type else False, + 'ros1': { + 'name': ros1_name, + 'type': ros1_type.rstrip('[]'), + 'cpptype': ros1_type.rstrip('[]').replace('/', '::') + }, + 'ros2': { + 'name': ros2_name, + 'type': ros2_type.rstrip('[]'), + 'cpptype': ros2_type.rstrip('[]').replace('/', '::msg::') + } + }) + if match: + services.append({ + 'ros1_name': pair[0].message_name, + 'ros2_name': pair[1].message_name, + 'ros1_package': pair[0].package_name, + 'ros2_package': pair[1].package_name, + 'fields': output + }) + return services + + +def update_ros1_field_information(ros1_field, package_name): + parts = ros1_field.base_type.split('/') + assert len(parts) in [1, 2] + if len(parts) == 1: + ros1_field.pkg_name = package_name + ros1_field.msg_name = parts[0] + else: + ros1_field.pkg_name = parts[0] + ros1_field.msg_name = parts[1] + + +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 + ros2_spec = load_ros2_message(ros2_msg) + if not ros2_spec: + return None + + mapping = Mapping(ros1_msg, ros2_msg) + + # check for manual field mapping rules first + for rule in mapping_rules: + if not rule.is_field_mapping(): + continue + if rule.ros1_package_name != ros1_msg.package_name or \ + rule.ros1_message_name != ros1_msg.message_name: + continue + if rule.ros2_package_name != ros2_msg.package_name or \ + rule.ros2_message_name != ros2_msg.message_name: + continue + + for ros1_field_selection, ros2_field_selection in rule.fields_1_to_2.items(): + try: + 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_selection + + "in the ROS 1 message '%s/%s'" % + (rule.ros1_package_name, rule.ros1_message_name), + file=sys.stderr) + continue + try: + 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_selection + + "in the ROS 2 message '%s/%s'" % + (rule.ros2_package_name, rule.ros2_message_name), + file=sys.stderr) + continue + mapping.add_field_pair(ros1_selected_fields, ros2_selected_fields) + return mapping + + # apply name based mapping of fields + ros1_field_missing_in_ros2 = False + + for ros1_field in ros1_spec.parsed_fields(): + for ros2_field in ros2_spec.fields: + if ros1_field.name.lower() == ros2_field.name: + # get package name and message name from ROS 1 field type + if ros2_field.type.pkg_name: + update_ros1_field_information( + ros1_field, ros1_msg.package_name) + mapping.add_field_pair(ros1_field, ros2_field) + break + else: + # this allows fields to exist in ROS 1 but not in ROS 2 + ros1_field_missing_in_ros2 = True + + if ros1_field_missing_in_ros2: + # if some fields exist in ROS 1 but not in ROS 2 + # check that no fields exist in ROS 2 but not in ROS 1 + # since then it might be the case that those have been renamed and should be mapped + for ros2_field in ros2_spec.fields: + for ros1_field in ros1_spec.parsed_fields(): + if ros1_field.name.lower() == ros2_field.name: + break + else: + # if fields from both sides are not mappable the whole message is not mappable + return None + + return mapping + + +def load_ros1_message(ros1_msg): + msg_context = genmsg.MsgContext.create_default() + message_path = os.path.join( + ros1_msg.prefix_path, ros1_msg.message_name + '.msg') + try: + spec = genmsg.msg_loader.load_msg_from_file( + msg_context, message_path, '%s/%s' % (ros1_msg.package_name, ros1_msg.message_name)) + except genmsg.InvalidMsgSpec: + return None + return spec + + +def load_ros1_service(ros1_srv): + srv_context = genmsg.MsgContext.create_default() + srv_path = os.path.join(ros1_srv.prefix_path, + ros1_srv.message_name + '.srv') + srv_name = '%s/%s' % (ros1_srv.package_name, ros1_srv.message_name) + try: + spec = genmsg.msg_loader.load_srv_from_file( + srv_context, srv_path, srv_name) + except genmsg.InvalidMsgSpec: + return None + return spec + + +def load_ros2_message(ros2_msg): + message_path = os.path.join( + ros2_msg.prefix_path, 'share', ros2_msg.package_name, 'msg', + ros2_msg.message_name + '.msg') + try: + spec = rosidl_adapter.parser.parse_message_file( + ros2_msg.package_name, message_path) + except rosidl_adapter.parser.InvalidSpecification: + return None + return spec + + +def load_ros2_service(ros2_srv): + srv_path = os.path.join( + ros2_srv.prefix_path, 'share', ros2_srv.package_name, 'srv', + ros2_srv.message_name + '.srv') + try: + spec = rosidl_adapter.parser.parse_service_file( + ros2_srv.package_name, srv_path) + except rosidl_adapter.parser.InvalidSpecification: + return None + return spec + + +# make field types hashable +def FieldHash(self): + return self.name.__hash__() + + +genmsg.msgs.Field.__hash__ = FieldHash +rosidl_adapter.parser.Field.__hash__ = FieldHash + + +class Mapping: + __slots__ = [ + 'ros1_msg', + 'ros2_msg', + 'fields_1_to_2', + 'fields_2_to_1', + 'depends_on_ros2_messages' + ] + + def __init__(self, ros1_msg, ros2_msg): + self.ros1_msg = ros1_msg + self.ros2_msg = ros2_msg + self.fields_1_to_2 = OrderedDict() + self.fields_2_to_1 = OrderedDict() + self.depends_on_ros2_messages = set() + + 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 __repr__(self): + # + " " + str(self.fields_1_to_2) + " " + str(self.fields_2_to_1) + " " + str(self.depends_on_ros2_messages) + return self.ros1_msg.message_name + ", " + self.ros2_msg.message_name + + +def camel_case_to_lower_case_underscore(value): + # insert an underscore before any upper case letter + # which is not followed by another upper case letter + value = re.sub('(.)([A-Z][a-z]+)', '\\1_\\2', value) + # insert an underscore before any upper case letter + # 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)] + + +if __name__ == "__main__": + generate_cpp(".", ".")