From 688768b5e3287bf8ee56a3389cf6cd485e653eb9 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sun, 3 May 2020 23:41:46 +0200 Subject: [PATCH 01/43] added action templates Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 19 ++++++ resource/get_mappings.cpp.em | 14 +++++ resource/interface_factories.cpp.em | 98 +++++++++++++++++++++++++++++ resource/pkg_factories.cpp.em | 19 ++++++ resource/pkg_factories.hpp.em | 9 +++ 5 files changed, 159 insertions(+) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 722d1852..3e8f0e48 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -69,4 +69,23 @@ std::unique_ptr get_service_factory(const std::string & return factory; } +std::unique_ptr get_action_factory(const std::string & ros_id, const std::string & package_name, const std::string & action_name) +{ +@[if not ros2_package_names]@ + (void)ros_id; + (void)package_name; + (void)action_name; +@[else]@ + std::unique_ptr factory; +@[end if]@ +@[for ros2_package_name in sorted(ros2_package_names)]@ + factory = get_action_factory_@(ros2_package_name)(ros_id, package_name, action_name); + if (factory) { + return factory; + } +@[end for]@ + // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); + return factory; +} + } // namespace ros1_bridge diff --git a/resource/get_mappings.cpp.em b/resource/get_mappings.cpp.em index 386f9cc7..e58a4cf3 100644 --- a/resource/get_mappings.cpp.em +++ b/resource/get_mappings.cpp.em @@ -89,4 +89,18 @@ get_all_service_mappings_2to1() return mappings; } +std::multimap +get_all_action_mappings_2to1() +{ + static std::multimap mappings = { +@[for a in actions]@ + { + "@(a['ros2_package'])/action/@(a['ros2_name'])", // ROS 2 + "@(a['ros1_package'])/@(a['ros1_name'])" // ROS 1 + }, +@[end for]@ + }; + return mappings; +} + } // namespace ros1_bridge diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index f3e536e4..66df243e 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -46,6 +46,16 @@ from rosidl_parser.definition import UnboundedString #include <@(service["ros2_package"])/srv/@(camel_case_to_lower_case_underscore(service["ros2_name"])).hpp> @[end for]@ +// include ROS 1 actions +@[for action in mapped_actions]@ +#include <@(action["ros1_package"])/@(action["ros1_name"]).h> +@[end for]@ + +// 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]@ + namespace ros1_bridge { @@ -104,6 +114,45 @@ get_service_factory_@(ros2_package_name)__@(interface_type)__@(interface.message return nullptr; } @ + +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"])" + ) { + return std::unique_ptr(new ActionFactory_2_1< + @(action["ros1_package"])::@(action["ros1_name"]), + @(action["ros2_package"])::srv::@(action["ros2_name"]) + >); + } + else if + ( + ros_id == "ros2" && + package_name == "@(action["ros2_package"])" && + action_name == "action/@(action["ros2_name"])" + ) + ) { + return std::unique_ptr(new ActionFactory_1_2< + @(action["ros1_package"])::@(action["ros1_name"]), + @(action["ros2_package"])::srv::@(action["ros2_name"]) + >); + } +@[end for]@ + return nullptr; +} +@ +} +@ // conversion functions for available interfaces @[for m in mapped_msgs]@ @@ -332,6 +381,55 @@ void ServiceFactory< @[ end for]@ @[ end for]@ +@[end for]@ + +@[for action in mapped_actions]@ +@[ for frm, to in [("1", "2"), ("2", "1")]]@ +@[ for type in ["Goal", "Result", "Feedback"]]@ +template <> +void ActionFactory_@(frm)_@(to)< +@(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]@ } // namespace ros1_bridge diff --git a/resource/pkg_factories.cpp.em b/resource/pkg_factories.cpp.em index c8d2ab21..52266f87 100644 --- a/resource/pkg_factories.cpp.em +++ b/resource/pkg_factories.cpp.em @@ -58,4 +58,23 @@ get_service_factory_@(ros2_package_name)(const std::string & ros_id, const std:: @[end for]@ return nullptr; } + +std::unique_ptr +get_action_factory_@(ros2_package_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name) +{ +@[if not ros2_action_types]@ + (void)ros_id; + (void)package_name; + (void)action_name; +@[else]@ + std::unique_ptr factory; +@[end if]@ +@[for a in ros2_action_types]@ + factory = get_action_factory_@(ros2_package_name)__action__@(a.message_name)(ros_id, package_name, action_name); + if (factory) { + return factory; + } +@[end for]@ + return nullptr; +} } // namespace ros1_bridge diff --git a/resource/pkg_factories.hpp.em b/resource/pkg_factories.hpp.em index 670d4fd2..af325bd1 100644 --- a/resource/pkg_factories.hpp.em +++ b/resource/pkg_factories.hpp.em @@ -28,6 +28,7 @@ from ros1_bridge import camel_case_to_lower_case_underscore #include #include +#include // include ROS 1 messages @[for ros1_msg in mapped_ros1_msgs]@ @@ -58,6 +59,14 @@ std::unique_ptr get_service_factory_@(ros2_package_name)__srv__@(s.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & service_name); @[end for]@ +std::unique_ptr +get_action_factory_@(ros2_package_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name); +@[for a in ros2_action_types]@ + +std::unique_ptr +get_action_factory_@(ros2_package_name)__action__@(a.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name); +@[end for]@ + // conversion functions for available interfaces @[for m in mappings]@ From 5bbc928d3af1925c7cef5983c448b7d0671716b6 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sun, 3 May 2020 23:43:24 +0200 Subject: [PATCH 02/43] generate action interface mappings Signed-off-by: Harsh Deshpande --- ros1_bridge/__init__.py | 430 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 401 insertions(+), 29 deletions(-) diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 7cc78d14..4c83efc9 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -20,8 +20,14 @@ import ament_index_python from catkin_pkg.package import parse_package # ROS 1 imports +try: + from cStringIO import StringIO # Python 2.x +except ImportError: + from io import StringIO # Python 3.x + import genmsg import genmsg.msg_loader +from genmsg.base import COMMENTCHAR, IODELIM import rosidl_adapter.parser from rosidl_cmake import expand_template @@ -75,14 +81,19 @@ def generate_cpp(output_path, template_dir): for m in data['mappings']} data.update( generate_services(rospack, message_string_pairs=message_string_pairs)) + data.update(generate_actions( + rospack, message_string_pairs=message_string_pairs)) + + print(data['actions']) 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']} + 'mappings': data['mappings'], 'services': data['services'], 'actions': data['actions']} expand_template(template_file, data_for_template, output_file) - unique_package_names = set(data['ros2_package_names_msg'] + data['ros2_package_names_srv']) + 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) @@ -108,6 +119,9 @@ def generate_cpp(output_path, template_dir): 'ros2_srv_types': [ s for s in data['all_ros2_srvs'] if s.package_name == ros2_package_name], + 'ros2_action_types': [ + s for s in data['all_ros2_actions'] + if s.package_name == ros2_package_name], # forward declaration of template specializations 'mappings': [ m for m in data['mappings'] @@ -123,6 +137,7 @@ def generate_cpp(output_path, template_dir): # call interface specific factory functions 'ros2_msg_types': data_pkg_hpp['ros2_msg_types'], 'ros2_srv_types': data_pkg_hpp['ros2_srv_types'], + 'ros2_action_types': data_pkg_hpp['ros2_action_types'], } template_file = os.path.join(template_dir, 'pkg_factories.cpp.em') output_file = os.path.join( @@ -130,7 +145,8 @@ def generate_cpp(output_path, template_dir): expand_template(template_file, data_pkg_cpp, output_file) for interface_type, interfaces in zip( - ['msg', 'srv'], [data['all_ros2_msgs'], data['all_ros2_srvs']] + ['msg', 'srv', 'action'], [data['all_ros2_msgs'], + data['all_ros2_srvs'], data['all_ros2_actions']] ): for interface in interfaces: if interface.package_name != ros2_package_name: @@ -139,20 +155,21 @@ def generate_cpp(output_path, template_dir): 'ros2_package_name': ros2_package_name, 'interface_type': interface_type, 'interface': interface, - 'mapped_msgs': [], - 'mapped_services': [], - } - if interface_type == 'msg': - data_idl_cpp['mapped_msgs'] += [ + '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] - if interface_type == 'srv': - data_idl_cpp['mapped_services'] += [ + 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') + s['ros2_name'] == interface.message_name], + 'mapped_actions': [ + s for s in data['actions'] + 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)) @@ -163,8 +180,10 @@ 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) + 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 @@ -184,7 +203,8 @@ def generate_messages(rospack=None): 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) + mapping = determine_field_mapping( + ros1_msg, ros2_msg, mapping_rules, msg_idx) if mapping: mappings.append(mapping) @@ -214,7 +234,8 @@ def generate_messages(rospack=None): ('%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(' -', '%s/%s' % + (d.package_name, d.message_name), file=sys.stderr) print(file=sys.stderr) return { @@ -239,6 +260,19 @@ def generate_services(rospack=None, message_string_pairs=None): } +def generate_actions(rospack=None, message_string_pairs=None): + 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, + } + + def get_ros1_messages(rospack=None): if not rospack: rospack = rospkg.RosPack() @@ -284,7 +318,8 @@ def get_ros2_messages(): continue if 'mapping_rules' not in export.attributes: continue - rule_file = os.path.join(package_path, export.attributes['mapping_rules']) + 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): @@ -346,7 +381,8 @@ def get_ros2_services(): continue if 'mapping_rules' not in export.attributes: continue - rule_file = os.path.join(package_path, export.attributes['mapping_rules']) + 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): @@ -363,6 +399,72 @@ def get_ros2_services(): return pkgs, srvs, rules +# 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 + + +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 Message: __slots__ = [ 'package_name', @@ -415,7 +517,8 @@ def __init__(self, data, expected_package_name): len(data) == (2 + int('enable_foreign_mappings' in data)) ) else: - raise Exception('Ignoring a rule without a ros1_package_name and/or ros2_package_name') + raise Exception( + 'Ignoring a rule without a ros1_package_name and/or ros2_package_name') def is_package_mapping(self): return self.package_mapping @@ -506,6 +609,52 @@ def __str__(self): return 'ServiceMappingRule(%s <-> %s)' % (self.ros1_package_name, self.ros2_package_name) +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_package_pairs(ros1_msgs, ros2_msgs, mapping_rules): pairs = [] # determine package names considered equal between ROS 1 and ROS 2 @@ -670,6 +819,96 @@ def determine_common_services( return services +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_action_name is None and rule.ros2_action_name is None: + if ros1_action.message_name == ros2_action.message_name: + pairs.append((ros1_action, ros2_action)) + else: + if ( + rule.ros1_action_name == ros1_action.message_name and + rule.ros2_action_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"): + 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 + + def update_ros1_field_information(ros1_field, package_name): parts = ros1_field.base_type.split('/') assert len(parts) in [1, 2] @@ -704,11 +943,14 @@ def consume_field(field): 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] + 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] + 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) @@ -766,7 +1008,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): 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) + 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 + @@ -776,7 +1019,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): continue try: ros2_selected_fields = \ - get_ros2_selected_fields(ros2_field_selection, ros2_spec, msg_idx) + 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 + @@ -826,7 +1070,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): 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') + 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)) @@ -837,10 +1082,122 @@ def load_ros1_message(ros1_msg): 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_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) + spec = genmsg.msg_loader.load_srv_from_file( + srv_context, srv_path, srv_name) + except genmsg.InvalidMsgSpec: + return None + return spec + + +# 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 + + +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 @@ -893,13 +1250,28 @@ def load_ros2_service(ros2_srv): 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) + spec = rosidl_adapter.parser.parse_service_file( + ros2_srv.package_name, srv_path) except rosidl_adapter.parser.InvalidSpecification: return None return spec +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 + # make field types hashable + + def FieldHash(self): return self.name.__hash__() From d4fc03efebbbcb22b34c18298328500c8dc0b4dc Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sun, 3 May 2020 23:44:52 +0200 Subject: [PATCH 03/43] added action factories Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 496 ++++++++++++++++++++++ include/ros1_bridge/bridge.hpp | 3 + include/ros1_bridge/factory_interface.hpp | 8 + 3 files changed, 507 insertions(+) create mode 100644 include/ros1_bridge/action_factory.hpp diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp new file mode 100644 index 00000000..bb77603c --- /dev/null +++ b/include/ros1_bridge/action_factory.hpp @@ -0,0 +1,496 @@ +// 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. + +#ifndef ACTION_BRIDGE__ACTION_BRIDGE_HPP_ +#define ACTION_BRIDGE__ACTION_BRIDGE_HPP_ + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunused-parameter" +#endif +#include +#include +#include +#include //Need this for the goal state. Need a better solution +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +// include ROS 2 +#include "rclcpp/rclcpp.hpp" +#include + +#include "factory_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ros1_bridge +{ + +template +class ActionFactory_1_2 : public ActionFactoryInterface +{ +public: + using ROS1Server = typename actionlib::ActionServer; + using ROS1GoalHandle = typename actionlib::ActionServer::GoalHandle; + using ROS1Goal = typename actionlib::ActionServer::Goal; + using ROS1Feedback = typename actionlib::ActionServer::Feedback; + using ROS1Result = typename actionlib::ActionServer::Result; + using ROS2Goal = typename ROS2_T::Goal; + using ROS2Feedback = typename ROS2_T::Feedback; + using ROS2Result = typename ROS2_T::Result; + using ROS2GoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; + using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; + using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + // ActionBridge_1_2( + // ros::NodeHandle ros1_node, + // rclcpp::Node::SharedPtr ros2_node, + // const std::string action_name) + // : ros1_node_(ros1_node), ros2_node_(ros2_node), + // server_(ros1_node, action_name, + // std::bind(&ActionBridge_1_2::goal_cb, this, std::placeholders::_1), + // std::bind(&ActionBridge_1_2::cancel_cb, this, std::placeholders::_1), + // false) + // { + // server_.start(); + // client_ = rclcpp_action::create_client(ros2_node, action_name); + // } + + virtual void create_server_client(ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) + { + this->ros1_node_ = ros1_node; + this->ros2_node_ = ros2_node; + + server_ = std::make_shared(ros1_node, action_name, + std::bind(&ActionFactory_1_2::goal_cb, this, std::placeholders::_1), + std::bind(&ActionFactory_1_2::cancel_cb, this, std::placeholders::_1), + false); + server_->start(); + client_ = rclcpp_action::create_client(ros2_node, action_name); + } + + void cancel_cb(ROS1GoalHandle gh1) + { + // try to find goal and cancel it + std::lock_guard lock(mutex_); + auto it = goals_.find(gh1.getGoalID().id); + if (it != goals_.end()) + { + std::thread([handler = it->second]() mutable { + handler->cancel(); + }).detach(); + } + } + + void goal_cb(ROS1GoalHandle gh1) + { + const std::string goal_id = gh1.getGoalID().id; + + // create a new handler for the goal + std::shared_ptr handler; + handler.reset(new GoalHandler(gh1, client_)); + std::lock_guard lock(mutex_); + goals_.insert(std::make_pair(goal_id, handler)); + + RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + std::thread([handler, goal_id, this]() mutable { + // execute the goal remotely + handler->handle(); + + // clean-up + std::lock_guard lock(mutex_); + goals_.erase(goal_id); + }).detach(); + } + + // static int main(const std::string &action_name, int argc, char *argv[]) + // { + // std::string node_name = "action_bridge_" + action_name; + // std::replace(node_name.begin(), node_name.end(), '/', '_'); + // // ROS 1 node + // ros::init(argc, argv, node_name); + // ros::NodeHandle ros1_node; + + // // ROS 2 node + // rclcpp::init(argc, argv); + // auto ros2_node = rclcpp::Node::make_shared(node_name); + + // ActionBridge_1_2 action_bridge(ros1_node, ros2_node, action_name); + + // // // ROS 1 asynchronous spinner + // ros::AsyncSpinner async_spinner(0); + // async_spinner.start(); + + // rclcpp::spin(ros2_node); + // ros::shutdown(); + // return 0; + // } + +private: + class GoalHandler + { + public: + void cancel() + { + std::lock_guard lock(mutex_); + canceled_ = true; + if (gh2_) + { // cancel goal if possible + auto fut = client_->async_cancel_goal(gh2_); + } + } + void handle() + { + auto goal1 = gh1_.getGoal(); + ROS2Goal goal2; + translate_goal_1_to_2(*gh1_.getGoal(), goal2); + + if (!client_->wait_for_action_server(std::chrono::seconds(1))) + { + std::cout << "Action server not available after waiting" << std::endl; + gh1_.setRejected(); + return; + } + + //Changes as per Dashing + auto send_goal_ops = ROS2SendGoalOptions(); + send_goal_ops.goal_response_callback = + [this](auto gh2_future) mutable { + auto goal_handle = gh2_future.get(); + if (!goal_handle) + { + gh1_.setRejected(); // goal was not accepted by remote server + return; + } + + gh1_.setAccepted(); + + { + std::lock_guard lock(mutex_); + gh2_ = goal_handle; + + if (canceled_) + { // cancel was called in between + auto fut = client_->async_cancel_goal(gh2_); + } + } + }; + + send_goal_ops.feedback_callback = + [this](ROS2GoalHandle, auto feedback2) mutable { + ROS1Feedback feedback1; + translate_feedback_2_to_1(feedback1, *feedback2); + gh1_.publishFeedback(feedback1); + }; + + // send goal to ROS2 server, set-up feedback + auto gh2_future = client_->async_send_goal(goal2, send_goal_ops); + + auto future_result = client_->async_get_result(gh2_future.get()); + auto res2 = future_result.get(); + + ROS1Result res1; + translate_result_2_to_1(res1, *(res2.result)); + + std::lock_guard lock(mutex_); + if (res2.code == rclcpp_action::ResultCode::SUCCEEDED) + { + gh1_.setSucceeded(res1); + } + else if (res2.code == rclcpp_action::ResultCode::CANCELED) + { + gh1_.setCanceled(res1); + } + else + { + gh1_.setAborted(res1); + } + } + + GoalHandler(ROS1GoalHandle &gh1, ROS2ClientSharedPtr &client) + : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} + + private: + ROS1GoalHandle gh1_; + ROS2GoalHandle gh2_; + ROS2ClientSharedPtr client_; + bool canceled_; // cancel was called + std::mutex mutex_; + }; + + ros::NodeHandle ros1_node_; + rclcpp::Node::SharedPtr ros2_node_; + + std::shared_ptr> server_; + ROS2ClientSharedPtr client_; + + std::mutex mutex_; + std::map> goals_; + + static void translate_goal_1_to_2(const ROS1Goal &, ROS2Goal &); + static void translate_result_2_to_1(ROS1Result &, const ROS2Result &); + static void translate_feedback_2_to_1(ROS1Feedback &, const ROS2Feedback &); +}; + +template +class ActionFactory_2_1 : public ActionFactoryInterface +{ +public: + using ROS2ServerGoalHandle = typename rclcpp_action::ServerGoalHandle; + using ROS1ClientGoalHandle = typename actionlib::ActionClient::GoalHandle; + using ROS2Goal = typename ROS2_T::Goal; + using ROS1Client = typename actionlib::ActionClient; + using ROS1Goal = typename actionlib::ActionServer::Goal; + using ROS1Feedback = typename actionlib::ActionServer::Feedback; + using ROS1Result = typename actionlib::ActionServer::Result; + using ROS1State = actionlib::SimpleClientGoalState; // There is no 'ClientGoalState'. Better solution? + using ROS2Feedback = typename ROS2_T::Feedback; + using ROS2Result = typename ROS2_T::Result; + using ROS2ServerSharedPtr = typename rclcpp_action::Server::SharedPtr; + using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + // ActionBridge_2_1( + // ros::NodeHandle ros1_node, + // rclcpp::Node::SharedPtr ros2_node, + // const std::string action_name) + // : ros1_node_(ros1_node), ros2_node_(ros2_node) + // { + // client_ = std::make_shared(ros1_node, action_name); + + // server_ = rclcpp_action::create_server(ros2_node_->get_node_base_interface(), + // ros2_node_->get_node_clock_interface(), + // ros2_node_->get_node_logging_interface(), + // ros2_node_->get_node_waitables_interface(), + // action_name, + // std::bind(&ActionBridge_2_1::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + // std::bind(&ActionBridge_2_1::handle_cancel, this, std::placeholders::_1), + // std::bind(&ActionBridge_2_1::handle_accepted, this, std::placeholders::_1)); + // } + + virtual void create_server_client(ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) + { + this->ros1_node_ = ros1_node; + this->ros2_node_ = ros2_node; + client_ = std::make_shared(ros1_node, action_name); + + server_ = rclcpp_action::create_server(ros2_node->get_node_base_interface(), + ros2_node->get_node_clock_interface(), + ros2_node->get_node_logging_interface(), + ros2_node->get_node_waitables_interface(), + action_name, + std::bind(&ActionFactory_2_1::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ActionFactory_2_1::handle_cancel, this, std::placeholders::_1), + std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); + } + + //ROS2 callbacks + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) + { + (void)uuid; + (void)goal; + if (!client_->waitForActionServerToStart(ros::Duration(1))) + { + std::cout << "Action server not available after waiting" << std::endl; + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + std::shared_ptr gh2) + { + // try to find goal and cancel it + std::lock_guard lock(mutex_); + auto it = goals_.find(get_goal_id_hash(gh2->get_goal_id())); + if (it != goals_.end()) + { + std::thread([handler = it->second]() mutable { + handler->cancel(); + }) + .detach(); + } + + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(std::shared_ptr gh2) + { + std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); + std::shared_ptr handler; + handler.reset(new GoalHandler(gh2, client_)); + std::lock_guard lock(mutex_); + goals_.insert(std::make_pair(goal_id, handler)); + + RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + std::thread([handler, goal_id, this]() mutable { + // execute the goal remotely + handler->handle(); + + // clean-up + std::lock_guard lock(mutex_); + goals_.erase(goal_id); + }) + .detach(); + } + + // static int main(const std::string &action_name, int argc, char *argv[]) + // { + // std::string node_name = "action_bridge_" + action_name; + // std::replace(node_name.begin(), node_name.end(), '/', '_'); + // // ROS 1 node + // ros::init(argc, argv, node_name); + // ros::NodeHandle ros1_node; + + // // ROS 2 node + // rclcpp::init(argc, argv); + // auto ros2_node = rclcpp::Node::make_shared(node_name); + + // ActionBridge_2_1 action_bridge(ros1_node, ros2_node, action_name); + + // // // ROS 1 asynchronous spinner + // ros::AsyncSpinner async_spinner(0); + // async_spinner.start(); + + // rclcpp::spin(ros2_node); + // ros::shutdown(); + // return 0; + // } + +private: + class GoalHandler + { + public: + void cancel() + { + std::lock_guard lock(mutex_); + canceled_ = true; + if (gh1_) + { + // cancel goal if possible + gh1_->cancel(); + } + } + void handle() + { + auto goal2 = gh2_->get_goal(); + ROS1Goal goal1; + translate_goal_2_to_1(*goal2, goal1); + + if (gh2_->is_canceling()) + { + auto result = std::make_shared(); + gh2_->canceled(result); + return; + } + + std::condition_variable cond_result; + std::atomic result_ready(false); + + auto goal_handle = client_->sendGoal( + goal1, + [this, &result_ready, &cond_result](ROS1ClientGoalHandle goal_handle) mutable // transition_cb + { + ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str()); + if (goal_handle.getCommState() == actionlib::CommState::RECALLING) + { + //cancelled before being processed + auto result2 = std::make_shared(); + gh2_->canceled(result2); + return; + } + else if (goal_handle.getCommState() == actionlib::CommState::ACTIVE) + { + std::lock_guard lock(mutex_); + gh1_ = std::make_shared(goal_handle); + } + else if (goal_handle.getCommState() == actionlib::CommState::DONE) + { + auto result2 = std::make_shared(); + auto result1 = goal_handle.getResult(); + translate_result_1_to_2(*result2, *result1); + ROS_INFO("Goal [%s]", goal_handle.getTerminalState().toString().c_str()); + if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) + { + gh2_->succeed(result2); + } + else + { + gh2_->abort(result2); + } + result_ready = true; + cond_result.notify_one(); + return; + } + }, + [this](ROS1ClientGoalHandle goal_handle, auto feedback1) mutable //feedback_cb + { + (void)goal_handle; + auto feedback2 = std::make_shared(); + translate_feedback_1_to_2(*feedback2, *feedback1); + gh2_->publish_feedback(feedback2); + }); + + std::mutex mutex_result; + std::unique_lock lck(mutex_result); + cond_result.wait(lck, [&result_ready] { + return result_ready.load(); + }); + } + + GoalHandler(std::shared_ptr &gh2, std::shared_ptr &client) + : gh2_(gh2), client_(client), canceled_(false) {} + + private: + std::shared_ptr gh1_; + std::shared_ptr gh2_; + std::shared_ptr client_; + bool canceled_; // cancel was called + std::mutex mutex_; + }; + + std::size_t get_goal_id_hash(const rclcpp_action::GoalUUID &uuid) + { + return std::hash{}(uuid); + } + + ros::NodeHandle ros1_node_; + rclcpp::Node::SharedPtr ros2_node_; + + std::shared_ptr client_; + ROS2ServerSharedPtr server_; + + std::mutex mutex_; + std::map> goals_; + + static void translate_goal_2_to_1(const ROS2Goal &, ROS1Goal &); + static void translate_result_1_to_2(ROS2Result &, const ROS1Result &); + static void translate_feedback_1_to_2(ROS2Feedback &, const ROS1Feedback &); +}; + +} // namespace ros1_bridge + +#endif // ACTION_BRIDGE__ACTION_BRIDGE_HPP_ diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index c4d79dfc..291ca9cd 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -72,6 +72,9 @@ get_factory( std::unique_ptr get_service_factory(const std::string &, const std::string &, const std::string &); +std::unique_ptr +get_action_factory(const std::string &, const std::string &, const std::string &); + Bridge1to2Handles create_bridge_from_1_to_2( ros::NodeHandle ros1_node, diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index 9a0f796d..1b1cb306 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -144,6 +144,14 @@ class ServiceFactoryInterface ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0; }; +class ActionFactoryInterface +{ +public: + virtual void create_server_client(ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) = 0; +}; + } // namespace ros1_bridge #endif // ROS1_BRIDGE__FACTORY_INTERFACE_HPP_ From d833d8cbe2eeb63405f448eba96c9c826bf352e6 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 5 May 2020 12:21:25 +0200 Subject: [PATCH 04/43] added action_bridge executable Signed-off-by: Harsh Deshpande --- CMakeLists.txt | 13 +++- include/ros1_bridge/action_factory.hpp | 76 ----------------------- src/action_bridge.cpp | 83 ++++++++++++++++++++++++++ 3 files changed, 94 insertions(+), 78 deletions(-) create mode 100644 src/action_bridge.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f878af4..c361e5a9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) # find ROS 1 packages set(cmake_extras_files cmake/find_ros1_package.cmake cmake/find_ros1_interface_packages.cmake) @@ -43,6 +44,7 @@ if(NOT ros1_roscpp_FOUND) endif() find_ros1_package(std_msgs REQUIRED) +find_ros1_package(actionlib REQUIRED) # Dependency that we should only look for if ROS 1 is installed (it's not present on a ROS 2 # system; see https://github.com/ros2/ros1_bridge/pull/331#issuecomment-1188111510) @@ -146,7 +148,7 @@ foreach(package_name ${ros2_interface_packages}) file(TO_CMAKE_PATH "${interface_file}" interface_name) get_filename_component(interface_basename "${interface_name}" NAME_WE) # skipping actions and request and response messages of services - if(NOT "${interface_name}" MATCHES "^(msg|srv)/" OR "${interface_basename}" MATCHES "_(Request|Response)$") + if(NOT "${interface_name}" MATCHES "^(msg|srv|action)/" OR "${interface_basename}" MATCHES "_(Request|Response)$") continue() endif() string(REPLACE "/" "__" interface_name "${interface_name}") @@ -225,7 +227,7 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} ${prefixed_ros1_message_packages} ${ros2_interface_packages} - "rclcpp" + "rclcpp" "rclcpp_action" "ros1_roscpp" "ros1_std_msgs") @@ -241,6 +243,13 @@ custom_executable(static_bridge target_link_libraries(static_bridge ${PROJECT_NAME}) +custom_executable(action_bridge + "src/action_bridge.cpp" + ROS1_DEPENDENCIES + TARGET_DEPENDENCIES ${ros2_interface_packages}) +target_link_libraries(action_bridge + ${PROJECT_NAME}) + custom_executable(parameter_bridge "src/parameter_bridge.cpp" ROS1_DEPENDENCIES diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index bb77603c..bc248d92 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -60,19 +60,6 @@ class ActionFactory_1_2 : public ActionFactoryInterface using ROS2GoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; - // ActionBridge_1_2( - // ros::NodeHandle ros1_node, - // rclcpp::Node::SharedPtr ros2_node, - // const std::string action_name) - // : ros1_node_(ros1_node), ros2_node_(ros2_node), - // server_(ros1_node, action_name, - // std::bind(&ActionBridge_1_2::goal_cb, this, std::placeholders::_1), - // std::bind(&ActionBridge_1_2::cancel_cb, this, std::placeholders::_1), - // false) - // { - // server_.start(); - // client_ = rclcpp_action::create_client(ros2_node, action_name); - // } virtual void create_server_client(ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, @@ -123,29 +110,6 @@ class ActionFactory_1_2 : public ActionFactoryInterface }).detach(); } - // static int main(const std::string &action_name, int argc, char *argv[]) - // { - // std::string node_name = "action_bridge_" + action_name; - // std::replace(node_name.begin(), node_name.end(), '/', '_'); - // // ROS 1 node - // ros::init(argc, argv, node_name); - // ros::NodeHandle ros1_node; - - // // ROS 2 node - // rclcpp::init(argc, argv); - // auto ros2_node = rclcpp::Node::make_shared(node_name); - - // ActionBridge_1_2 action_bridge(ros1_node, ros2_node, action_name); - - // // // ROS 1 asynchronous spinner - // ros::AsyncSpinner async_spinner(0); - // async_spinner.start(); - - // rclcpp::spin(ros2_node); - // ros::shutdown(); - // return 0; - // } - private: class GoalHandler { @@ -268,23 +232,6 @@ class ActionFactory_2_1 : public ActionFactoryInterface using ROS2Result = typename ROS2_T::Result; using ROS2ServerSharedPtr = typename rclcpp_action::Server::SharedPtr; using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; - // ActionBridge_2_1( - // ros::NodeHandle ros1_node, - // rclcpp::Node::SharedPtr ros2_node, - // const std::string action_name) - // : ros1_node_(ros1_node), ros2_node_(ros2_node) - // { - // client_ = std::make_shared(ros1_node, action_name); - - // server_ = rclcpp_action::create_server(ros2_node_->get_node_base_interface(), - // ros2_node_->get_node_clock_interface(), - // ros2_node_->get_node_logging_interface(), - // ros2_node_->get_node_waitables_interface(), - // action_name, - // std::bind(&ActionBridge_2_1::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - // std::bind(&ActionBridge_2_1::handle_cancel, this, std::placeholders::_1), - // std::bind(&ActionBridge_2_1::handle_accepted, this, std::placeholders::_1)); - // } virtual void create_server_client(ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, @@ -357,29 +304,6 @@ class ActionFactory_2_1 : public ActionFactoryInterface .detach(); } - // static int main(const std::string &action_name, int argc, char *argv[]) - // { - // std::string node_name = "action_bridge_" + action_name; - // std::replace(node_name.begin(), node_name.end(), '/', '_'); - // // ROS 1 node - // ros::init(argc, argv, node_name); - // ros::NodeHandle ros1_node; - - // // ROS 2 node - // rclcpp::init(argc, argv); - // auto ros2_node = rclcpp::Node::make_shared(node_name); - - // ActionBridge_2_1 action_bridge(ros1_node, ros2_node, action_name); - - // // // ROS 1 asynchronous spinner - // ros::AsyncSpinner async_spinner(0); - // async_spinner.start(); - - // rclcpp::spin(ros2_node); - // ros::shutdown(); - // return 0; - // } - private: class GoalHandler { diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp new file mode 100644 index 00000000..33a61dfa --- /dev/null +++ b/src/action_bridge.cpp @@ -0,0 +1,83 @@ +// Copyright 2015 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. + +#include + +// include ROS 1 +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunused-parameter" +#endif +#include "ros/ros.h" +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +// include ROS 2 +#include "rclcpp/rclcpp.hpp" + +#include "ros1_bridge/bridge.hpp" + +int main(int argc, char *argv[]) +{ + // ROS 1 node + ros::init(argc, argv, "ros_bridge"); + ros::NodeHandle ros1_node; + + // ROS 2 node + rclcpp::init(argc, argv); + auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); + + std::string dir = argv[1]; + std::string package = argv[2]; + std::string type = argv[3]; + std::string name = argv[4]; + + std::cout << dir << " " << package << " " << type << " " << name << std::endl; + + //ros1 example_tutorials Fibonacci fibonacci + + auto factory = ros1_bridge::get_action_factory( + dir, package, type); + if (factory) + { + printf("created action factory"); + try + { + factory->create_server_client(ros1_node, ros2_node, name); + // printf("Created 2 to 1 bridge for service %s\n", name.data()); + } + catch (std::runtime_error &e) + { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } + else + { + fprintf(stderr, "Failed to create a factory\n"); + } + + // ROS 1 asynchronous spinner + ros::AsyncSpinner async_spinner(1); + async_spinner.start(); + + // ROS 2 spinning loop + rclcpp::executors::SingleThreadedExecutor executor; + while (ros1_node.ok() && rclcpp::ok()) + { + executor.spin_node_once(ros2_node, std::chrono::milliseconds(1000)); + } + + return 0; +} From 629697ec13d7ee85b809282f9a3bef391db4401a Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 5 May 2020 12:21:53 +0200 Subject: [PATCH 05/43] fix template Signed-off-by: Harsh Deshpande --- resource/interface_factories.cpp.em | 57 ++++++++++++++++++----------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index 66df243e..d4fb8142 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -48,7 +48,7 @@ from rosidl_parser.definition import UnboundedString // include ROS 1 actions @[for action in mapped_actions]@ -#include <@(action["ros1_package"])/@(action["ros1_name"]).h> +#include <@(action["ros1_package"])/@(action["ros1_name"])Action.h> @[end for]@ // include ROS 2 actions @@ -125,26 +125,24 @@ get_action_factory_@(ros2_package_name)__@(interface_type)__@(interface.message_ @[end if]@ @[for action in mapped_actions]@ if ( - ( - ros_id == "ros1" && - package_name == "@(action["ros1_package"])" && - action_name == "@(action["ros1_name"])" - ) { - return std::unique_ptr(new ActionFactory_2_1< - @(action["ros1_package"])::@(action["ros1_name"]), - @(action["ros2_package"])::srv::@(action["ros2_name"]) - >); - } - else if + ros_id == "ros1" && + package_name == "@(action["ros1_package"])" && + action_name == "@(action["ros1_name"])" + ) { + return std::unique_ptr(new ActionFactory_2_1< + @(action["ros1_package"])::@(action["ros1_name"])Action, + @(action["ros2_package"])::action::@(action["ros2_name"]) + >); + } + else if ( ros_id == "ros2" && package_name == "@(action["ros2_package"])" && action_name == "action/@(action["ros2_name"])" - ) ) { return std::unique_ptr(new ActionFactory_1_2< - @(action["ros1_package"])::@(action["ros1_name"]), - @(action["ros2_package"])::srv::@(action["ros2_name"]) + @(action["ros1_package"])::@(action["ros1_name"])Action, + @(action["ros2_package"])::action::@(action["ros2_name"]) >); } @[end for]@ @@ -387,16 +385,29 @@ void ServiceFactory< @[ for frm, to in [("1", "2"), ("2", "1")]]@ @[ for type in ["Goal", "Result", "Feedback"]]@ template <> -void ActionFactory_@(frm)_@(to)< +@[ if type == "Goal"]@ +@{ +frm_, to_ = frm, to +const_1 = "const " +const_2 = "" +}@ +@[ else]@ +@{ +frm_, to_ = to, frm +const_1 = "" +const_2 = "const " +}@ +@[ end if]@ +void ActionFactory_@(frm_)_@(to_)< @(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)) + @(const_1)ROS@(frm)@(type) &@(type.lower())@(frm), + @(const_2)ROS@(to)@(type) &@(type.lower())@(to)) @[ else]@ - const ROS@(to)Goal &@(type.lower())@(to), - ROS@(frm)Goal &@(type.lower())@(frm)) + @(const_1)ROS@(to)@(type) &@(type.lower())@(to), + @(const_2)ROS@(frm)@(type) &@(type.lower())@(frm)) @[ end if]@ { @[ for field in action["fields"][type.lower()]]@ @@ -415,9 +426,13 @@ void ActionFactory_@(frm)_@(to)< auto & @(field["ros" + to]["name"])@(to) = @(type.lower())@(to).@(field["ros" + to]["name"]); @[ end if]@ @[ if field["basic"]]@ +@[ if field["ros2"]["type"].startswith("builtin_interfaces") ]@ + ros1_bridge::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); +@[ else]@ @(field["ros" + to]["name"])@(to) = @(field["ros" + frm]["name"])@(frm); +@[ end if]@ @[ else]@ - Factory<@(field["ros" + frm]["cpptype"]),@(field["ros" + to]["cpptype"])>::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); + Factory<@(field["ros1"]["cpptype"]),@(field["ros2"]["cpptype"])>::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); @[end if]@ @[ if field["array"]]@ } From 1539a30bfaa593aa92e2699ca46e34cdf1e5380e Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 5 May 2020 12:22:17 +0200 Subject: [PATCH 06/43] hack for GripperCommand Signed-off-by: Harsh Deshpande --- ros1_bridge/__init__.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 4c83efc9..3e54d71d 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -84,8 +84,6 @@ def generate_cpp(output_path, template_dir): data.update(generate_actions( rospack, message_string_pairs=message_string_pairs)) - print(data['actions']) - template_file = os.path.join(template_dir, 'get_mappings.cpp.em') output_file = os.path.join(output_path, 'get_mappings.cpp') data_for_template = { @@ -881,7 +879,7 @@ def determine_common_actions( # 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"): + if (ros1_type, ros2_type) not in message_string_pairs and not ros2_type.startswith("builtin_interfaces") and "GripperCommand" not in ros2_type: match = False break output[direction].append({ From 5d96c75bea1d432b3d5b93bf7cd62e6f9f99d0d9 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 9 Jun 2020 08:24:57 +0200 Subject: [PATCH 07/43] Update include/ros1_bridge/action_factory.hpp Co-authored-by: tomoya Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index bc248d92..3e91d0d7 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Fraunhofer IPA +// Copyright 2020 Fraunhofer IPA // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From e265f37e749781cd5eb6bee9638da1e2c422f0d2 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 13 Nov 2020 12:32:27 +0000 Subject: [PATCH 08/43] Fix python script errors in action generation Signed-off-by: Victor Lopez Signed-off-by: Harsh Deshpande --- ros1_bridge/__init__.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 3e54d71d..8b73f0df 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -153,19 +153,25 @@ def generate_cpp(output_path, template_dir): 'ros2_package_name': ros2_package_name, 'interface_type': interface_type, 'interface': interface, - 'mapped_msgs': [ + 'mapped_msgs': [], + 'mapped_services': [], + 'mapped_actions': [], + } + if interface_type == 'msg': + data_idl_cpp['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], - 'mapped_actions': [ + s['ros2_name'] == interface.message_name] + if interface_type == 'action': + data_idl_cpp['mapped_actions'] += [ s for s in data['actions'] if s['ros2_package'] == ros2_package_name and - s['ros2_name'] == interface.message_name], - } + s['ros2_name'] == interface.message_name] + template_file = os.path.join( template_dir, 'interface_factories.cpp.em') output_file = os.path.join( From 2d519585d8884139cfe1597290b2546393ee6b37 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 13 Nov 2020 12:32:59 +0000 Subject: [PATCH 09/43] Foxy and later action compatibility Signed-off-by: Victor Lopez Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 3e91d0d7..82665b31 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -136,11 +136,14 @@ class ActionFactory_1_2 : public ActionFactoryInterface return; } + // goal_response_callback signature changed after foxy, this implementation + // works with both + std::shared_future gh2_future; //Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); send_goal_ops.goal_response_callback = - [this](auto gh2_future) mutable { - auto goal_handle = gh2_future.get(); + [this, &gh2_future](auto gh2) mutable { + auto goal_handle = gh2_future.get(); if (!goal_handle) { gh1_.setRejected(); // goal was not accepted by remote server @@ -168,7 +171,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface }; // send goal to ROS2 server, set-up feedback - auto gh2_future = client_->async_send_goal(goal2, send_goal_ops); + gh2_future = client_->async_send_goal(goal2, send_goal_ops); auto future_result = client_->async_get_result(gh2_future.get()); auto res2 = future_result.get(); From 0040380bbb626845e6c00eaa97e6f9eddfabbc8b Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 17 Nov 2020 09:48:14 +0100 Subject: [PATCH 10/43] Reorder ros node initialization to allow renaming of bridge Signed-off-by: Harsh Deshpande --- src/action_bridge.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index 33a61dfa..c79df303 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -31,14 +31,15 @@ int main(int argc, char *argv[]) { - // ROS 1 node - ros::init(argc, argv, "ros_bridge"); - ros::NodeHandle ros1_node; - // ROS 2 node + // must be before ROS1, because ros::init consumes args like __name and we cannot remap the node rclcpp::init(argc, argv); auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); + // ROS 1 node + ros::init(argc, argv, "ros_bridge"); + ros::NodeHandle ros1_node; + std::string dir = argv[1]; std::string package = argv[2]; std::string type = argv[3]; From 580ad9e554250b20eb87dc422c839038fb1e8b3f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 24 May 2021 16:46:12 +0200 Subject: [PATCH 11/43] fix error during rebase Signed-off-by: Harsh Deshpande --- ros1_bridge/__init__.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 8b73f0df..9101fd8c 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -161,8 +161,9 @@ def generate_cpp(output_path, template_dir): data_idl_cpp['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': [ + m.ros2_msg.message_name == interface.message_name] + if interface_type == 'srv': + data_idl_cpp['mapped_services'] += [ s for s in data['services'] if s['ros2_package'] == ros2_package_name and s['ros2_name'] == interface.message_name] From 33b8bceda07100fcc4d931868edd0d0c9f432281 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 24 May 2021 16:48:16 +0200 Subject: [PATCH 12/43] updated deprecated GoalResponseCallback signature https://github.com/ros2/rclcpp/blob/master/rclcpp_action/include/rclcpp_action/client.hpp#L289-L299 Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 82665b31..11bda605 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -57,9 +57,10 @@ class ActionFactory_1_2 : public ActionFactoryInterface using ROS2Goal = typename ROS2_T::Goal; using ROS2Feedback = typename ROS2_T::Feedback; using ROS2Result = typename ROS2_T::Result; - using ROS2GoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; + using ROS2ClientGoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + using ROS2GoalHandle = typename rclcpp_action::Client::GoalHandle::SharedPtr; virtual void create_server_client(ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, @@ -138,12 +139,12 @@ class ActionFactory_1_2 : public ActionFactoryInterface // goal_response_callback signature changed after foxy, this implementation // works with both - std::shared_future gh2_future; + std::shared_future gh2_future; //Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); send_goal_ops.goal_response_callback = - [this, &gh2_future](auto gh2) mutable { - auto goal_handle = gh2_future.get(); + [this, &gh2_future](ROS2GoalHandle gh2) mutable { + auto goal_handle = gh2_future.get(); if (!goal_handle) { gh1_.setRejected(); // goal was not accepted by remote server @@ -164,7 +165,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface }; send_goal_ops.feedback_callback = - [this](ROS2GoalHandle, auto feedback2) mutable { + [this](ROS2ClientGoalHandle, auto feedback2) mutable { ROS1Feedback feedback1; translate_feedback_2_to_1(feedback1, *feedback2); gh1_.publishFeedback(feedback1); @@ -199,7 +200,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface private: ROS1GoalHandle gh1_; - ROS2GoalHandle gh2_; + ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; bool canceled_; // cancel was called std::mutex mutex_; From 3d8dee1663ba8907f4e412363b8689ef40a4c52c Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 18 Nov 2020 11:12:27 +0100 Subject: [PATCH 13/43] Update resource/interface_factories.cpp.em Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Signed-off-by: Harsh Deshpande --- resource/interface_factories.cpp.em | 2 -- 1 file changed, 2 deletions(-) diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index d4fb8142..99a693e9 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -149,8 +149,6 @@ get_action_factory_@(ros2_package_name)__@(interface_type)__@(interface.message_ return nullptr; } @ -} -@ // conversion functions for available interfaces @[for m in mapped_msgs]@ From 109180678c25d6d877c39d34091cd468e919d416 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 18 Nov 2020 15:31:57 +0100 Subject: [PATCH 14/43] applied uncrustify patches Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 533 +++++++++++----------- include/ros1_bridge/factory_interface.hpp | 7 +- src/action_bridge.cpp | 25 +- 3 files changed, 283 insertions(+), 282 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 11bda605..b4d55522 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -12,40 +12,41 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ACTION_BRIDGE__ACTION_BRIDGE_HPP_ -#define ACTION_BRIDGE__ACTION_BRIDGE_HPP_ +#ifndef ROS1_BRIDGE__ACTION_FACTORY_HPP_ +#define ROS1_BRIDGE__ACTION_FACTORY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include #ifdef __clang__ #pragma clang diagnostic push #pragma clang diagnostic ignored "-Wunused-parameter" #endif -#include -#include #include -#include //Need this for the goal state. Need a better solution +#include // Need this for the goal state. Need a better solution +#include +#include #ifdef __clang__ #pragma clang diagnostic pop #endif // include ROS 2 -#include "rclcpp/rclcpp.hpp" #include #include "factory_interface.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" namespace ros1_bridge { -template +template class ActionFactory_1_2 : public ActionFactoryInterface { public: @@ -76,66 +77,71 @@ class ActionFactory_1_2 : public ActionFactoryInterface server_->start(); client_ = rclcpp_action::create_client(ros2_node, action_name); } + } - void cancel_cb(ROS1GoalHandle gh1) - { - // try to find goal and cancel it - std::lock_guard lock(mutex_); - auto it = goals_.find(gh1.getGoalID().id); - if (it != goals_.end()) - { - std::thread([handler = it->second]() mutable { - handler->cancel(); - }).detach(); - } - } - - void goal_cb(ROS1GoalHandle gh1) - { - const std::string goal_id = gh1.getGoalID().id; + void goal_cb(ROS1GoalHandle gh1) + { + const std::string goal_id = gh1.getGoalID().id; - // create a new handler for the goal - std::shared_ptr handler; - handler.reset(new GoalHandler(gh1, client_)); - std::lock_guard lock(mutex_); - goals_.insert(std::make_pair(goal_id, handler)); + // create a new handler for the goal + std::shared_ptr handler; + handler.reset(new GoalHandler(gh1, client_)); + std::lock_guard lock(mutex_); + goals_.insert(std::make_pair(goal_id, handler)); - RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); - std::thread([handler, goal_id, this]() mutable { - // execute the goal remotely - handler->handle(); + RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + std::thread( + [handler, goal_id, this]() mutable { + // execute the goal remotely + handler->handle(); - // clean-up - std::lock_guard lock(mutex_); - goals_.erase(goal_id); - }).detach(); - } + // clean-up + std::lock_guard lock(mutex_); + goals_.erase(goal_id); + }).detach(); + } private: - class GoalHandler + class GoalHandler + { +public: + void cancel() { - public: - void cancel() - { + std::lock_guard lock(mutex_); + canceled_ = true; + if (gh2_) { // cancel goal if possible + auto fut = client_->async_cancel_goal(gh2_); + } + } + void handle() + { + auto goal1 = gh1_.getGoal(); + ROS2Goal goal2; + translate_goal_1_to_2(*gh1_.getGoal(), goal2); + + if (!client_->wait_for_action_server(std::chrono::seconds(1))) { + std::cout << "Action server not available after waiting" << std::endl; + gh1_.setRejected(); + return; + } + + // goal_response_callback signature changed after foxy, this implementation + // works with both + std::shared_future gh2_future; + // Changes as per Dashing + auto send_goal_ops = ROS2SendGoalOptions(); + send_goal_ops.goal_response_callback = [this, &gh2_future](auto gh2) mutable { + auto goal_handle = gh2_future.get(); + if (!goal_handle) { + gh1_.setRejected(); // goal was not accepted by remote server + return; + } + + gh1_.setAccepted(); + + { std::lock_guard lock(mutex_); - canceled_ = true; - if (gh2_) - { // cancel goal if possible - auto fut = client_->async_cancel_goal(gh2_); - } - } - void handle() - { - auto goal1 = gh1_.getGoal(); - ROS2Goal goal2; - translate_goal_1_to_2(*gh1_.getGoal(), goal2); - - if (!client_->wait_for_action_server(std::chrono::seconds(1))) - { - std::cout << "Action server not available after waiting" << std::endl; - gh1_.setRejected(); - return; - } + gh2_ = goal_handle; // goal_response_callback signature changed after foxy, this implementation // works with both @@ -185,18 +191,36 @@ class ActionFactory_1_2 : public ActionFactoryInterface { gh1_.setSucceeded(res1); } - else if (res2.code == rclcpp_action::ResultCode::CANCELED) - { - gh1_.setCanceled(res1); - } - else - { - gh1_.setAborted(res1); - } - } + } + }; + + send_goal_ops.feedback_callback = [this](ROS2GoalHandle, auto feedback2) mutable { + ROS1Feedback feedback1; + translate_feedback_2_to_1(feedback1, *feedback2); + gh1_.publishFeedback(feedback1); + }; + + // send goal to ROS2 server, set-up feedback + gh2_future = client_->async_send_goal(goal2, send_goal_ops); + + auto future_result = client_->async_get_result(gh2_future.get()); + auto res2 = future_result.get(); + + ROS1Result res1; + translate_result_2_to_1(res1, *(res2.result)); + + std::lock_guard lock(mutex_); + if (res2.code == rclcpp_action::ResultCode::SUCCEEDED) { + gh1_.setSucceeded(res1); + } else if (res2.code == rclcpp_action::ResultCode::CANCELED) { + gh1_.setCanceled(res1); + } else { + gh1_.setAborted(res1); + } + } - GoalHandler(ROS1GoalHandle &gh1, ROS2ClientSharedPtr &client) - : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} + GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) + : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} private: ROS1GoalHandle gh1_; @@ -206,219 +230,204 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::mutex mutex_; }; - ros::NodeHandle ros1_node_; - rclcpp::Node::SharedPtr ros2_node_; + ros::NodeHandle ros1_node_; + rclcpp::Node::SharedPtr ros2_node_; - std::shared_ptr> server_; - ROS2ClientSharedPtr client_; + std::shared_ptr> server_; + ROS2ClientSharedPtr client_; - std::mutex mutex_; - std::map> goals_; + std::mutex mutex_; + std::map> goals_; - static void translate_goal_1_to_2(const ROS1Goal &, ROS2Goal &); - static void translate_result_2_to_1(ROS1Result &, const ROS2Result &); - static void translate_feedback_2_to_1(ROS1Feedback &, const ROS2Feedback &); + static void translate_goal_1_to_2(const ROS1Goal &, ROS2Goal &); + static void translate_result_2_to_1(ROS1Result &, const ROS2Result &); + static void translate_feedback_2_to_1(ROS1Feedback &, const ROS2Feedback &); }; -template +template class ActionFactory_2_1 : public ActionFactoryInterface { public: - using ROS2ServerGoalHandle = typename rclcpp_action::ServerGoalHandle; - using ROS1ClientGoalHandle = typename actionlib::ActionClient::GoalHandle; - using ROS2Goal = typename ROS2_T::Goal; - using ROS1Client = typename actionlib::ActionClient; - using ROS1Goal = typename actionlib::ActionServer::Goal; - using ROS1Feedback = typename actionlib::ActionServer::Feedback; - using ROS1Result = typename actionlib::ActionServer::Result; - using ROS1State = actionlib::SimpleClientGoalState; // There is no 'ClientGoalState'. Better solution? - using ROS2Feedback = typename ROS2_T::Feedback; - using ROS2Result = typename ROS2_T::Result; - using ROS2ServerSharedPtr = typename rclcpp_action::Server::SharedPtr; - using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; - - virtual void create_server_client(ros::NodeHandle ros1_node, - rclcpp::Node::SharedPtr ros2_node, - const std::string action_name) - { - this->ros1_node_ = ros1_node; - this->ros2_node_ = ros2_node; - client_ = std::make_shared(ros1_node, action_name); - - server_ = rclcpp_action::create_server(ros2_node->get_node_base_interface(), - ros2_node->get_node_clock_interface(), - ros2_node->get_node_logging_interface(), - ros2_node->get_node_waitables_interface(), - action_name, - std::bind(&ActionFactory_2_1::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&ActionFactory_2_1::handle_cancel, this, std::placeholders::_1), - std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); + using ROS2ServerGoalHandle = typename rclcpp_action::ServerGoalHandle; + using ROS1ClientGoalHandle = typename actionlib::ActionClient::GoalHandle; + using ROS2Goal = typename ROS2_T::Goal; + using ROS1Client = typename actionlib::ActionClient; + using ROS1Goal = typename actionlib::ActionServer::Goal; + using ROS1Feedback = typename actionlib::ActionServer::Feedback; + using ROS1Result = typename actionlib::ActionServer::Result; + using ROS1State = + actionlib::SimpleClientGoalState; // There is no 'ClientGoalState'. Better solution? + using ROS2Feedback = typename ROS2_T::Feedback; + using ROS2Result = typename ROS2_T::Result; + using ROS2ServerSharedPtr = typename rclcpp_action::Server::SharedPtr; + using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + + virtual void create_server_client( + ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) + { + this->ros1_node_ = ros1_node; + this->ros2_node_ = ros2_node; + client_ = std::make_shared(ros1_node, action_name); + + server_ = rclcpp_action::create_server( + ros2_node->get_node_base_interface(), ros2_node->get_node_clock_interface(), + ros2_node->get_node_logging_interface(), ros2_node->get_node_waitables_interface(), + action_name, + std::bind( + &ActionFactory_2_1::handle_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&ActionFactory_2_1::handle_cancel, this, std::placeholders::_1), + std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); + } + + // ROS2 callbacks + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + (void)uuid; + (void)goal; + if (!client_->waitForActionServerToStart(ros::Duration(1))) { + std::cout << "Action server not available after waiting" << std::endl; + return rclcpp_action::GoalResponse::REJECT; } - //ROS2 callbacks - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &uuid, - std::shared_ptr goal) - { - (void)uuid; - (void)goal; - if (!client_->waitForActionServerToStart(ros::Duration(1))) - { - std::cout << "Action server not available after waiting" << std::endl; - return rclcpp_action::GoalResponse::REJECT; - } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::CancelResponse handle_cancel(std::shared_ptr gh2) + { + // try to find goal and cancel it + std::lock_guard lock(mutex_); + auto it = goals_.find(get_goal_id_hash(gh2->get_goal_id())); + if (it != goals_.end()) { + std::thread([handler = it->second]() mutable {handler->cancel();}).detach(); } - rclcpp_action::CancelResponse handle_cancel( - std::shared_ptr gh2) - { - // try to find goal and cancel it - std::lock_guard lock(mutex_); - auto it = goals_.find(get_goal_id_hash(gh2->get_goal_id())); - if (it != goals_.end()) - { - std::thread([handler = it->second]() mutable { - handler->cancel(); - }) - .detach(); - } + return rclcpp_action::CancelResponse::ACCEPT; + } - return rclcpp_action::CancelResponse::ACCEPT; - } + void handle_accepted(std::shared_ptr gh2) + { + std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); + std::shared_ptr handler; + handler.reset(new GoalHandler(gh2, client_)); + std::lock_guard lock(mutex_); + goals_.insert(std::make_pair(goal_id, handler)); - void handle_accepted(std::shared_ptr gh2) - { - std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); - std::shared_ptr handler; - handler.reset(new GoalHandler(gh2, client_)); - std::lock_guard lock(mutex_); - goals_.insert(std::make_pair(goal_id, handler)); + RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + std::thread( + [handler, goal_id, this]() mutable { + // execute the goal remotely + handler->handle(); - RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); - std::thread([handler, goal_id, this]() mutable { - // execute the goal remotely - handler->handle(); - - // clean-up - std::lock_guard lock(mutex_); - goals_.erase(goal_id); - }) - .detach(); - } + // clean-up + std::lock_guard lock(mutex_); + goals_.erase(goal_id); + }).detach(); + } private: - class GoalHandler + class GoalHandler + { +public: + void cancel() + { + std::lock_guard lock(mutex_); + canceled_ = true; + if (gh1_) { + // cancel goal if possible + gh1_->cancel(); + } + } + void handle() { - public: - void cancel() + auto goal2 = gh2_->get_goal(); + ROS1Goal goal1; + translate_goal_2_to_1(*goal2, goal1); + + if (gh2_->is_canceling()) { + auto result = std::make_shared(); + gh2_->canceled(result); + return; + } + + std::condition_variable cond_result; + std::atomic result_ready(false); + + auto goal_handle = client_->sendGoal( + goal1, + [this, &result_ready, + &cond_result](ROS1ClientGoalHandle goal_handle) mutable // transition_cb { + ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str()); + if (goal_handle.getCommState() == actionlib::CommState::RECALLING) { + // cancelled before being processed + auto result2 = std::make_shared(); + gh2_->canceled(result2); + return; + } else if (goal_handle.getCommState() == actionlib::CommState::ACTIVE) { std::lock_guard lock(mutex_); - canceled_ = true; - if (gh1_) - { - // cancel goal if possible - gh1_->cancel(); + gh1_ = std::make_shared(goal_handle); + } else if (goal_handle.getCommState() == actionlib::CommState::DONE) { + auto result2 = std::make_shared(); + auto result1 = goal_handle.getResult(); + translate_result_1_to_2(*result2, *result1); + ROS_INFO("Goal [%s]", goal_handle.getTerminalState().toString().c_str()); + if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) { + gh2_->succeed(result2); + } else { + gh2_->abort(result2); } - } - void handle() + result_ready = true; + cond_result.notify_one(); + return; + } + }, + [this](ROS1ClientGoalHandle goal_handle, auto feedback1) mutable // feedback_cb { - auto goal2 = gh2_->get_goal(); - ROS1Goal goal1; - translate_goal_2_to_1(*goal2, goal1); - - if (gh2_->is_canceling()) - { - auto result = std::make_shared(); - gh2_->canceled(result); - return; - } - - std::condition_variable cond_result; - std::atomic result_ready(false); + (void)goal_handle; + auto feedback2 = std::make_shared(); + translate_feedback_1_to_2(*feedback2, *feedback1); + gh2_->publish_feedback(feedback2); + }); + + std::mutex mutex_result; + std::unique_lock lck(mutex_result); + cond_result.wait(lck, [&result_ready] {return result_ready.load();}); + } - auto goal_handle = client_->sendGoal( - goal1, - [this, &result_ready, &cond_result](ROS1ClientGoalHandle goal_handle) mutable // transition_cb - { - ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str()); - if (goal_handle.getCommState() == actionlib::CommState::RECALLING) - { - //cancelled before being processed - auto result2 = std::make_shared(); - gh2_->canceled(result2); - return; - } - else if (goal_handle.getCommState() == actionlib::CommState::ACTIVE) - { - std::lock_guard lock(mutex_); - gh1_ = std::make_shared(goal_handle); - } - else if (goal_handle.getCommState() == actionlib::CommState::DONE) - { - auto result2 = std::make_shared(); - auto result1 = goal_handle.getResult(); - translate_result_1_to_2(*result2, *result1); - ROS_INFO("Goal [%s]", goal_handle.getTerminalState().toString().c_str()); - if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) - { - gh2_->succeed(result2); - } - else - { - gh2_->abort(result2); - } - result_ready = true; - cond_result.notify_one(); - return; - } - }, - [this](ROS1ClientGoalHandle goal_handle, auto feedback1) mutable //feedback_cb - { - (void)goal_handle; - auto feedback2 = std::make_shared(); - translate_feedback_1_to_2(*feedback2, *feedback1); - gh2_->publish_feedback(feedback2); - }); - - std::mutex mutex_result; - std::unique_lock lck(mutex_result); - cond_result.wait(lck, [&result_ready] { - return result_ready.load(); - }); - } - - GoalHandler(std::shared_ptr &gh2, std::shared_ptr &client) - : gh2_(gh2), client_(client), canceled_(false) {} + GoalHandler(std::shared_ptr & gh2, std::shared_ptr & client) + : gh2_(gh2), client_(client), canceled_(false) {} - private: - std::shared_ptr gh1_; - std::shared_ptr gh2_; - std::shared_ptr client_; - bool canceled_; // cancel was called - std::mutex mutex_; - }; +private: + std::shared_ptr gh1_; + std::shared_ptr gh2_; + std::shared_ptr client_; + bool canceled_; // cancel was called + std::mutex mutex_; + }; - std::size_t get_goal_id_hash(const rclcpp_action::GoalUUID &uuid) - { - return std::hash{}(uuid); - } + std::size_t get_goal_id_hash(const rclcpp_action::GoalUUID & uuid) + { + return std::hash{} (uuid); + } - ros::NodeHandle ros1_node_; - rclcpp::Node::SharedPtr ros2_node_; + ros::NodeHandle ros1_node_; + rclcpp::Node::SharedPtr ros2_node_; - std::shared_ptr client_; - ROS2ServerSharedPtr server_; + std::shared_ptr client_; + ROS2ServerSharedPtr server_; - std::mutex mutex_; - std::map> goals_; + std::mutex mutex_; + std::map> goals_; - static void translate_goal_2_to_1(const ROS2Goal &, ROS1Goal &); - static void translate_result_1_to_2(ROS2Result &, const ROS1Result &); - static void translate_feedback_1_to_2(ROS2Feedback &, const ROS1Feedback &); + static void translate_goal_2_to_1(const ROS2Goal &, ROS1Goal &); + static void translate_result_1_to_2(ROS2Result &, const ROS1Result &); + static void translate_feedback_1_to_2(ROS2Feedback &, const ROS1Feedback &); }; -} // namespace ros1_bridge +} // namespace ros1_bridge -#endif // ACTION_BRIDGE__ACTION_BRIDGE_HPP_ +#endif // ROS1_BRIDGE__ACTION_FACTORY_HPP_ diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index 1b1cb306..c07cde82 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -147,9 +147,10 @@ class ServiceFactoryInterface class ActionFactoryInterface { public: - virtual void create_server_client(ros::NodeHandle ros1_node, - rclcpp::Node::SharedPtr ros2_node, - const std::string action_name) = 0; + virtual void create_server_client( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) = 0; }; } // namespace ros1_bridge diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index c79df303..c04f5d7b 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -26,10 +26,9 @@ // include ROS 2 #include "rclcpp/rclcpp.hpp" - #include "ros1_bridge/bridge.hpp" -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { // ROS 2 node // must be before ROS1, because ros::init consumes args like __name and we cannot remap the node @@ -47,25 +46,18 @@ int main(int argc, char *argv[]) std::cout << dir << " " << package << " " << type << " " << name << std::endl; - //ros1 example_tutorials Fibonacci fibonacci + // ros1 example_tutorials Fibonacci fibonacci - auto factory = ros1_bridge::get_action_factory( - dir, package, type); - if (factory) - { + auto factory = ros1_bridge::get_action_factory(dir, package, type); + if (factory) { printf("created action factory"); - try - { + try { factory->create_server_client(ros1_node, ros2_node, name); // printf("Created 2 to 1 bridge for service %s\n", name.data()); - } - catch (std::runtime_error &e) - { + } catch (std::runtime_error & e) { fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); } - } - else - { + } else { fprintf(stderr, "Failed to create a factory\n"); } @@ -75,8 +67,7 @@ int main(int argc, char *argv[]) // ROS 2 spinning loop rclcpp::executors::SingleThreadedExecutor executor; - while (ros1_node.ok() && rclcpp::ok()) - { + while (ros1_node.ok() && rclcpp::ok()) { executor.spin_node_once(ros2_node, std::chrono::milliseconds(1000)); } From 74188ff9c9c0d5f669cbbffdba68e58aad9ca4aa Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 20 Nov 2020 01:20:44 +0100 Subject: [PATCH 15/43] fix action mapping rules Signed-off-by: Harsh Deshpande --- resource/interface_factories.cpp.em | 2 +- ros1_bridge/__init__.py | 37 +++++++++++++++++++---------- src/action_bridge.cpp | 2 +- 3 files changed, 27 insertions(+), 14 deletions(-) diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index 99a693e9..a5a03874 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -410,7 +410,7 @@ void ActionFactory_@(frm_)_@(to_)< { @[ 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()); + @(type.lower())@(to).@(field["ros" + to]["name"]).resize(@(type.lower())@(frm).@(field["ros" + frm]["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 ( diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 9101fd8c..631e2885 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -793,12 +793,18 @@ def determine_common_services( 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: - match = False - break + if ros1_name != ros2_name: + # if the names do not match, check first if the types are builtin + ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) + ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type() + # Check if types are primitive types + if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type: + # 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 and + not ros2_type.startswith('builtin_interfaces')): + match = False + break output[direction].append({ 'basic': False if '/' in ros1_type else True, 'array': True if '[]' in ros1_type else False, @@ -881,14 +887,21 @@ def determine_common_actions( 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 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") and "GripperCommand" not in ros2_type: - match = False - break + + ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) + ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type() + + # Check if types are primitive types + if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type: + # 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") and "GripperCommand" not in ros2_type: + match = False + break output[direction].append({ 'basic': False if '/' in ros1_type else True, 'array': True if '[]' in ros1_type else False, diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index c04f5d7b..bb0185b1 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -50,7 +50,7 @@ int main(int argc, char * argv[]) auto factory = ros1_bridge::get_action_factory(dir, package, type); if (factory) { - printf("created action factory"); + printf("created action factory\n"); try { factory->create_server_client(ros1_node, ros2_node, name); // printf("Created 2 to 1 bridge for service %s\n", name.data()); From 5cd1321d4a887106ff1e8c6d831df91145a08572 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sat, 21 Nov 2020 10:34:00 +0100 Subject: [PATCH 16/43] print available action pairs Signed-off-by: Harsh Deshpande --- include/ros1_bridge/bridge.hpp | 3 +++ src/dynamic_bridge.cpp | 9 +++++++++ 2 files changed, 12 insertions(+) diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index 291ca9cd..5ccef637 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -64,6 +64,9 @@ get_all_message_mappings_2to1(); std::multimap get_all_service_mappings_2to1(); +std::multimap +get_all_action_mappings_2to1(); + std::shared_ptr get_factory( const std::string & ros1_type_name, diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 1e1a5522..3e208e3d 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -111,6 +111,15 @@ bool parse_command_options( } else { printf("No service type conversion pairs supported.\n"); } + mappings_2to1 = ros1_bridge::get_all_action_mappings_2to1(); + if (mappings_2to1.size() > 0) { + printf("Supported ROS 2 <=> ROS 1 action type conversion pairs:\n"); + for (auto & pair : mappings_2to1) { + printf(" - '%s' (ROS 2) <=> '%s' (ROS 1)\n", pair.first.c_str(), pair.second.c_str()); + } + } else { + printf("No action type conversion pairs supported.\n"); + } return false; } From 3c3fe174ce10cd89726c3fd75aba9ec105e844dd Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sat, 21 Nov 2020 10:36:31 +0100 Subject: [PATCH 17/43] get active ROS1 action servers and clients Signed-off-by: Harsh Deshpande --- src/dynamic_bridge.cpp | 163 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 163 insertions(+) diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 3e208e3d..c451975f 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -19,6 +19,7 @@ #include #include #include +#include // include ROS 1 #ifdef __clang__ @@ -39,6 +40,7 @@ #include "rcpputils/scope_exit.hpp" #include "ros1_bridge/bridge.hpp" +#include "ros1_bridge/action_factory.hpp" std::mutex g_bridge_mutex; @@ -466,6 +468,158 @@ void get_ros1_service_info( ros1_services[key]["name"] = std::string(t.begin() + t.find("/") + 1, t.end()); } +inline bool is_action_topic(std::map &actions, + std::map &action_nums, const bool is_action_type, + const std::string topic_name, const std::string topic_name_ends_with, + const std::string type, const std::string type_ends_with) { + // check if the topic name and topic types are as expected + if (boost::algorithm::ends_with(topic_name.c_str(), topic_name_ends_with.c_str()) && + boost::algorithm::ends_with(type.c_str(), type_ends_with.c_str())) { + // extract action name from topic name + std::string name = topic_name.substr(0, topic_name.find(topic_name_ends_with.c_str())); + if (actions.find(name) == actions.end()) { + actions[name] = ""; + action_nums[name] = 0; + } + + // e.g.: topic type of '/fibonacci/goal' is 'actionlib_tutorials/FibonacciActionGoal' + // Thus, action type is 'actionlib_tutorials/Fibonacci' + std::string action_type = type.substr(0, type.find(type_ends_with.c_str())); + actions[name] = is_action_type ? action_type : actions[name]; + action_nums[name] += 1; + return true; + } + return false; +} + +// if topics 'goal' with type 'ActionGoal' and 'cancel' with type 'GoalID' are pubs, then it is an +// action client +// equivalent ROS2 action pkg and type can be retrieved from get_mappings.cpp +void get_active_ros1_actions(std::map publishers, + std::map subscribers, + std::map &active_ros1_action_servers, + std::map &active_ros1_action_clients) { + // check if the topics end with 'goal', 'result', 'cancel', 'status' + + // find topics that end with goal and cancel, find corresponding result, status and feedback + // in the other map + std::map::iterator it; + std::map::iterator it_num; + // store count of pubs and subs for each action + std::map action_server_nums, action_client_nums; + + for (it = publishers.begin(); it != publishers.end(); it++) { + // check for action client + if (is_action_topic(active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/cancel", it->second.c_str(), "/GoalID")) { + continue; + } else if (is_action_topic(active_ros1_action_clients, action_client_nums, true, + it->first.c_str(), "/goal", it->second.c_str(), "ActionGoal")) { + continue; + } + + // check for action server + else if (is_action_topic(active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/feedback", it->second.c_str(), + "ActionFeedback")) { + continue; + } + if (is_action_topic(active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/result", it->second.c_str(), "ActionResult")) { + continue; + } else if (is_action_topic(active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/status", it->second.c_str(), + "GoalStatusArray")) { + continue; + } + } + + // subscribers do not report their types, but use it to confirm action + for (it = subscribers.begin(); it != subscribers.end(); it++) { + // check for action server + if (is_action_topic(active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/cancel", it->second.c_str(), "")) { + continue; + } else if (is_action_topic(active_ros1_action_servers, action_server_nums, true, + it->first.c_str(), "/goal", it->second.c_str(), "")) { + continue; + } + + // check for action client + else if (is_action_topic(active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/feedback", it->second.c_str(), "")) { + continue; + } else if (is_action_topic(active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/result", it->second.c_str(), "")) { + continue; + } else if (is_action_topic(active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/status", it->second.c_str(), "")) { + continue; + } + } + + for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { + if (it_num->second != 5) { + active_ros1_action_clients.erase(it_num->first); + } + } + for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { + if (it_num->second != 5) { + active_ros1_action_servers.erase(it_num->first); + } + } +} + +// how does ros2 action list determine active interfaces? +// ref: opt/ros/foxy/lib/python3.8/site-packages/ros2action/verb/list.py +// https://github.com/ros2/rcl/blob/master/rcl_action/src/rcl_action/graph.c +void get_active_ros2_actions(const std::map active_ros2_publishers, + const std::map active_ros2_subscribers, + std::map &active_ros2_action_servers, + std::map &active_ros2_action_clients) { + std::map::const_iterator it; + std::map::iterator it_num; + std::map action_server_nums, action_client_nums; + for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { + if (is_action_topic(active_ros2_action_servers, action_server_nums, true, it->first.c_str(), + "/_action/feedback", it->second.c_str(), "_FeedbackMessage")) { + continue; + } else if (is_action_topic(active_ros2_action_servers, action_server_nums, false, + it->first.c_str(), "/_action/status", it->second.c_str(), + "GoalStatusArray")) { + continue; + } + } + for (it = active_ros2_subscribers.begin(); it != active_ros2_subscribers.end(); it++) { + if (is_action_topic(active_ros2_action_clients, action_client_nums, true, + it->first.c_str(), "/_action/feedback", it->second.c_str(), + "_FeedbackMessage")) { + continue; + } else if (is_action_topic(active_ros2_action_clients, action_client_nums, false, + it->first.c_str(), "/_action/status", it->second.c_str(), + "GoalStatusArray")) { + continue; + } + } + for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { + printf("%d", it_num->second); + if (it_num->second != 2) { + active_ros2_action_clients.erase(it_num->first); + } + } + for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { + if (it_num->second != 2) { + active_ros2_action_servers.erase(it_num->first); + } + } + for (it = active_ros2_action_servers.begin(); it != active_ros2_action_servers.end(); it++) { + printf("ROS2 action server: %s %s\n", it->first.c_str(), it->second.c_str()); + } + for (it = active_ros2_action_clients.begin(); it != active_ros2_action_clients.end(); it++) { + printf("ROS2 action client: %s %s\n", it->first.c_str(), it->second.c_str()); + } +} + int main(int argc, char * argv[]) { bool output_topic_introspection; @@ -609,6 +763,11 @@ int main(int argc, char * argv[]) } } + // check actions + std::map active_ros1_action_servers, active_ros1_action_clients; + get_active_ros1_actions(current_ros1_publishers, current_ros1_subscribers, + active_ros1_action_servers, active_ros1_action_clients); + if (output_topic_introspection) { printf("\n"); } @@ -768,6 +927,10 @@ int main(int argc, char * argv[]) } } + std::map active_ros2_action_servers, active_ros2_action_clients; + get_active_ros2_actions(current_ros2_publishers, current_ros2_subscribers, + active_ros2_action_servers, active_ros2_action_clients); + { std::lock_guard lock(g_bridge_mutex); ros2_services = active_ros2_services; From 0210c17bf75780424dd082501e389e795ff1b6cc Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 23 Nov 2020 18:45:11 +0100 Subject: [PATCH 18/43] remove prints Signed-off-by: Harsh Deshpande --- src/dynamic_bridge.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index c451975f..4ccd737a 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -602,7 +602,6 @@ void get_active_ros2_actions(const std::map active_ros } } for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { - printf("%d", it_num->second); if (it_num->second != 2) { active_ros2_action_clients.erase(it_num->first); } @@ -612,12 +611,6 @@ void get_active_ros2_actions(const std::map active_ros active_ros2_action_servers.erase(it_num->first); } } - for (it = active_ros2_action_servers.begin(); it != active_ros2_action_servers.end(); it++) { - printf("ROS2 action server: %s %s\n", it->first.c_str(), it->second.c_str()); - } - for (it = active_ros2_action_clients.begin(); it != active_ros2_action_clients.end(); it++) { - printf("ROS2 action client: %s %s\n", it->first.c_str(), it->second.c_str()); - } } int main(int argc, char * argv[]) From b256b9fe4751fd9f8a9c616eb1488e7a06e45da7 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 24 Nov 2020 14:37:46 +0100 Subject: [PATCH 19/43] fix mapping rules for services Signed-off-by: Harsh Deshpande --- resource/interface_factories.cpp.em | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index a5a03874..b831b47f 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -364,10 +364,14 @@ void ServiceFactory< auto & @(field["ros2"]["name"])2 = req2.@(field["ros2"]["name"]); @[ end if]@ @[ if field["basic"]]@ - @(field["ros2"]["name"])@(to) = @(field["ros1"]["name"])@(frm); +@[ if field["ros2"]["type"].startswith("builtin_interfaces") ]@ + ros1_bridge::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); +@[ else]@ + @(field["ros" + to]["name"])@(to) = @(field["ros" + frm]["name"])@(frm); +@[ end if]@ @[ else]@ Factory<@(field["ros1"]["cpptype"]),@(field["ros2"]["cpptype"])>::convert_@(frm)_to_@(to)(@ -@(field["ros2"]["name"])@(frm), @(field["ros1"]["name"])@(to)); +@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); @[ end if]@ @[ if field["array"]]@ } From aafcf3a0cfbfb7b48fd3a8e3044ddff7df6fc61f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 26 May 2021 10:07:06 +0200 Subject: [PATCH 20/43] reformatted with uncrustify Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 128 ++++------- src/dynamic_bridge.cpp | 287 ++++++++++++++----------- 2 files changed, 207 insertions(+), 208 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index b4d55522..881ada3b 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -50,32 +50,41 @@ template class ActionFactory_1_2 : public ActionFactoryInterface { public: - using ROS1Server = typename actionlib::ActionServer; - using ROS1GoalHandle = typename actionlib::ActionServer::GoalHandle; - using ROS1Goal = typename actionlib::ActionServer::Goal; - using ROS1Feedback = typename actionlib::ActionServer::Feedback; - using ROS1Result = typename actionlib::ActionServer::Result; - using ROS2Goal = typename ROS2_T::Goal; - using ROS2Feedback = typename ROS2_T::Feedback; - using ROS2Result = typename ROS2_T::Result; - using ROS2ClientGoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; - using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; - using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; - using ROS2GoalHandle = typename rclcpp_action::Client::GoalHandle::SharedPtr; - - virtual void create_server_client(ros::NodeHandle ros1_node, - rclcpp::Node::SharedPtr ros2_node, - const std::string action_name) - { - this->ros1_node_ = ros1_node; - this->ros2_node_ = ros2_node; - - server_ = std::make_shared(ros1_node, action_name, - std::bind(&ActionFactory_1_2::goal_cb, this, std::placeholders::_1), - std::bind(&ActionFactory_1_2::cancel_cb, this, std::placeholders::_1), - false); - server_->start(); - client_ = rclcpp_action::create_client(ros2_node, action_name); + using ROS1Server = typename actionlib::ActionServer; + using ROS1GoalHandle = typename actionlib::ActionServer::GoalHandle; + using ROS1Goal = typename actionlib::ActionServer::Goal; + using ROS1Feedback = typename actionlib::ActionServer::Feedback; + using ROS1Result = typename actionlib::ActionServer::Result; + using ROS2Goal = typename ROS2_T::Goal; + using ROS2Feedback = typename ROS2_T::Feedback; + using ROS2Result = typename ROS2_T::Result; + using ROS2ClientGoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; + using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; + using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + using ROS2GoalHandle = typename rclcpp_action::Client::GoalHandle::SharedPtr; + + virtual void create_server_client( + ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) + { + this->ros1_node_ = ros1_node; + this->ros2_node_ = ros2_node; + + server_ = std::make_shared( + ros1_node, action_name, + std::bind(&ActionFactory_1_2::goal_cb, this, std::placeholders::_1), + std::bind(&ActionFactory_1_2::cancel_cb, this, std::placeholders::_1), false); + server_->start(); + client_ = rclcpp_action::create_client(ros2_node, action_name); + } + + void cancel_cb(ROS1GoalHandle gh1) + { + // try to find goal and cancel it + std::lock_guard lock(mutex_); + auto it = goals_.find(gh1.getGoalID().id); + if (it != goals_.end()) { + std::thread([handler = it->second]() mutable {handler->cancel();}).detach(); } } @@ -127,10 +136,10 @@ class ActionFactory_1_2 : public ActionFactoryInterface // goal_response_callback signature changed after foxy, this implementation // works with both - std::shared_future gh2_future; + std::shared_future gh2_future; // Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); - send_goal_ops.goal_response_callback = [this, &gh2_future](auto gh2) mutable { + send_goal_ops.goal_response_callback = [this, &gh2_future](ROS2GoalHandle gh2) mutable { auto goal_handle = gh2_future.get(); if (!goal_handle) { gh1_.setRejected(); // goal was not accepted by remote server @@ -143,53 +152,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::lock_guard lock(mutex_); gh2_ = goal_handle; - // goal_response_callback signature changed after foxy, this implementation - // works with both - std::shared_future gh2_future; - //Changes as per Dashing - auto send_goal_ops = ROS2SendGoalOptions(); - send_goal_ops.goal_response_callback = - [this, &gh2_future](ROS2GoalHandle gh2) mutable { - auto goal_handle = gh2_future.get(); - if (!goal_handle) - { - gh1_.setRejected(); // goal was not accepted by remote server - return; - } - - gh1_.setAccepted(); - - { - std::lock_guard lock(mutex_); - gh2_ = goal_handle; - - if (canceled_) - { // cancel was called in between - auto fut = client_->async_cancel_goal(gh2_); - } - } - }; - - send_goal_ops.feedback_callback = - [this](ROS2ClientGoalHandle, auto feedback2) mutable { - ROS1Feedback feedback1; - translate_feedback_2_to_1(feedback1, *feedback2); - gh1_.publishFeedback(feedback1); - }; - - // send goal to ROS2 server, set-up feedback - gh2_future = client_->async_send_goal(goal2, send_goal_ops); - - auto future_result = client_->async_get_result(gh2_future.get()); - auto res2 = future_result.get(); - - ROS1Result res1; - translate_result_2_to_1(res1, *(res2.result)); - - std::lock_guard lock(mutex_); - if (res2.code == rclcpp_action::ResultCode::SUCCEEDED) - { - gh1_.setSucceeded(res1); + if (canceled_) { // cancel was called in between + auto fut = client_->async_cancel_goal(gh2_); } } }; @@ -222,13 +186,13 @@ class ActionFactory_1_2 : public ActionFactoryInterface GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} - private: - ROS1GoalHandle gh1_; - ROS2ClientGoalHandle gh2_; - ROS2ClientSharedPtr client_; - bool canceled_; // cancel was called - std::mutex mutex_; - }; +private: + ROS1GoalHandle gh1_; + ROS2ClientGoalHandle gh2_; + ROS2ClientSharedPtr client_; + bool canceled_; // cancel was called + std::mutex mutex_; + }; ros::NodeHandle ros1_node_; rclcpp::Node::SharedPtr ros2_node_; diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 4ccd737a..8c36a5dc 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -468,149 +468,182 @@ void get_ros1_service_info( ros1_services[key]["name"] = std::string(t.begin() + t.find("/") + 1, t.end()); } -inline bool is_action_topic(std::map &actions, - std::map &action_nums, const bool is_action_type, - const std::string topic_name, const std::string topic_name_ends_with, - const std::string type, const std::string type_ends_with) { - // check if the topic name and topic types are as expected - if (boost::algorithm::ends_with(topic_name.c_str(), topic_name_ends_with.c_str()) && - boost::algorithm::ends_with(type.c_str(), type_ends_with.c_str())) { - // extract action name from topic name - std::string name = topic_name.substr(0, topic_name.find(topic_name_ends_with.c_str())); - if (actions.find(name) == actions.end()) { - actions[name] = ""; - action_nums[name] = 0; - } - - // e.g.: topic type of '/fibonacci/goal' is 'actionlib_tutorials/FibonacciActionGoal' - // Thus, action type is 'actionlib_tutorials/Fibonacci' - std::string action_type = type.substr(0, type.find(type_ends_with.c_str())); - actions[name] = is_action_type ? action_type : actions[name]; - action_nums[name] += 1; - return true; +inline bool is_action_topic( + std::map & actions, + std::map & action_nums, const bool is_action_type, + const std::string topic_name, const std::string topic_name_ends_with, + const std::string type, const std::string type_ends_with) +{ + // check if the topic name and topic types are as expected + if (boost::algorithm::ends_with(topic_name.c_str(), topic_name_ends_with.c_str()) && + boost::algorithm::ends_with(type.c_str(), type_ends_with.c_str())) + { + // extract action name from topic name + std::string name = topic_name.substr(0, topic_name.find(topic_name_ends_with.c_str())); + if (actions.find(name) == actions.end()) { + actions[name] = ""; + action_nums[name] = 0; } - return false; + + // e.g.: topic type of '/fibonacci/goal' is 'actionlib_tutorials/FibonacciActionGoal' + // Thus, action type is 'actionlib_tutorials/Fibonacci' + std::string action_type = type.substr(0, type.find(type_ends_with.c_str())); + actions[name] = is_action_type ? action_type : actions[name]; + action_nums[name] += 1; + return true; + } + return false; } // if topics 'goal' with type 'ActionGoal' and 'cancel' with type 'GoalID' are pubs, then it is an // action client // equivalent ROS2 action pkg and type can be retrieved from get_mappings.cpp -void get_active_ros1_actions(std::map publishers, - std::map subscribers, - std::map &active_ros1_action_servers, - std::map &active_ros1_action_clients) { - // check if the topics end with 'goal', 'result', 'cancel', 'status' - - // find topics that end with goal and cancel, find corresponding result, status and feedback - // in the other map - std::map::iterator it; - std::map::iterator it_num; - // store count of pubs and subs for each action - std::map action_server_nums, action_client_nums; - - for (it = publishers.begin(); it != publishers.end(); it++) { - // check for action client - if (is_action_topic(active_ros1_action_clients, action_client_nums, false, - it->first.c_str(), "/cancel", it->second.c_str(), "/GoalID")) { - continue; - } else if (is_action_topic(active_ros1_action_clients, action_client_nums, true, - it->first.c_str(), "/goal", it->second.c_str(), "ActionGoal")) { - continue; - } - - // check for action server - else if (is_action_topic(active_ros1_action_servers, action_server_nums, false, - it->first.c_str(), "/feedback", it->second.c_str(), - "ActionFeedback")) { - continue; - } - if (is_action_topic(active_ros1_action_servers, action_server_nums, false, - it->first.c_str(), "/result", it->second.c_str(), "ActionResult")) { - continue; - } else if (is_action_topic(active_ros1_action_servers, action_server_nums, false, - it->first.c_str(), "/status", it->second.c_str(), - "GoalStatusArray")) { - continue; - } +void get_active_ros1_actions( + std::map publishers, + std::map subscribers, + std::map & active_ros1_action_servers, + std::map & active_ros1_action_clients) +{ + // check if the topics end with 'goal', 'result', 'cancel', 'status' + + // find topics that end with goal and cancel, find corresponding result, status and feedback + // in the other map + std::map::iterator it; + std::map::iterator it_num; + // store count of pubs and subs for each action + std::map action_server_nums, action_client_nums; + + for (it = publishers.begin(); it != publishers.end(); it++) { + // check for action client + if (is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/cancel", it->second.c_str(), "/GoalID")) + { + continue; + } else if (is_action_topic( + active_ros1_action_clients, action_client_nums, true, + it->first.c_str(), "/goal", it->second.c_str(), "ActionGoal")) + { + continue; } + // check for action server + else if (is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/feedback", it->second.c_str(), + "ActionFeedback")) + { + continue; + } + if (is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/result", it->second.c_str(), "ActionResult")) + { + continue; + } else if (is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/status", it->second.c_str(), + "GoalStatusArray")) + { + continue; + } + } - // subscribers do not report their types, but use it to confirm action - for (it = subscribers.begin(); it != subscribers.end(); it++) { - // check for action server - if (is_action_topic(active_ros1_action_servers, action_server_nums, false, - it->first.c_str(), "/cancel", it->second.c_str(), "")) { - continue; - } else if (is_action_topic(active_ros1_action_servers, action_server_nums, true, - it->first.c_str(), "/goal", it->second.c_str(), "")) { - continue; - } - - // check for action client - else if (is_action_topic(active_ros1_action_clients, action_client_nums, false, - it->first.c_str(), "/feedback", it->second.c_str(), "")) { - continue; - } else if (is_action_topic(active_ros1_action_clients, action_client_nums, false, - it->first.c_str(), "/result", it->second.c_str(), "")) { - continue; - } else if (is_action_topic(active_ros1_action_clients, action_client_nums, false, - it->first.c_str(), "/status", it->second.c_str(), "")) { - continue; - } + // subscribers do not report their types, but use it to confirm action + for (it = subscribers.begin(); it != subscribers.end(); it++) { + // check for action server + if (is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/cancel", it->second.c_str(), "")) + { + continue; + } else if (is_action_topic( + active_ros1_action_servers, action_server_nums, true, + it->first.c_str(), "/goal", it->second.c_str(), "")) + { + continue; + } + // check for action client + else if (is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/feedback", it->second.c_str(), "")) + { + continue; + } else if (is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/result", it->second.c_str(), "")) + { + continue; + } else if (is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/status", it->second.c_str(), "")) + { + continue; } + } - for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { - if (it_num->second != 5) { - active_ros1_action_clients.erase(it_num->first); - } + for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { + if (it_num->second != 5) { + active_ros1_action_clients.erase(it_num->first); } - for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { - if (it_num->second != 5) { - active_ros1_action_servers.erase(it_num->first); - } + } + for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { + if (it_num->second != 5) { + active_ros1_action_servers.erase(it_num->first); } + } } // how does ros2 action list determine active interfaces? // ref: opt/ros/foxy/lib/python3.8/site-packages/ros2action/verb/list.py // https://github.com/ros2/rcl/blob/master/rcl_action/src/rcl_action/graph.c -void get_active_ros2_actions(const std::map active_ros2_publishers, - const std::map active_ros2_subscribers, - std::map &active_ros2_action_servers, - std::map &active_ros2_action_clients) { - std::map::const_iterator it; - std::map::iterator it_num; - std::map action_server_nums, action_client_nums; - for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { - if (is_action_topic(active_ros2_action_servers, action_server_nums, true, it->first.c_str(), - "/_action/feedback", it->second.c_str(), "_FeedbackMessage")) { - continue; - } else if (is_action_topic(active_ros2_action_servers, action_server_nums, false, - it->first.c_str(), "/_action/status", it->second.c_str(), - "GoalStatusArray")) { - continue; - } +void get_active_ros2_actions( + const std::map active_ros2_publishers, + const std::map active_ros2_subscribers, + std::map & active_ros2_action_servers, + std::map & active_ros2_action_clients) +{ + std::map::const_iterator it; + std::map::iterator it_num; + std::map action_server_nums, action_client_nums; + for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { + if (is_action_topic( + active_ros2_action_servers, action_server_nums, true, it->first.c_str(), + "/_action/feedback", it->second.c_str(), "_FeedbackMessage")) + { + continue; + } else if (is_action_topic( + active_ros2_action_servers, action_server_nums, false, + it->first.c_str(), "/_action/status", it->second.c_str(), + "GoalStatusArray")) + { + continue; } - for (it = active_ros2_subscribers.begin(); it != active_ros2_subscribers.end(); it++) { - if (is_action_topic(active_ros2_action_clients, action_client_nums, true, - it->first.c_str(), "/_action/feedback", it->second.c_str(), - "_FeedbackMessage")) { - continue; - } else if (is_action_topic(active_ros2_action_clients, action_client_nums, false, - it->first.c_str(), "/_action/status", it->second.c_str(), - "GoalStatusArray")) { - continue; - } + } + for (it = active_ros2_subscribers.begin(); it != active_ros2_subscribers.end(); it++) { + if (is_action_topic( + active_ros2_action_clients, action_client_nums, true, + it->first.c_str(), "/_action/feedback", it->second.c_str(), + "_FeedbackMessage")) + { + continue; + } else if (is_action_topic( + active_ros2_action_clients, action_client_nums, false, + it->first.c_str(), "/_action/status", it->second.c_str(), + "GoalStatusArray")) + { + continue; } - for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { - if (it_num->second != 2) { - active_ros2_action_clients.erase(it_num->first); - } + } + for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { + if (it_num->second != 2) { + active_ros2_action_clients.erase(it_num->first); } - for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { - if (it_num->second != 2) { - active_ros2_action_servers.erase(it_num->first); - } + } + for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { + if (it_num->second != 2) { + active_ros2_action_servers.erase(it_num->first); } + } } int main(int argc, char * argv[]) @@ -758,8 +791,9 @@ int main(int argc, char * argv[]) // check actions std::map active_ros1_action_servers, active_ros1_action_clients; - get_active_ros1_actions(current_ros1_publishers, current_ros1_subscribers, - active_ros1_action_servers, active_ros1_action_clients); + get_active_ros1_actions( + current_ros1_publishers, current_ros1_subscribers, + active_ros1_action_servers, active_ros1_action_clients); if (output_topic_introspection) { printf("\n"); @@ -921,8 +955,9 @@ int main(int argc, char * argv[]) } std::map active_ros2_action_servers, active_ros2_action_clients; - get_active_ros2_actions(current_ros2_publishers, current_ros2_subscribers, - active_ros2_action_servers, active_ros2_action_clients); + get_active_ros2_actions( + current_ros2_publishers, current_ros2_subscribers, + active_ros2_action_servers, active_ros2_action_clients); { std::lock_guard lock(g_bridge_mutex); From 9c83a9331e3022033864761e7da7ef95a61f6408 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sun, 6 Jun 2021 18:26:22 +0200 Subject: [PATCH 21/43] map actions in dynamic_bridge Signed-off-by: Harsh Deshpande --- src/dynamic_bridge.cpp | 149 +++++++++++++++++++++++++++++++++++------ 1 file changed, 130 insertions(+), 19 deletions(-) diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 8c36a5dc..3100cb2b 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -143,10 +143,16 @@ void update_bridge( const std::map & ros2_subscribers, const std::map> & ros1_services, const std::map> & ros2_services, + const std::map> & ros1_action_servers, + const std::map> & ros1_action_clients, + const std::map> & ros2_action_servers, + const std::map> & ros2_action_clients, std::map & bridges_1to2, std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, + std::map> & action_bridges_1_to_2, + std::map> & action_bridges_2_to_1, bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) { std::lock_guard lock(g_bridge_mutex); @@ -392,6 +398,79 @@ void update_bridge( ++it; } } + + // create bridges for ros1 actions + for (auto & ros1_action : ros1_action_servers) { + auto & name = ros1_action.first; + auto & details = ros1_action.second; + if ( + action_bridges_1_to_2.find(name) == action_bridges_1_to_2.end() && + action_bridges_2_to_1.find(name) == action_bridges_2_to_1.end()) + { + auto factory = ros1_bridge::get_action_factory( + "ros1", details.at("package"), details.at("type")); + if (factory) { + try { + action_bridges_2_to_1[name].reset(factory.get()); + factory->create_server_client(ros1_node, ros2_node, name); + printf("Created 2 to 1 bridge for action %s\n", name.data()); + } catch (std::runtime_error & e) { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } + } + } + + // create bridges for ros2 actions + for (auto & ros2_action : ros2_action_servers) { + auto & name = ros2_action.first; + auto & details = ros2_action.second; + if ( + action_bridges_1_to_2.find(name) == action_bridges_1_to_2.end() && + action_bridges_2_to_1.find(name) == action_bridges_2_to_1.end()) + { + auto factory = ros1_bridge::get_action_factory( + "ros2", details.at("package"), details.at("type")); + if (factory) { + try { + action_bridges_1_to_2[name].reset(factory.get()); + factory->create_server_client(ros1_node, ros2_node, name); + printf("Created 1 to 2 bridge for action %s\n", name.data()); + } catch (std::runtime_error & e) { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } + } + } + + // remove obsolete ros1 actions + for (auto it = action_bridges_2_to_1.begin(); it != action_bridges_2_to_1.end(); ) { + if (ros1_action_servers.find(it->first) == ros1_action_servers.end()) { + printf("Removed 2 to 1 bridge for action %s\n", it->first.data()); + try { + it = action_bridges_2_to_1.erase(it); + } catch (std::runtime_error & e) { + fprintf(stderr, "There was an error while removing 2 to 1 bridge: %s\n", e.what()); + } + } else { + ++it; + } + } + + // remove obsolete ros2 actions + for (auto it = action_bridges_1_to_2.begin(); it != action_bridges_1_to_2.end(); ) { + if (ros2_action_servers.find(it->first) == ros2_action_servers.end()) { + printf("Removed 1 to 2 bridge for action %s\n", it->first.data()); + try { + // it->second.server.shutdown(); + it = action_bridges_1_to_2.erase(it); + } catch (std::runtime_error & e) { + fprintf(stderr, "There was an error while removing 1 to 2 bridge: %s\n", e.what()); + } + } else { + ++it; + } + } } void get_ros1_service_info( @@ -469,10 +548,10 @@ void get_ros1_service_info( } inline bool is_action_topic( - std::map & actions, + std::map> & actions, std::map & action_nums, const bool is_action_type, const std::string topic_name, const std::string topic_name_ends_with, - const std::string type, const std::string type_ends_with) + const std::string type, const std::string type_ends_with, bool is_ros2 = false) { // check if the topic name and topic types are as expected if (boost::algorithm::ends_with(topic_name.c_str(), topic_name_ends_with.c_str()) && @@ -481,15 +560,27 @@ inline bool is_action_topic( // extract action name from topic name std::string name = topic_name.substr(0, topic_name.find(topic_name_ends_with.c_str())); if (actions.find(name) == actions.end()) { - actions[name] = ""; + actions[name]["package"] = ""; + actions[name]["type"] = ""; action_nums[name] = 0; } // e.g.: topic type of '/fibonacci/goal' is 'actionlib_tutorials/FibonacciActionGoal' - // Thus, action type is 'actionlib_tutorials/Fibonacci' - std::string action_type = type.substr(0, type.find(type_ends_with.c_str())); - actions[name] = is_action_type ? action_type : actions[name]; + // Thus, package name is action type is 'actionlib_tutorials' and + // action type is 'Fibonacci' + if (!type.empty() && is_action_type) { + std::string pkg_name = type.substr(0, type.find("/")); + std::string action_type = type.substr(type.find_last_of("/") + 1, type.length() - (type.find_last_of("/") + type_ends_with.length() + 1)); + actions[name]["package"] = pkg_name; + if(is_ros2) { + actions[name]["type"] = "action/" + action_type; + } else { + actions[name]["type"] = action_type; + } + } + action_nums[name] += 1; + return true; } return false; @@ -501,8 +592,8 @@ inline bool is_action_topic( void get_active_ros1_actions( std::map publishers, std::map subscribers, - std::map & active_ros1_action_servers, - std::map & active_ros1_action_clients) + std::map> & active_ros1_action_servers, + std::map> & active_ros1_action_clients) { // check if the topics end with 'goal', 'result', 'cancel', 'status' @@ -528,7 +619,7 @@ void get_active_ros1_actions( } // check for action server else if (is_action_topic( - active_ros1_action_servers, action_server_nums, false, + active_ros1_action_servers, action_server_nums, true, it->first.c_str(), "/feedback", it->second.c_str(), "ActionFeedback")) { @@ -557,7 +648,7 @@ void get_active_ros1_actions( { continue; } else if (is_action_topic( - active_ros1_action_servers, action_server_nums, true, + active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/goal", it->second.c_str(), "")) { continue; @@ -599,8 +690,8 @@ void get_active_ros1_actions( void get_active_ros2_actions( const std::map active_ros2_publishers, const std::map active_ros2_subscribers, - std::map & active_ros2_action_servers, - std::map & active_ros2_action_clients) + std::map> & active_ros2_action_servers, + std::map> & active_ros2_action_clients) { std::map::const_iterator it; std::map::iterator it_num; @@ -608,13 +699,13 @@ void get_active_ros2_actions( for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { if (is_action_topic( active_ros2_action_servers, action_server_nums, true, it->first.c_str(), - "/_action/feedback", it->second.c_str(), "_FeedbackMessage")) + "/_action/feedback", it->second.c_str(), "_FeedbackMessage", true)) { continue; } else if (is_action_topic( active_ros2_action_servers, action_server_nums, false, it->first.c_str(), "/_action/status", it->second.c_str(), - "GoalStatusArray")) + "GoalStatusArray"), true) { continue; } @@ -623,13 +714,13 @@ void get_active_ros2_actions( if (is_action_topic( active_ros2_action_clients, action_client_nums, true, it->first.c_str(), "/_action/feedback", it->second.c_str(), - "_FeedbackMessage")) + "_FeedbackMessage", true)) { continue; } else if (is_action_topic( active_ros2_action_clients, action_client_nums, false, it->first.c_str(), "/_action/status", it->second.c_str(), - "GoalStatusArray")) + "GoalStatusArray", true)) { continue; } @@ -644,6 +735,7 @@ void get_active_ros2_actions( active_ros2_action_servers.erase(it_num->first); } } + } int main(int argc, char * argv[]) @@ -673,11 +765,16 @@ int main(int argc, char * argv[]) std::map ros2_subscribers; std::map> ros1_services; std::map> ros2_services; - + std::map> ros1_action_servers; + std::map> ros1_action_clients; + std::map> ros2_action_servers; + std::map> ros2_action_clients; std::map bridges_1to2; std::map bridges_2to1; std::map service_bridges_1_to_2; std::map service_bridges_2_to_1; + std::map> action_bridges_1_to_2; + std::map> action_bridges_2_to_1; // setup polling of ROS 1 master auto ros1_poll = [ @@ -686,7 +783,10 @@ int main(int argc, char * argv[]) &ros2_publishers, &ros2_subscribers, &bridges_1to2, &bridges_2to1, &ros1_services, &ros2_services, + &ros1_action_servers, &ros1_action_clients, + &ros2_action_servers, &ros2_action_clients, &service_bridges_1_to_2, &service_bridges_2_to_1, + &action_bridges_1_to_2, &action_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics ](const ros::TimerEvent &) -> void @@ -790,7 +890,7 @@ int main(int argc, char * argv[]) } // check actions - std::map active_ros1_action_servers, active_ros1_action_clients; + std::map> active_ros1_action_servers, active_ros1_action_clients; get_active_ros1_actions( current_ros1_publishers, current_ros1_subscribers, active_ros1_action_servers, active_ros1_action_clients); @@ -810,8 +910,11 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, + ros1_action_servers, ros1_action_clients, + ros2_action_servers, ros2_action_clients, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, + action_bridges_1_to_2, action_bridges_2_to_1, bridge_all_1to2_topics, bridge_all_2to1_topics); }; @@ -827,8 +930,11 @@ int main(int argc, char * argv[]) &ros1_publishers, &ros1_subscribers, &ros2_publishers, &ros2_subscribers, &ros1_services, &ros2_services, + &ros1_action_servers, &ros1_action_clients, + &ros2_action_servers, &ros2_action_clients, &bridges_1to2, &bridges_2to1, &service_bridges_1_to_2, &service_bridges_2_to_1, + &action_bridges_1_to_2, &action_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics, &already_ignored_topics, &already_ignored_services @@ -954,7 +1060,7 @@ int main(int argc, char * argv[]) } } - std::map active_ros2_action_servers, active_ros2_action_clients; + std::map> active_ros2_action_servers, active_ros2_action_clients; get_active_ros2_actions( current_ros2_publishers, current_ros2_subscribers, active_ros2_action_servers, active_ros2_action_clients); @@ -962,6 +1068,8 @@ int main(int argc, char * argv[]) { std::lock_guard lock(g_bridge_mutex); ros2_services = active_ros2_services; + ros2_action_servers = active_ros2_action_servers; + ros2_action_clients = active_ros2_action_clients; } if (output_topic_introspection) { @@ -979,8 +1087,11 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, + ros1_action_servers, ros1_action_clients, + ros2_action_servers, ros2_action_clients, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, + action_bridges_1_to_2, action_bridges_2_to_1, bridge_all_1to2_topics, bridge_all_2to1_topics); }; From bc350f4b2f5f32afb471a70ea56962491c81564b Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 7 Jun 2021 11:56:54 +0200 Subject: [PATCH 22/43] shutdown internal server before removing the bridge Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 16 ++++++++++++++++ include/ros1_bridge/factory_interface.hpp | 2 ++ src/dynamic_bridge.cpp | 15 +++++++++++++-- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 881ada3b..af2f2b83 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -78,6 +78,14 @@ class ActionFactory_1_2 : public ActionFactoryInterface client_ = rclcpp_action::create_client(ros2_node, action_name); } + virtual void shutdown() { + std::lock_guard lock(mutex_); + for (auto goal : goals_) { + std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); + } + server_.reset(); + } + void cancel_cb(ROS1GoalHandle gh1) { // try to find goal and cancel it @@ -245,6 +253,14 @@ class ActionFactory_2_1 : public ActionFactoryInterface std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); } + virtual void shutdown() { + std::lock_guard lock(mutex_); + for (auto goal : goals_) { + std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); + } + server_.reset(); + } + // ROS2 callbacks rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index c07cde82..8da33498 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -151,6 +151,8 @@ class ActionFactoryInterface ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string action_name) = 0; + + virtual void shutdown() = 0; }; } // namespace ros1_bridge diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 3100cb2b..4773a78f 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -411,8 +411,8 @@ void update_bridge( "ros1", details.at("package"), details.at("type")); if (factory) { try { - action_bridges_2_to_1[name].reset(factory.get()); factory->create_server_client(ros1_node, ros2_node, name); + action_bridges_2_to_1[name] = std::move(factory); printf("Created 2 to 1 bridge for action %s\n", name.data()); } catch (std::runtime_error & e) { fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); @@ -433,8 +433,8 @@ void update_bridge( "ros2", details.at("package"), details.at("type")); if (factory) { try { - action_bridges_1_to_2[name].reset(factory.get()); factory->create_server_client(ros1_node, ros2_node, name); + action_bridges_1_to_2[name] = std::move(factory); printf("Created 1 to 2 bridge for action %s\n", name.data()); } catch (std::runtime_error & e) { fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); @@ -448,6 +448,8 @@ void update_bridge( if (ros1_action_servers.find(it->first) == ros1_action_servers.end()) { printf("Removed 2 to 1 bridge for action %s\n", it->first.data()); try { + it->second->shutdown(); + it->second.reset(); it = action_bridges_2_to_1.erase(it); } catch (std::runtime_error & e) { fprintf(stderr, "There was an error while removing 2 to 1 bridge: %s\n", e.what()); @@ -463,6 +465,8 @@ void update_bridge( printf("Removed 1 to 2 bridge for action %s\n", it->first.data()); try { // it->second.server.shutdown(); + it->second->shutdown(); + it->second.reset(); it = action_bridges_1_to_2.erase(it); } catch (std::runtime_error & e) { fprintf(stderr, "There was an error while removing 1 to 2 bridge: %s\n", e.what()); @@ -895,6 +899,13 @@ int main(int argc, char * argv[]) current_ros1_publishers, current_ros1_subscribers, active_ros1_action_servers, active_ros1_action_clients); + { + std::lock_guard lock(g_bridge_mutex); + ros1_services = active_ros1_services; + ros1_action_servers = active_ros1_action_servers; + ros1_action_clients = active_ros1_action_clients; + } + if (output_topic_introspection) { printf("\n"); } From deb5805754f12fd091a4e5d5d60efdb656a8fa8f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 12 Jan 2022 18:17:52 +0100 Subject: [PATCH 23/43] updated GoalResponseCallback signature Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index af2f2b83..06bbaf9c 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -147,7 +147,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::shared_future gh2_future; // Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); - send_goal_ops.goal_response_callback = [this, &gh2_future](ROS2GoalHandle gh2) mutable { + send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { auto goal_handle = gh2_future.get(); if (!goal_handle) { gh1_.setRejected(); // goal was not accepted by remote server From 69c815bf9530ccc61afe9687cbdd08f81db44d13 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 12 Jan 2022 21:52:32 +0100 Subject: [PATCH 24/43] updated README Signed-off-by: Harsh Deshpande --- README.md | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/README.md b/README.md index 59d9509f..49263fdf 100644 --- a/README.md +++ b/README.md @@ -502,3 +502,51 @@ topics: ``` Note that the `qos` section can be omitted entirely and options not set are left default. + +# Action bridge + +This bridge extends the `ros1_bridge` to action interface. The bridge works in both directions, meaning an action goal can be sent from ROS1 client to ROS2 server, or from ROS2 client to ROS1 server. + +The arguments for `action_bridge` node are: +`direction`: from client (`ros1` or `ros2`) +e.g.: +- `ROS1` client to `ROS2` server --> `direction` = `ros1` +- `ROS2` client to `ROS1` server --> `direction` = `ros2` + +`package`: package of the `ROS1` server node +`type`: action interface type of `ROS1` +`name`: action name + +For sending goals from ROS2 action client to ROS1 action server +``` +# Terminal 1 -- action bridge +# Make sure roscore is already running +source /setup.bash +ros2 run ros1_bridge action_bridge ros1 actionlib_tutorials Fibonacci fibonacci + +# Terminal 2 -- ROS1 action server +source /setup.bash +rosrun actionlib_tutorials fibonacci_server + +# Terminal 3 -- ROS2 action client +source /setup.bash +ros2 run action_tutorials_cpp fibonacci_action_client 20 +``` + +For sending goals from ROS1 action client to ROS2 action server +``` +# Terminal 1 -- action bridge +# Make sure roscore is already running +source /setup.bash +ros2 run ros1_bridge action_bridge ros2 action_tutorials_interfaces action/Fibonacci fibonacci + +# Terminal 2 -- ROS2 action server +source /setup.bash +ros2 run action_tutorials_cpp fibonacci_action_server + +# Terminal 3 -- ROS1 action client +source /setup.bash +rosrun actionlib_tutorials fibonacci_client 20 +``` + +`dynamic_bridge` has been extended to handle actions as well. From 1f600e01851b273dde13a8e65e1ac25e5767e78d Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Thu, 13 Jan 2022 01:43:40 +0100 Subject: [PATCH 25/43] fix formatting added NOLINT whenever both cpplint and uncrustify cannot be satisfied https://github.com/ament/ament_lint/issues/158 Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 11 +++-- ros1_bridge/__init__.py | 28 ++++++----- src/dynamic_bridge.cpp | 68 ++++++++++++++++---------- 3 files changed, 65 insertions(+), 42 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 06bbaf9c..bce04f4a 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -78,7 +78,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface client_ = rclcpp_action::create_client(ros2_node, action_name); } - virtual void shutdown() { + virtual void shutdown() + { std::lock_guard lock(mutex_); for (auto goal : goals_) { std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); @@ -147,7 +148,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::shared_future gh2_future; // Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); - send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { + send_goal_ops.goal_response_callback = + [this, &gh2_future](std::shared_future gh2) mutable { auto goal_handle = gh2_future.get(); if (!goal_handle) { gh1_.setRejected(); // goal was not accepted by remote server @@ -253,7 +255,8 @@ class ActionFactory_2_1 : public ActionFactoryInterface std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); } - virtual void shutdown() { + virtual void shutdown() + { std::lock_guard lock(mutex_); for (auto goal : goals_) { std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); @@ -391,7 +394,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface std::size_t get_goal_id_hash(const rclcpp_action::GoalUUID & uuid) { - return std::hash{} (uuid); + return std::hash{}(uuid); } ros::NodeHandle ros1_node_; diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 631e2885..69d1f61e 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -26,8 +26,8 @@ from io import StringIO # Python 3.x import genmsg -import genmsg.msg_loader from genmsg.base import COMMENTCHAR, IODELIM +import genmsg.msg_loader import rosidl_adapter.parser from rosidl_cmake import expand_template @@ -423,7 +423,7 @@ def get_ros1_actions(rospack=None): # 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"): + for action_name in rosmsg._list_types(path, 'msg', '.action'): actions.append(Message(package_name, action_name, path)) return actions @@ -462,7 +462,8 @@ def get_ros2_actions(): 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')): + 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: @@ -899,7 +900,9 @@ def determine_common_actions( # 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") and "GripperCommand" not in ros2_type: + if ((ros1_type, ros2_type) not in message_string_pairs and + not ros2_type.startswith('builtin_interfaces') and + 'GripperCommand' not in ros2_type): match = False break output[direction].append({ @@ -1112,6 +1115,7 @@ def load_ros1_service(ros1_srv): # genmsg/actions.py + class ActionSpec(object): def __init__(self, goal, result, feedback, text, full_name='', short_name='', package=''): @@ -1147,12 +1151,11 @@ def __ne__(self, other): return not self.__eq__(other) def __repr__(self): - return "ActionSpec[%s, %s, %s]" % (repr(self.goal), repr(self.result), repr(self.feedback)) + 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. @@ -1170,15 +1173,15 @@ def load_action_from_string(msg_context, text, full_name): 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 + s = l.split(COMMENTCHAR)[0].strip() # strip comments + if s.startswith(IODELIM): # lenient, by request if count == 0: accum = text_result count = 1 else: accum = text_feedback else: - accum.write(l+'\n') + accum.write(s+'\n') # create separate MsgSpec objects for each half of file msg_goal = genmsg.msg_loader.load_msg_from_string( @@ -1194,8 +1197,8 @@ def load_action_from_string(msg_context, text, full_name): 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*. + 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 @@ -1215,7 +1218,8 @@ def load_ros1_action(ros1_action): 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)) + msg_context, message_path, '%s/%s' % + (ros1_action.package_name, ros1_action.message_name)) except genmsg.InvalidMsgSpec: return None return spec @@ -1283,7 +1287,7 @@ def load_ros2_action(ros2_action): spec = rosidl_adapter.parser.parse_action_file( ros2_action.package_name, actiom_path) except rosidl_adapter.parser.InvalidSpecification: - print("Invalid spec") + print('Invalid spec') return None return spec diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 4773a78f..90255d1e 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include // NOLINT // include ROS 1 #ifdef __clang__ @@ -151,8 +151,10 @@ void update_bridge( std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, - std::map> & action_bridges_1_to_2, - std::map> & action_bridges_2_to_1, + std::map> & action_bridges_1_to_2, + std::map> & action_bridges_2_to_1, bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) { std::lock_guard lock(g_bridge_mutex); @@ -574,9 +576,12 @@ inline bool is_action_topic( // action type is 'Fibonacci' if (!type.empty() && is_action_type) { std::string pkg_name = type.substr(0, type.find("/")); - std::string action_type = type.substr(type.find_last_of("/") + 1, type.length() - (type.find_last_of("/") + type_ends_with.length() + 1)); + std::string action_type = + type.substr( + type.find_last_of("/") + 1, + type.length() - (type.find_last_of("/") + type_ends_with.length() + 1)); actions[name]["package"] = pkg_name; - if(is_ros2) { + if (is_ros2) { actions[name]["type"] = "action/" + action_type; } else { actions[name]["type"] = action_type; @@ -610,31 +615,34 @@ void get_active_ros1_actions( for (it = publishers.begin(); it != publishers.end(); it++) { // check for action client - if (is_action_topic( + if ( + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/cancel", it->second.c_str(), "/GoalID")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_clients, action_client_nums, true, it->first.c_str(), "/goal", it->second.c_str(), "ActionGoal")) { continue; - } - // check for action server - else if (is_action_topic( + } else if ( // NOLINT // check for action server + is_action_topic( active_ros1_action_servers, action_server_nums, true, it->first.c_str(), "/feedback", it->second.c_str(), "ActionFeedback")) { continue; } - if (is_action_topic( + if ( + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/result", it->second.c_str(), "ActionResult")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/status", it->second.c_str(), "GoalStatusArray")) @@ -646,29 +654,32 @@ void get_active_ros1_actions( // subscribers do not report their types, but use it to confirm action for (it = subscribers.begin(); it != subscribers.end(); it++) { // check for action server - if (is_action_topic( + if ( + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/cancel", it->second.c_str(), "")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/goal", it->second.c_str(), "")) { continue; - } - // check for action client - else if (is_action_topic( + } else if ( // NOLINT // check for action client + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/feedback", it->second.c_str(), "")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/result", it->second.c_str(), "")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/status", it->second.c_str(), "")) { @@ -701,12 +712,14 @@ void get_active_ros2_actions( std::map::iterator it_num; std::map action_server_nums, action_client_nums; for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { - if (is_action_topic( + if ( + is_action_topic( active_ros2_action_servers, action_server_nums, true, it->first.c_str(), "/_action/feedback", it->second.c_str(), "_FeedbackMessage", true)) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros2_action_servers, action_server_nums, false, it->first.c_str(), "/_action/status", it->second.c_str(), "GoalStatusArray"), true) @@ -715,13 +728,15 @@ void get_active_ros2_actions( } } for (it = active_ros2_subscribers.begin(); it != active_ros2_subscribers.end(); it++) { - if (is_action_topic( + if ( + is_action_topic( active_ros2_action_clients, action_client_nums, true, it->first.c_str(), "/_action/feedback", it->second.c_str(), "_FeedbackMessage", true)) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros2_action_clients, action_client_nums, false, it->first.c_str(), "/_action/status", it->second.c_str(), "GoalStatusArray", true)) @@ -739,7 +754,6 @@ void get_active_ros2_actions( active_ros2_action_servers.erase(it_num->first); } } - } int main(int argc, char * argv[]) @@ -894,7 +908,8 @@ int main(int argc, char * argv[]) } // check actions - std::map> active_ros1_action_servers, active_ros1_action_clients; + std::map> active_ros1_action_servers, + active_ros1_action_clients; get_active_ros1_actions( current_ros1_publishers, current_ros1_subscribers, active_ros1_action_servers, active_ros1_action_clients); @@ -1071,7 +1086,8 @@ int main(int argc, char * argv[]) } } - std::map> active_ros2_action_servers, active_ros2_action_clients; + std::map> active_ros2_action_servers, + active_ros2_action_clients; get_active_ros2_actions( current_ros2_publishers, current_ros2_subscribers, active_ros2_action_servers, active_ros2_action_clients); From 1dd38b31baf34bd852df1998188e83b58207189f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Thu, 16 Jun 2022 07:22:48 +0200 Subject: [PATCH 26/43] add actionlib dependency with condition Signed-off-by: Harsh Deshpande --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 77773553..9d038575 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ rosidl_parser builtin_interfaces + actionlib libboost-dev pkg-config python3-yaml @@ -35,6 +36,7 @@ pkg-config + actionlib builtin_interfaces python3-yaml rclcpp From a5eb300e2ddfa211e12540d2a829c409cda8e426 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 6 Jul 2022 11:16:21 +0200 Subject: [PATCH 27/43] added dependency on rclcpp_action Signed-off-by: Harsh Deshpande --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 9d038575..3cf855ce 100644 --- a/package.xml +++ b/package.xml @@ -28,6 +28,7 @@ pkg-config python3-yaml rclcpp + rclcpp_action rcpputils rcutils rmw_implementation_cmake @@ -40,6 +41,7 @@ builtin_interfaces python3-yaml rclcpp + rclcpp_action rcpputils rcutils std_msgs From 91b3197b4bf95494203b39956c997e7658fd2e18 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:49:58 +0100 Subject: [PATCH 28/43] Update CMakeLists.txt Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c361e5a9..a4e1bff4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -227,7 +227,8 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} ${prefixed_ros1_message_packages} ${ros2_interface_packages} - "rclcpp" "rclcpp_action" + "rclcpp" + "rclcpp_action" "ros1_roscpp" "ros1_std_msgs") From 51ac119cf9d0bb3a395cb7d50239af573adba4f6 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:50:43 +0100 Subject: [PATCH 29/43] Update README.md Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 49263fdf..4c75bb3d 100644 --- a/README.md +++ b/README.md @@ -505,9 +505,9 @@ Note that the `qos` section can be omitted entirely and options not set are left # Action bridge -This bridge extends the `ros1_bridge` to action interface. The bridge works in both directions, meaning an action goal can be sent from ROS1 client to ROS2 server, or from ROS2 client to ROS1 server. +This bridge extends the `ros1_bridge` to support actions. The bridge works in both directions, meaning an action goal can be sent from ROS 1 client to ROS 2 server, or from ROS 2 client to ROS 1 server. -The arguments for `action_bridge` node are: +The arguments for the `action_bridge` node are: `direction`: from client (`ros1` or `ros2`) e.g.: - `ROS1` client to `ROS2` server --> `direction` = `ros1` @@ -517,34 +517,34 @@ e.g.: `type`: action interface type of `ROS1` `name`: action name -For sending goals from ROS2 action client to ROS1 action server +For sending goals from a ROS 2 action client to a ROS 1 action server ``` # Terminal 1 -- action bridge # Make sure roscore is already running source /setup.bash ros2 run ros1_bridge action_bridge ros1 actionlib_tutorials Fibonacci fibonacci -# Terminal 2 -- ROS1 action server +# Terminal 2 -- ROS 1 action server source /setup.bash rosrun actionlib_tutorials fibonacci_server -# Terminal 3 -- ROS2 action client +# Terminal 3 -- ROS 2 action client source /setup.bash ros2 run action_tutorials_cpp fibonacci_action_client 20 ``` -For sending goals from ROS1 action client to ROS2 action server +For sending goals from a ROS 1 action client to a ROS 2 action server ``` # Terminal 1 -- action bridge # Make sure roscore is already running source /setup.bash ros2 run ros1_bridge action_bridge ros2 action_tutorials_interfaces action/Fibonacci fibonacci -# Terminal 2 -- ROS2 action server +# Terminal 2 -- ROS 2 action server source /setup.bash ros2 run action_tutorials_cpp fibonacci_action_server -# Terminal 3 -- ROS1 action client +# Terminal 3 -- ROS 1 action client source /setup.bash rosrun actionlib_tutorials fibonacci_client 20 ``` From 94404b00f00354e3e2d247e6ff8e43d95ef6e754 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:54:28 +0100 Subject: [PATCH 30/43] Update include/ros1_bridge/action_factory.hpp Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index bce04f4a..95763c67 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -122,7 +122,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface private: class GoalHandler { -public: + public: void cancel() { std::lock_guard lock(mutex_); @@ -131,6 +131,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface auto fut = client_->async_cancel_goal(gh2_); } } + void handle() { auto goal1 = gh1_.getGoal(); @@ -196,7 +197,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} -private: + private: ROS1GoalHandle gh1_; ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; @@ -314,7 +315,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface private: class GoalHandler { -public: + public: void cancel() { std::lock_guard lock(mutex_); @@ -384,7 +385,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface GoalHandler(std::shared_ptr & gh2, std::shared_ptr & client) : gh2_(gh2), client_(client), canceled_(false) {} -private: + private: std::shared_ptr gh1_; std::shared_ptr gh2_; std::shared_ptr client_; From d7fdc6de31ef47d75c1e22b9bffcd741c6138a5f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:58:25 +0100 Subject: [PATCH 31/43] Update resource/get_factory.cpp.em Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 3e8f0e48..a1745ebf 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -85,7 +85,7 @@ std::unique_ptr get_action_factory(const std::string & r } @[end for]@ // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); - return factory; + return null_ptr; } } // namespace ros1_bridge From c071edf26647ecd5306991dd068658e297d42492 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:59:41 +0100 Subject: [PATCH 32/43] Update ros1_bridge/__init__.py Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- ros1_bridge/__init__.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 69d1f61e..97dc89cb 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -796,10 +796,10 @@ def determine_common_services( ros2_name = ros2_fields[direction][i].name if ros1_name != ros2_name: # if the names do not match, check first if the types are builtin - ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) - ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type() + ros1_is_builtin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) + ros2_is_builtin = ros2_fields[direction][i].type.is_primitive_type() # Check if types are primitive types - if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type: + if not ros1_is_builtin or not ros2_is_builtin or ros1_type != ros2_type: # 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 and @@ -892,11 +892,11 @@ def determine_common_actions( # if the message types have a custom mapping their names # might not be equal, therefore check the message pairs - ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) - ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type() + ros1_is_builtin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) + ros2_is_builtin = ros2_fields[direction][i].type.is_primitive_type() # Check if types are primitive types - if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type: + if not ros1_is_builtin or not ros2_is_builtin or ros1_type != ros2_type: # the check for 'builtin_interfaces' should be removed # once merged with __init__.py # It seems to handle it already @@ -1280,12 +1280,12 @@ def load_ros2_service(ros2_srv): def load_ros2_action(ros2_action): - actiom_path = os.path.join( + action_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) + ros2_action.package_name, action_path) except rosidl_adapter.parser.InvalidSpecification: print('Invalid spec') return None From be56382acea790fc35bec9f7a8e7eb0f9a746e6d Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 09:04:16 +0100 Subject: [PATCH 33/43] Update src/action_bridge.cpp Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- src/action_bridge.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index bb0185b1..14c35d62 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -46,14 +46,12 @@ int main(int argc, char * argv[]) std::cout << dir << " " << package << " " << type << " " << name << std::endl; - // ros1 example_tutorials Fibonacci fibonacci auto factory = ros1_bridge::get_action_factory(dir, package, type); if (factory) { printf("created action factory\n"); try { factory->create_server_client(ros1_node, ros2_node, name); - // printf("Created 2 to 1 bridge for service %s\n", name.data()); } catch (std::runtime_error & e) { fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); } From 94b9c70cd3fb88e15cca6214f0e9e03aea51a961 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:04:38 +0200 Subject: [PATCH 34/43] risk leaking if the reset call crashes somehow Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 95763c67..0a0af0e4 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -103,7 +103,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface // create a new handler for the goal std::shared_ptr handler; - handler.reset(new GoalHandler(gh1, client_)); + handler = std::make_shared(gh1, client_)); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); @@ -296,7 +296,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface { std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); std::shared_ptr handler; - handler.reset(new GoalHandler(gh2, client_)); + handler = std::make_shared(gh2, client_)); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); From 21bee289923a44c91cb6b7462ceb9d9d9a57e7d0 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:07:58 +0200 Subject: [PATCH 35/43] Add goal ID to log output so it's more useful Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 0a0af0e4..15f06c7f 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -107,7 +107,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); - RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + RCLCPP_INFO_STREAM(ros2_node_->get_logger(), "Sending goal " << goal_id); std::thread( [handler, goal_id, this]() mutable { // execute the goal remotely From 7ba2784064b7920ef72d9b0f437cce0636f44726 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:08:33 +0200 Subject: [PATCH 36/43] rename variable so it's distinction from the variable with the same name in the enclosing class Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 15f06c7f..9ba5ff27 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -125,7 +125,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface public: void cancel() { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_gh_); canceled_ = true; if (gh2_) { // cancel goal if possible auto fut = client_->async_cancel_goal(gh2_); @@ -160,7 +160,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface gh1_.setAccepted(); { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_gh_); gh2_ = goal_handle; if (canceled_) { // cancel was called in between @@ -184,7 +184,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface ROS1Result res1; translate_result_2_to_1(res1, *(res2.result)); - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_gh_); if (res2.code == rclcpp_action::ResultCode::SUCCEEDED) { gh1_.setSucceeded(res1); } else if (res2.code == rclcpp_action::ResultCode::CANCELED) { @@ -202,7 +202,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; bool canceled_; // cancel was called - std::mutex mutex_; + std::mutex mutex_gh_; }; ros::NodeHandle ros1_node_; From 084ed7f4807d1bc4a8ae0ec77ab7e4012d481142 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:10:50 +0200 Subject: [PATCH 37/43] Change cout to use RCLCPP logging macros. Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 9ba5ff27..36746297 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -139,7 +139,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface translate_goal_1_to_2(*gh1_.getGoal(), goal2); if (!client_->wait_for_action_server(std::chrono::seconds(1))) { - std::cout << "Action server not available after waiting" << std::endl; + RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); gh1_.setRejected(); return; } @@ -273,7 +273,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface (void)uuid; (void)goal; if (!client_->waitForActionServerToStart(ros::Duration(1))) { - std::cout << "Action server not available after waiting" << std::endl; + RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); return rclcpp_action::GoalResponse::REJECT; } From 86a81403876d1d81563fcde9d90f146307a9a209 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:16:06 +0200 Subject: [PATCH 38/43] remove commented out code Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 2 -- 1 file changed, 2 deletions(-) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index a1745ebf..955fdef6 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -65,7 +65,6 @@ std::unique_ptr get_service_factory(const std::string & return factory; } @[end for]@ - // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); return factory; } @@ -84,7 +83,6 @@ std::unique_ptr get_action_factory(const std::string & r return factory; } @[end for]@ - // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); return null_ptr; } From 12838b916dbce8baf289cf9dc927061bf320fe5a Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 12:48:28 +0200 Subject: [PATCH 39/43] fix parenthesis Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 36746297..9e914804 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -27,7 +27,6 @@ #ifdef __clang__ #pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wunused-parameter" #endif #include #include // Need this for the goal state. Need a better solution @@ -103,7 +102,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface // create a new handler for the goal std::shared_ptr handler; - handler = std::make_shared(gh1, client_)); + handler = std::make_shared(gh1, client_); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); @@ -296,7 +295,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface { std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); std::shared_ptr handler; - handler = std::make_shared(gh2, client_)); + handler = std::make_shared(gh2, client_); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); From 6b10de45a3aa78ded273bd3a99259c205e258e42 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 12:53:35 +0200 Subject: [PATCH 40/43] remove unused params Signed-off-by: Harsh Deshpande --- src/action_bridge.cpp | 1 - src/dynamic_bridge.cpp | 8 ++------ 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index 14c35d62..16f2c881 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -17,7 +17,6 @@ // include ROS 1 #ifdef __clang__ #pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wunused-parameter" #endif #include "ros/ros.h" #ifdef __clang__ diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 90255d1e..526a9a22 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -144,9 +144,7 @@ void update_bridge( const std::map> & ros1_services, const std::map> & ros2_services, const std::map> & ros1_action_servers, - const std::map> & ros1_action_clients, const std::map> & ros2_action_servers, - const std::map> & ros2_action_clients, std::map & bridges_1to2, std::map & bridges_2to1, std::map & service_bridges_1_to_2, @@ -936,8 +934,7 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, - ros1_action_servers, ros1_action_clients, - ros2_action_servers, ros2_action_clients, + ros1_action_servers, ros2_action_servers, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, action_bridges_1_to_2, action_bridges_2_to_1, @@ -1114,8 +1111,7 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, - ros1_action_servers, ros1_action_clients, - ros2_action_servers, ros2_action_clients, + ros1_action_servers, ros2_action_servers, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, action_bridges_1_to_2, action_bridges_2_to_1, From 514f53d2032ff40eaa91dcd281b6208cc1ad4d3f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 12:53:51 +0200 Subject: [PATCH 41/43] fix nullptr keyword Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 955fdef6..758844df 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -83,7 +83,7 @@ std::unique_ptr get_action_factory(const std::string & r return factory; } @[end for]@ - return null_ptr; + return nullptr; } } // namespace ros1_bridge From 5e3c8410c2815aa23d4ee1f9c2b3a2b9189f8ba6 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 13:01:23 +0200 Subject: [PATCH 42/43] remove outdated comments Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 9e914804..437b0bd8 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -143,10 +143,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface return; } - // goal_response_callback signature changed after foxy, this implementation - // works with both std::shared_future gh2_future; - // Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { From 79a46de628a34a74caa619a8a34f7752b9e1152b Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 17:20:13 +0200 Subject: [PATCH 43/43] pass logger to GoalHandler Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 437b0bd8..88785b25 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -102,7 +102,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface // create a new handler for the goal std::shared_ptr handler; - handler = std::make_shared(gh1, client_); + handler = std::make_shared(gh1, client_, ros2_node_->get_logger()); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); @@ -138,7 +138,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface translate_goal_1_to_2(*gh1_.getGoal(), goal2); if (!client_->wait_for_action_server(std::chrono::seconds(1))) { - RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); + RCLCPP_INFO(this->logger_, "Action server not available after waiting"); gh1_.setRejected(); return; } @@ -190,13 +190,14 @@ class ActionFactory_1_2 : public ActionFactoryInterface } } - GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) - : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} + GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client, rclcpp::Logger logger) + : gh1_(gh1), gh2_(nullptr), client_(client), logger_(logger), canceled_(false) {} private: ROS1GoalHandle gh1_; ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; + rclcpp::Logger logger_; bool canceled_; // cancel was called std::mutex mutex_gh_; };