diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 705f2845..004afca4 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros1_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.9.7 (2023-05-27) +------------------ +* remove xmlrpcpp from package.xml (`#406 `_) +* Parametrization of `parameter_bridge` quality of service [Port of the commits (ec44770) and (86b4245) to foxy branch] (`#401 `_) +* Contributors: Dharini Dutia, Lucyanno Frota + 0.9.4 (2020-09-10) ------------------ * use hardcoded QoS (keep all, transient local) for /tf_static topic in dynamic_bridge (`#282 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index b9e6a8ff..6836317f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,10 @@ endif() find_ros1_package(std_msgs 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) +find_package(xmlrpcpp REQUIRED) + # find ROS 1 packages with messages / services include(cmake/find_ros1_interface_packages.cmake) find_ros1_interface_packages(ros1_message_packages) @@ -99,7 +103,7 @@ foreach(package_name ${ros2_interface_packages}) if(NOT "${package_name}" STREQUAL "builtin_interfaces") list(APPEND generated_files "${generated_path}/${package_name}_factories.cpp") list(APPEND generated_files "${generated_path}/${package_name}_factories.hpp") - foreach(interface_file ${${package_name}_INTERFACE_FILES}) + foreach(interface_file ${${package_name}_IDL_FILES}) 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 @@ -209,6 +213,13 @@ custom_executable(dynamic_bridge target_link_libraries(dynamic_bridge ${PROJECT_NAME}) +custom_executable(dynamic_parameter_bridge + "src/dynamic_parameter_bridge.cpp" + ROS1_DEPENDENCIES + TARGET_DEPENDENCIES ${ros2_interface_packages}) +target_link_libraries(dynamic_parameter_bridge + ${PROJECT_NAME}) + if(TEST_ROS1_BRIDGE) custom_executable(test_ros2_client_cpp "test/test_ros2_client.cpp") ament_target_dependencies("test_ros2_client_cpp${target_suffix}" "ros1_roscpp" "diagnostic_msgs") diff --git a/README.md b/README.md index f52db7b0..9f23324e 100644 --- a/README.md +++ b/README.md @@ -346,3 +346,120 @@ Launch AddTwoInts client: . /setup.bash ros2 run demo_nodes_cpp add_two_ints_client ``` + +## Example 4: bridge only selected topics and services +This example expands on example 3 by selecting a subset of topics and services to be bridged. +This is handy when, for example, you have a system that runs most of it's stuff in either ROS 1 or ROS 2 but needs a few nodes from the 'opposite' version of ROS. +Where the `dynamic_bridge` bridges all topics and service, the `parameter_bridge` uses the ROS 1 parameter server to choose which topics and services are bridged. +For example, to bridge only eg. the `/chatter` topic and the `/add_two_ints service` from ROS1 to ROS2, create this configuration file, `bridge.yaml`: + +```yaml +topics: + - + topic: /chatter # ROS1 topic name + type: std_msgs/msg/String # ROS2 type name + queue_size: 1 # For the publisher back to ROS1 +services_1_to_2: + - + service: /add_two_ints # ROS1 service name + type: example_interfaces/srv/AddTwoInts # The ROS2 type name +``` + +Start a ROS 1 roscore: + +```bash +# Shell A (ROS 1 only): +. /opt/ros/melodic/setup.bash +# Or, on OSX, something like: +# . ~/ros_catkin_ws/install_isolated/setup.bash +roscore +``` + +Then load the bridge.yaml config file and start the talker to publish on the `/chatter` topic: + +```bash +Shell B: (ROS1 only): +. /opt/ros/melodic/setup.bash +# Or, on OSX, something like: +# . ~/ros_catkin_ws/install_isolated/setup.bash +rosparam load bridge.yaml + +rosrun rospy_tutorials talker +``` + +```bash +Shell C: (ROS1 only): +. /opt/ros/melodic/setup.bash +# Or, on OSX, something like: +# . ~/ros_catkin_ws/install_isolated/setup.bash + +rosrun roscpp_tutorials add_two_ints_server +``` + +Then, in a few ROS 2 terminals: + +```bash +# Shell D: +. /setup.bash +ros2 run ros1_bridge parameter_bridge +``` + +If all is well, the logging shows it is creating bridges for the topic and service and you should be able to call the service and listen to the ROS 1 talker from ROS 2: + +```bash +# Shell E: +. /setup.bash +ros2 run demo_nodes_cpp listener +``` +This should start printing text like `I heard: [hello world ...]` with a timestamp. + +```bash +# Shell F: +. /setup.bash +ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}" +``` +If all is well, the output should contain `example_interfaces.srv.AddTwoInts_Response(sum=3)` + +### Parametrizing Quality of Service +An advantage of ROS 2 over ROS 1 is the possibility to define different Quality of Service settings per topic. +The parameter bridge optionally allows for this as well. +For some topics, like `/tf_static` this is actually required, as this is a latching topic in ROS 1. +In ROS 2 with the `parameter_bridge`, this requires that topic to be configured as such: + +```yaml +topics: + - + topic: /tf_static + type: tf2_msgs/msg/TFMessage + queue_size: 1 + qos: + history: keep_all + durability: transient_local +``` + +All other QoS options (as documented here in https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html) are available: + +```yaml +topics: + - + topic: /some_ros1_topic + type: std_msgs/msg/String + queue_size: 1 + qos: + history: keep_last # OR keep_all, then you can omit `depth` parameter below + depth: 10 # Only required when history == keep_last + reliability: reliable # OR best_effort + durability: transient_local # OR volatile + deadline: + secs: 10 + nsecs: 2345 + lifespan: + secs: 20 + nsecs: 3456 + liveliness: liveliness_system_default # Values from https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html, eg. LIVELINESS_AUTOMATIC + liveliness_lease_duration: + secs: 40 + nsecs: 5678 +``` + +Note that the `qos` section can be omitted entirely and options not set are left default. \ No newline at end of file diff --git a/bin/ros1_bridge_generate_factories b/bin/ros1_bridge_generate_factories old mode 100755 new mode 100644 diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index c0485df9..c4d79dfc 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -127,6 +127,16 @@ create_bidirectional_bridge( const std::string & topic_name, size_t queue_size = 10); +BridgeHandles +create_bidirectional_bridge( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string & ros1_type_name, + const std::string & ros2_type_name, + const std::string & topic_name, + size_t queue_size, + const rclcpp::QoS & publisher_qos); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__BRIDGE_HPP_ diff --git a/package.xml b/package.xml index a4b17b40..158a3f20 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ ros1_bridge - 0.9.4 + 0.9.7 A simple bridge between ROS 1 and ROS 2 Dirk Thomas Apache License 2.0 diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index 28a3e8ac..27939c4b 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -20,6 +20,10 @@ @ @{ from ros1_bridge import camel_case_to_lower_case_underscore +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import BoundedSequence +from rosidl_parser.definition import NamespacedType }@ #include "@(ros2_package_name)_factories.hpp" @@ -118,35 +122,46 @@ Factory< @{ ros1_field_selection = '.'.join((str(field.name) for field in ros1_fields)) ros2_field_selection = '.'.join((str(field.name) for field in ros2_fields)) + +if isinstance(ros2_fields[-1].type, NamespacedType): + namespaces = ros2_fields[-1].type.namespaces + assert len(namespaces) == 2 and namespaces[1] == 'msg', \ + "messages not using the ', msg, ' triplet are not supported" } -@[ if not ros2_fields[-1].type.is_array]@ +@[ if not isinstance(ros2_fields[-1].type, AbstractNestedType)]@ // convert non-array field -@[ if not ros2_fields[-1].type.pkg_name]@ +@[ if not isinstance(ros2_fields[-1].type, NamespacedType)]@ // convert primitive field ros2_msg.@(ros2_field_selection) = ros1_msg.@(ros1_field_selection); -@[ elif ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@ +@[ elif ros2_fields[-1].type.namespaces[0] == 'builtin_interfaces']@ // convert builtin field ros1_bridge::convert_1_to_2(ros1_msg.@(ros1_field_selection), ros2_msg.@(ros2_field_selection)); @[ else]@ // convert sub message field Factory< @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name), - @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type) + @(ros2_fields[-1].type.namespaces[0])::msg::@(ros2_fields[-1].type.name) >::convert_1_to_2( ros1_msg.@(ros1_field_selection), ros2_msg.@(ros2_field_selection)); @[ end if]@ @[ else]@ - // convert array field -@[ if not ros2_fields[-1].type.array_size or ros2_fields[-1].type.is_upper_bound]@ - // ensure array size -@[ if ros2_fields[-1].type.is_upper_bound]@ - // check boundary - assert(ros1_msg.@(ros1_field_selection).size() <= @(ros2_fields[-1].type.array_size)); + // convert array or sequence field +@[ if isinstance(ros2_fields[-1].type, AbstractSequence)]@ + // dynamically sized sequence, ensure destination sequence/vector size is large enough +@[ if isinstance(ros2_fields[-1].type, BoundedSequence)]@ + // bounded size sequence, check that the ros 1 vector size is not larger than the upper bound for the target + assert(ros1_msg.@(ros1_field_selection).size() <= @(ros2_fields[-1].type.maximum_size)); @[ end if]@ - // dynamic arrays must be resized + // resize ros2 field to match the ros1 field ros2_msg.@(ros2_field_selection).resize(ros1_msg.@(ros1_field_selection).size()); +@[ else]@ + // statically sized array + static_assert( + (ros2_msg.@(ros2_field_selection).size()) >= (ros1_msg.@(ros1_field_selection).size()), + "destination array not large enough for source array" + ); @[ end if]@ -@[ if not ros2_fields[-1].type.pkg_name]@ +@[ if not isinstance(ros2_fields[-1].type.value_type, NamespacedType)]@ // convert primitive array elements std::copy( ros1_msg.@(ros1_field_selection).begin(), @@ -155,22 +170,22 @@ ros2_field_selection = '.'.join((str(field.name) for field in ros2_fields)) @[ else]@ // copy element wise since the type is different { - auto ros1_it = ros1_msg.@(ros1_field_selection).begin(); + auto ros1_it = ros1_msg.@(ros1_field_selection).cbegin(); auto ros2_it = ros2_msg.@(ros2_field_selection).begin(); for ( ; - ros1_it != ros1_msg.@(ros1_field_selection).end() && + ros1_it != ros1_msg.@(ros1_field_selection).cend() && ros2_it != ros2_msg.@(ros2_field_selection).end(); ++ros1_it, ++ros2_it ) { // convert sub message element -@[ if ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@ +@[ if ros2_fields[-1].type.value_type.namespaces[0] == 'builtin_interfaces']@ ros1_bridge::convert_1_to_2(*ros1_it, *ros2_it); @[ else]@ Factory< @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name), - @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type) + @(ros2_fields[-1].type.value_type.namespaces[0])::msg::@(ros2_fields[-1].type.value_type.name) >::convert_1_to_2( *ros1_it, *ros2_it); @[ end if]@ @@ -198,31 +213,42 @@ Factory< @{ ros2_field_selection = '.'.join((str(field.name) for field in ros2_fields)) ros1_field_selection = '.'.join((str(field.name) for field in ros1_fields)) + +if isinstance(ros2_fields[-1].type, NamespacedType): + namespaces = ros2_fields[-1].type.namespaces + assert len(namespaces) == 2 and namespaces[1] == 'msg', \ + "messages not using the ', msg, ' triplet are not supported" } -@[ if not ros2_fields[-1].type.is_array]@ +@[ if not isinstance(ros2_fields[-1].type, AbstractNestedType)]@ // convert non-array field -@[ if not ros2_fields[-1].type.pkg_name]@ +@[ if not isinstance(ros2_fields[-1].type, NamespacedType)]@ // convert primitive field ros1_msg.@(ros1_field_selection) = ros2_msg.@(ros2_field_selection); -@[ elif ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@ +@[ elif ros2_fields[-1].type.namespaces[0] == 'builtin_interfaces']@ // convert builtin field ros1_bridge::convert_2_to_1(ros2_msg.@(ros2_field_selection), ros1_msg.@(ros1_field_selection)); @[ else]@ // convert sub message field Factory< @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name), - @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type) + @(ros2_fields[-1].type.namespaces[0])::msg::@(ros2_fields[-1].type.name) >::convert_2_to_1( ros2_msg.@(ros2_field_selection), ros1_msg.@(ros1_field_selection)); @[ end if]@ @[ else]@ - // convert array field -@[ if not ros2_fields[-1].type.array_size or ros2_fields[-1].type.is_upper_bound]@ - // ensure array size - // dynamic arrays must be resized + // convert array or sequence field +@[ if isinstance(ros2_fields[-1].type, AbstractSequence)]@ + // dynamically sized sequence, ensure destination vector size is large enough + // resize ros1 field to match the ros2 field ros1_msg.@(ros1_field_selection).resize(ros2_msg.@(ros2_field_selection).size()); +@[ else]@ + // statically sized array + static_assert( + (ros1_msg.@(ros1_field_selection).size()) >= (ros2_msg.@(ros2_field_selection).size()), + "destination array not large enough for source array" + ); @[ end if]@ -@[ if not ros2_fields[-1].type.pkg_name]@ +@[ if not isinstance(ros2_fields[-1].type.value_type, NamespacedType)]@ // convert primitive array elements std::copy( ros2_msg.@(ros2_field_selection).begin(), @@ -231,22 +257,22 @@ ros1_field_selection = '.'.join((str(field.name) for field in ros1_fields)) @[ else]@ // copy element wise since the type is different { - auto ros2_it = ros2_msg.@(ros2_field_selection).begin(); + auto ros2_it = ros2_msg.@(ros2_field_selection).cbegin(); auto ros1_it = ros1_msg.@(ros1_field_selection).begin(); for ( ; - ros2_it != ros2_msg.@(ros2_field_selection).end() && + ros2_it != ros2_msg.@(ros2_field_selection).cend() && ros1_it != ros1_msg.@(ros1_field_selection).end(); ++ros2_it, ++ros1_it ) { // convert sub message element -@[ if ros2_fields[-1].type.pkg_name == 'builtin_interfaces']@ +@[ if ros2_fields[-1].type.value_type.namespaces[0] == 'builtin_interfaces']@ ros1_bridge::convert_2_to_1(*ros2_it, *ros1_it); @[ else]@ Factory< @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name), - @(ros2_fields[-1].type.pkg_name)::msg::@(ros2_fields[-1].type.type) + @(ros2_fields[-1].type.value_type.namespaces[0])::msg::@(ros2_fields[-1].type.value_type.name) >::convert_2_to_1( *ros2_it, *ros1_it); @[ end if]@ diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 9f89c706..7a3fc296 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -25,6 +25,7 @@ import rosidl_adapter.parser from rosidl_cmake import expand_template +import rosidl_parser.parser import yaml @@ -684,11 +685,17 @@ def consume_field(field): def get_ros2_selected_fields(ros2_field_selection, parent_ros2_spec, msg_idx): selected_fields = [] fields = ros2_field_selection.split('.') - current_field = [f for f in parent_ros2_spec.fields if f.name == fields[0]][0] + current_field = [ + member for member in parent_ros2_spec.structure.members + if member.name == fields[0] + ][0] selected_fields.append(current_field) for field in fields[1:]: parent_ros2_spec = load_ros2_message(msg_idx.ros2_get_from_field(current_field)) - current_field = [f for f in parent_ros2_spec.fields if f.name == field][0] + current_field = [ + member for member in parent_ros2_spec.structure.members + if member.name == field + ][0] selected_fields.append(current_field) return tuple(selected_fields) @@ -752,12 +759,11 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): ros1_field_missing_in_ros2 = False for ros1_field in ros1_spec.parsed_fields(): - for ros2_field in ros2_spec.fields: - if ros1_field.name.lower() == ros2_field.name: + for ros2_member in ros2_spec.structure.members: + if ros1_field.name.lower() == ros2_member.name: # get package name and message name from ROS 1 field type - if ros2_field.type.pkg_name: - update_ros1_field_information(ros1_field, ros1_msg.package_name) - mapping.add_field_pair(ros1_field, ros2_field) + update_ros1_field_information(ros1_field, ros1_msg.package_name) + mapping.add_field_pair(ros1_field, ros2_member) break else: # this allows fields to exist in ROS 1 but not in ROS 2 @@ -767,9 +773,9 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): # if some fields exist in ROS 1 but not in ROS 2 # check that no fields exist in ROS 2 but not in ROS 1 # since then it might be the case that those have been renamed and should be mapped - for ros2_field in ros2_spec.fields: + for ros2_member in ros2_spec.structure.members: for ros1_field in ros1_spec.parsed_fields(): - if ros1_field.name.lower() == ros2_field.name: + if ros1_field.name.lower() == ros2_member.name: break else: # if fields from both sides are not mappable the whole message is not mappable @@ -801,14 +807,45 @@ def load_ros1_service(ros1_srv): def load_ros2_message(ros2_msg): - message_path = os.path.join( - ros2_msg.prefix_path, 'share', ros2_msg.package_name, 'msg', - ros2_msg.message_name + '.msg') - try: - spec = rosidl_adapter.parser.parse_message_file(ros2_msg.package_name, message_path) - except rosidl_adapter.parser.InvalidSpecification: - return None - return spec + message_basepath = os.path.join(ros2_msg.prefix_path, 'share') + message_relative_path = \ + os.path.join(ros2_msg.package_name, 'msg', ros2_msg.message_name) + message_path = os.path.join(message_basepath, message_relative_path) + # Check to see if the message is defined as a .msg file or an .idl file, + # but preferring '.idl' if both exist. + if os.path.exists(message_path + '.idl'): + message_path += '.idl' + message_relative_path += '.idl' + elif os.path.exists(message_path + '.msg'): + message_path += '.msg' + message_relative_path += '.msg' + else: + raise RuntimeError( + f"message '{ros2_msg.package_name}/msg/{ros2_msg.message_name}' " + f"was not found in prefix '{ros2_msg.prefix_path}' with either " + f"file extension '.msg' or '.idl'") + # We don't support .msg files, but that shouldn't be a problem since an .idl + # version should have been created when the package was built by rosidl_adapter. + if message_path.endswith('.msg'): + raise RuntimeError( + "ros1_bridge cannot process ROS 2 message definitions that lack a '.idl' version, " + "which normally does not occur as rosidl_adapter should create a '.idl' version " + "when building the message package which contains the original '.msg' file." + ) + if not message_path.endswith('.idl'): + raise RuntimeError( + f"message_path '{message_path}' unexpectedly does not end with '.idl'" + ) + idl_locator = \ + rosidl_parser.definition.IdlLocator(message_basepath, message_relative_path) + spec = rosidl_parser.parser.parse_idl_file(idl_locator) + messages = spec.content.get_elements_of_type(rosidl_parser.definition.Message) + if len(messages) != 1: + raise RuntimeError( + 'unexpectedly found multiple message definitions when processing ' + f"message '{ros2_msg.package_name}/msg/{ros2_msg.message_name}'" + ) + return messages[0] def load_ros2_service(ros2_srv): @@ -847,26 +884,34 @@ def __init__(self, ros1_msg, ros2_msg): self.fields_2_to_1 = OrderedDict() self.depends_on_ros2_messages = set() - def add_field_pair(self, ros1_fields, ros2_fields): + def add_field_pair(self, ros1_fields, ros2_members): """ Add a new mapping for a pair of ROS 1 and ROS 2 messages. :type ros1_fields: either a genmsg.msgs.Field object with additional attributes `pkg_name` and `msg_name` as defined by `update_ros1_field_information`, or a tuple of objects of that type - :type ros2_field: either a rosidl_adapter.parser.Field object, or a tuple objects of - that type + :type ros2_members: a single, or list of, rosidl_parser.definition.Member object(s) """ if not isinstance(ros1_fields, tuple): ros1_fields = (ros1_fields,) - if not isinstance(ros2_fields, tuple): - ros2_fields = (ros2_fields, ) - self.fields_1_to_2[ros1_fields] = ros2_fields - self.fields_2_to_1[ros2_fields] = ros1_fields - for ros2_field in ros2_fields: - if ros2_field.type.pkg_name and ros2_field.type.pkg_name != 'builtin_interfaces': - self.depends_on_ros2_messages.add( - Message(ros2_field.type.pkg_name, ros2_field.type.type)) + if not isinstance(ros2_members, tuple): + ros2_members = (ros2_members, ) + self.fields_1_to_2[ros1_fields] = ros2_members + self.fields_2_to_1[ros2_members] = ros1_fields + for ros2_member in ros2_members: + # If the member is not a namespaced type, skip. + if not isinstance(ros2_member.type, rosidl_parser.definition.NamespacedType): + continue + # If it is, then the type will have a namespaced name, e.g. (std_msgs, msg, String) + # If it is not of the standard ('', 'msg', ''), skip it + if len(ros2_member.type.namespaces) != 2 or ros2_member.type.namespaces[1] != 'msg': + continue + # Extract the package name and message name + pkg_name = ros2_member.type.namespaces[0] + msg_name = ros2_member.type.name + if pkg_name != 'builtin_interfaces': + self.depends_on_ros2_messages.add(Message(pkg_name, msg_name)) def camel_case_to_lower_case_underscore(value): diff --git a/src/bridge.cpp b/src/bridge.cpp index ec682acc..2186b1e7 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -137,4 +137,28 @@ create_bidirectional_bridge( return handles; } +BridgeHandles +create_bidirectional_bridge( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string & ros1_type_name, + const std::string & ros2_type_name, + const std::string & topic_name, + size_t queue_size, + const rclcpp::QoS & publisher_qos) +{ + RCLCPP_INFO( + ros2_node->get_logger(), "create bidirectional bridge for topic %s", + topic_name.c_str()); + BridgeHandles handles; + handles.bridge1to2 = create_bridge_from_1_to_2( + ros1_node, ros2_node, + ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, publisher_qos); + handles.bridge2to1 = create_bridge_from_2_to_1( + ros2_node, ros1_node, + ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size, + handles.bridge1to2.ros2_publisher); + return handles; +} + } // namespace ros1_bridge diff --git a/src/dynamic_parameter_bridge.cpp b/src/dynamic_parameter_bridge.cpp new file mode 100644 index 00000000..ff63ddbb --- /dev/null +++ b/src/dynamic_parameter_bridge.cpp @@ -0,0 +1,877 @@ +// 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 +#include +#include +#include +#include +#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/this_node.h" +#include "ros/header.h" +#include "ros/service_manager.h" +#include "ros/transport/transport_tcp.h" + +// include ROS 2 +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/scope_exit.hpp" + +#include "rcutils/get_env.h" + +#include "ros1_bridge/bridge.hpp" + + +std::mutex g_bridge_mutex; + +struct Bridge1to2HandlesAndMessageTypes +{ + ros1_bridge::Bridge1to2Handles bridge_handles; + std::string ros1_type_name; + std::string ros2_type_name; +}; + +struct Bridge2to1HandlesAndMessageTypes +{ + ros1_bridge::Bridge2to1Handles bridge_handles; + std::string ros1_type_name; + std::string ros2_type_name; +}; + +bool find_command_option(const std::vector & args, const std::string & option) +{ + return std::find(args.begin(), args.end(), option) != args.end(); +} + +bool get_flag_option(const std::vector & args, const std::string & option) +{ + auto it = std::find(args.begin(), args.end(), option); + return it != args.end(); +} + +bool parse_command_options( + int argc, char ** argv, bool & output_topic_introspection, + bool & bridge_all_1to2_topics, bool & bridge_all_2to1_topics) +{ + std::vector args(argv, argv + argc); + + if (find_command_option(args, "-h") || find_command_option(args, "--help")) { + std::stringstream ss; + ss << "Usage:" << std::endl; + ss << " -h, --help: This message." << std::endl; + ss << " --show-introspection: Print output of introspection of both sides of the bridge."; + ss << std::endl; + ss << " --print-pairs: Print a list of the supported ROS 2 <=> ROS 1 conversion pairs."; + ss << std::endl; + ss << " --bridge-all-topics: Bridge all topics in both directions, whether or not there is "; + ss << "a matching subscriber." << std::endl; + ss << " --bridge-all-1to2-topics: Bridge all ROS 1 topics to ROS 2, whether or not there is "; + ss << "a matching subscriber." << std::endl; + ss << " --bridge-all-2to1-topics: Bridge all ROS 2 topics to ROS 1, whether or not there is "; + ss << "a matching subscriber." << std::endl; + std::cout << ss.str(); + return false; + } + + if (get_flag_option(args, "--print-pairs")) { + auto mappings_2to1 = ros1_bridge::get_all_message_mappings_2to1(); + if (mappings_2to1.size() > 0) { + printf("Supported ROS 2 <=> ROS 1 message 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 message type conversion pairs supported.\n"); + } + mappings_2to1 = ros1_bridge::get_all_service_mappings_2to1(); + if (mappings_2to1.size() > 0) { + printf("Supported ROS 2 <=> ROS 1 service 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 service type conversion pairs supported.\n"); + } + return false; + } + + output_topic_introspection = get_flag_option(args, "--show-introspection"); + + bool bridge_all_topics = get_flag_option(args, "--bridge-all-topics"); + bridge_all_1to2_topics = bridge_all_topics || get_flag_option(args, "--bridge-all-1to2-topics"); + bridge_all_2to1_topics = bridge_all_topics || get_flag_option(args, "--bridge-all-2to1-topics"); + + return true; +} + +void update_bridge( + ros::NodeHandle & ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::map & ros1_publishers, + const std::map & ros1_subscribers, + const std::map & ros2_publishers, + const std::map & ros2_subscribers, + const std::map> & ros1_services, + const std::map> & ros2_services, + std::map & bridges_1to2, + std::map & bridges_2to1, + std::map & service_bridges_1_to_2, + std::map & service_bridges_2_to_1, + bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) +{ + std::lock_guard lock(g_bridge_mutex); + + // create 1to2 bridges + for (auto ros1_publisher : ros1_publishers) { + // identify topics available as ROS 1 publishers as well as ROS 2 subscribers + auto topic_name = ros1_publisher.first; + std::string ros1_type_name = ros1_publisher.second; + std::string ros2_type_name; + + auto ros2_subscriber = ros2_subscribers.find(topic_name); + if (ros2_subscriber == ros2_subscribers.end()) { + if (!bridge_all_1to2_topics) { + continue; + } + // update the ROS 2 type name to be that of the anticipated bridged type + // TODO(dhood): support non 1-1 "bridge-all" mappings + bool mapping_found = ros1_bridge::get_1to2_mapping(ros1_type_name, ros2_type_name); + if (!mapping_found) { + // printf("No known mapping for ROS 1 type '%s'\n", ros1_type_name.c_str()); + continue; + } + // printf("topic name '%s' has ROS 2 publishers\n", topic_name.c_str()); + } else { + ros2_type_name = ros2_subscriber->second; + // printf("topic name '%s' has ROS 1 publishers and ROS 2 subscribers\n", topic_name.c_str()); + } + + // check if 1to2 bridge for the topic exists + if (bridges_1to2.find(topic_name) != bridges_1to2.end()) { + auto bridge = bridges_1to2.find(topic_name)->second; + if (bridge.ros1_type_name == ros1_type_name && bridge.ros2_type_name == ros2_type_name) { + // skip if bridge with correct types is already in place + continue; + } + // remove existing bridge with previous types + bridges_1to2.erase(topic_name); + printf("replace 1to2 bridge for topic '%s'\n", topic_name.c_str()); + } + + Bridge1to2HandlesAndMessageTypes bridge; + bridge.ros1_type_name = ros1_type_name; + bridge.ros2_type_name = ros2_type_name; + + auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); + if (topic_name == "/tf_static") { + ros2_publisher_qos.keep_all(); + ros2_publisher_qos.transient_local(); + } + try { + bridge.bridge_handles = ros1_bridge::create_bridge_from_1_to_2( + ros1_node, ros2_node, + bridge.ros1_type_name, topic_name, 10, + bridge.ros2_type_name, topic_name, ros2_publisher_qos); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to create 1to2 bridge for topic '%s' " + "with ROS 1 type '%s' and ROS 2 type '%s': %s\n", + topic_name.c_str(), bridge.ros1_type_name.c_str(), bridge.ros2_type_name.c_str(), e.what()); + if (std::string(e.what()).find("No template specialization") != std::string::npos) { + fprintf(stderr, "check the list of supported pairs with the `--print-pairs` option\n"); + } + continue; + } + + bridges_1to2[topic_name] = bridge; + printf( + "created 1to2 bridge for topic '%s' with ROS 1 type '%s' and ROS 2 type '%s'\n", + topic_name.c_str(), bridge.ros1_type_name.c_str(), bridge.ros2_type_name.c_str()); + } + + // create 2to1 bridges + for (auto ros2_publisher : ros2_publishers) { + // identify topics available as ROS 1 subscribers as well as ROS 2 publishers + auto topic_name = ros2_publisher.first; + std::string ros2_type_name = ros2_publisher.second; + std::string ros1_type_name; + + auto ros1_subscriber = ros1_subscribers.find(topic_name); + if (ros1_subscriber == ros1_subscribers.end()) { + if (!bridge_all_2to1_topics) { + continue; + } + // update the ROS 1 type name to be that of the anticipated bridged type + // TODO(dhood): support non 1-1 "bridge-all" mappings + bool mapping_found = ros1_bridge::get_2to1_mapping(ros2_type_name, ros1_type_name); + if (!mapping_found) { + // printf("No known mapping for ROS 2 type '%s'\n", ros2_type_name.c_str()); + continue; + } + // printf("topic name '%s' has ROS 2 publishers\n", topic_name.c_str()); + } else { + ros1_type_name = ros1_subscriber->second; + // printf("topic name '%s' has ROS 1 subscribers and ROS 2 publishers\n", topic_name.c_str()); + } + + // check if 2to1 bridge for the topic exists + if (bridges_2to1.find(topic_name) != bridges_2to1.end()) { + auto bridge = bridges_2to1.find(topic_name)->second; + if ((bridge.ros1_type_name == ros1_type_name || bridge.ros1_type_name == "") && + bridge.ros2_type_name == ros2_type_name) + { + // skip if bridge with correct types is already in place + continue; + } + // remove existing bridge with previous types + bridges_2to1.erase(topic_name); + printf("replace 2to1 bridge for topic '%s'\n", topic_name.c_str()); + } + + Bridge2to1HandlesAndMessageTypes bridge; + bridge.ros1_type_name = ros1_type_name; + bridge.ros2_type_name = ros2_type_name; + + try { + bridge.bridge_handles = ros1_bridge::create_bridge_from_2_to_1( + ros2_node, ros1_node, + bridge.ros2_type_name, topic_name, 10, + bridge.ros1_type_name, topic_name, 10); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to create 2to1 bridge for topic '%s' " + "with ROS 2 type '%s' and ROS 1 type '%s': %s\n", + topic_name.c_str(), bridge.ros2_type_name.c_str(), bridge.ros1_type_name.c_str(), e.what()); + if (std::string(e.what()).find("No template specialization") != std::string::npos) { + fprintf(stderr, "check the list of supported pairs with the `--print-pairs` option\n"); + } + continue; + } + + bridges_2to1[topic_name] = bridge; + printf( + "created 2to1 bridge for topic '%s' with ROS 2 type '%s' and ROS 1 type '%s'\n", + topic_name.c_str(), bridge.ros2_type_name.c_str(), bridge.ros1_type_name.c_str()); + } + + // remove obsolete bridges + std::vector to_be_removed_1to2; + for (auto it : bridges_1to2) { + std::string topic_name = it.first; + if ( + ros1_publishers.find(topic_name) == ros1_publishers.end() || + (!bridge_all_1to2_topics && ros2_subscribers.find(topic_name) == ros2_subscribers.end())) + { + to_be_removed_1to2.push_back(topic_name); + } + } + for (auto topic_name : to_be_removed_1to2) { + bridges_1to2.erase(topic_name); + printf("removed 1to2 bridge for topic '%s'\n", topic_name.c_str()); + } + + std::vector to_be_removed_2to1; + for (auto it : bridges_2to1) { + std::string topic_name = it.first; + if ( + (!bridge_all_2to1_topics && ros1_subscribers.find(topic_name) == ros1_subscribers.end()) || + ros2_publishers.find(topic_name) == ros2_publishers.end()) + { + to_be_removed_2to1.push_back(topic_name); + } + } + for (auto topic_name : to_be_removed_2to1) { + bridges_2to1.erase(topic_name); + printf("removed 2to1 bridge for topic '%s'\n", topic_name.c_str()); + } + + // create bridges for ros1 services + for (auto & service : ros1_services) { + auto & name = service.first; + auto & details = service.second; + if ( + service_bridges_2_to_1.find(name) == service_bridges_2_to_1.end() && + service_bridges_1_to_2.find(name) == service_bridges_1_to_2.end()) + { + auto factory = ros1_bridge::get_service_factory( + "ros1", details.at("package"), details.at("name")); + if (factory) { + try { + service_bridges_2_to_1[name] = factory->service_bridge_2_to_1(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()); + } + } + } + } + + // create bridges for ros2 services + for (auto & service : ros2_services) { + auto & name = service.first; + auto & details = service.second; + if ( + service_bridges_1_to_2.find(name) == service_bridges_1_to_2.end() && + service_bridges_2_to_1.find(name) == service_bridges_2_to_1.end()) + { + auto factory = ros1_bridge::get_service_factory( + "ros2", details.at("package"), details.at("name")); + if (factory) { + try { + service_bridges_1_to_2[name] = factory->service_bridge_1_to_2(ros1_node, ros2_node, name); + printf("Created 1 to 2 bridge for service %s\n", name.data()); + } catch (std::runtime_error & e) { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } + } + } + + // remove obsolete ros1 services + for (auto it = service_bridges_2_to_1.begin(); it != service_bridges_2_to_1.end(); ) { + if (ros1_services.find(it->first) == ros1_services.end()) { + printf("Removed 2 to 1 bridge for service %s\n", it->first.data()); + try { + it = service_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 services + for (auto it = service_bridges_1_to_2.begin(); it != service_bridges_1_to_2.end(); ) { + if (ros2_services.find(it->first) == ros2_services.end()) { + printf("Removed 1 to 2 bridge for service %s\n", it->first.data()); + try { + it->second.server.shutdown(); + it = service_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( + const std::string name, std::map> & ros1_services) +{ + // NOTE(rkozik): + // I tried to use Connection class but could not make it work + // auto callback = [](const ros::ConnectionPtr&, const ros::Header&) + // { printf("Callback\n"); return true; }; + // ros::HeaderReceivedFunc f(callback); + // ros::ConnectionPtr connection(new ros::Connection); + // connection->initialize(transport, false, ros::HeaderReceivedFunc()); + ros::ServiceManager manager; + std::string host; + std::uint32_t port; + if (!manager.lookupService(name, host, port)) { + fprintf(stderr, "Failed to look up %s\n", name.data()); + return; + } + ros::TransportTCPPtr transport(new ros::TransportTCP(nullptr, ros::TransportTCP::SYNCHRONOUS)); + auto transport_exit = rclcpp::make_scope_exit( + [transport]() { + transport->close(); + }); + if (!transport->connect(host, port)) { + fprintf(stderr, "Failed to connect to %s:%d\n", host.data(), port); + return; + } + ros::M_string header_out; + header_out["probe"] = "1"; + header_out["md5sum"] = "*"; + header_out["service"] = name; + header_out["callerid"] = ros::this_node::getName(); + boost::shared_array buffer; + uint32_t len; + ros::Header::write(header_out, buffer, len); + std::vector message(len + 4); + std::memcpy(&message[0], &len, 4); + std::memcpy(&message[4], buffer.get(), len); + transport->write(message.data(), message.size()); + uint32_t length; + auto read = transport->read(reinterpret_cast(&length), 4); + if (read != 4) { + fprintf(stderr, "Failed to read a response from a service server\n"); + return; + } + std::vector response(length); + read = transport->read(response.data(), length); + if (read < 0 || static_cast(read) != length) { + fprintf(stderr, "Failed to read a response from a service server\n"); + return; + } + std::string key = name; + ros1_services[key] = std::map(); + ros::Header header_in; + std::string error; + auto success = header_in.parse(response.data(), length, error); + if (!success) { + fprintf(stderr, "%s\n", error.data()); + return; + } + for (std::string field : {"type"}) { + std::string value; + auto success = header_in.getValue(field, value); + if (!success) { + fprintf(stderr, "Failed to read '%s' from a header for '%s'\n", field.data(), key.c_str()); + ros1_services.erase(key); + return; + } + ros1_services[key][field] = value; + } + std::string t = ros1_services[key]["type"]; + ros1_services[key]["package"] = std::string(t.begin(), t.begin() + t.find("/")); + ros1_services[key]["name"] = std::string(t.begin() + t.find("/") + 1, t.end()); +} + +int main(int argc, char * argv[]) +{ + bool output_topic_introspection; + bool bridge_all_1to2_topics; + bool bridge_all_2to1_topics; + if (!parse_command_options( + argc, argv, output_topic_introspection, bridge_all_1to2_topics, bridge_all_2to1_topics)) + { + return 0; + } + + //temp + bridge_all_1to2_topics = bridge_all_2to1_topics = true; + + // ROS 2 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; + + // mapping of available topic names to type names + std::map ros1_publishers; + std::map ros1_subscribers; + std::map ros2_publishers; + std::map ros2_subscribers; + std::map> ros1_services; + std::map> ros2_services; + + std::map bridges_1to2; + std::map bridges_2to1; + std::map service_bridges_1_to_2; + std::map service_bridges_2_to_1; + + + // bridge all topics listed in a ROS 1 parameter + // the topics parameter needs to be an array + // and each item needs to be a dictionary with the following keys; + // topic: the name of the topic to bridge (e.g. '/topic_name') + // type: the type of the topic to bridge (e.g. 'pkgname/msg/MsgName') + // queue_size: the queue size to use (default: 100) + const char * topics_parameter_name = "topics"; + // the services parameters need to be arrays + // and each item needs to be a dictionary with the following keys; + // topic: the name of the topic to bridge (e.g. '/service_name') + // type: the type of the topic to bridge (e.g. 'pkgname/srv/SrvName') + const char * services_1_to_2_parameter_name = "services_1_to_2"; + const char * services_2_to_1_parameter_name = "services_2_to_1"; + if (argc > 1) { + topics_parameter_name = argv[1]; + } + if (argc > 2) { + services_1_to_2_parameter_name = argv[2]; + } + if (argc > 3) { + services_2_to_1_parameter_name = argv[3]; + } + + XmlRpc::XmlRpcValue topics; + std::set topic_list; + if ( + ros1_node.getParam(topics_parameter_name, topics) && + topics.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (size_t i = 0; i < static_cast(topics.size()); ++i) { + topic_list.insert(static_cast(topics[i]["topic"])); + } + } + + XmlRpc::XmlRpcValue services_1_to_2; + std::set services_1_to_2_list; + if ( + ros1_node.getParam(services_1_to_2_parameter_name, services_1_to_2) && + services_1_to_2.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (size_t i = 0; i < static_cast(services_1_to_2.size()); ++i) { + services_1_to_2_list.insert(static_cast(services_1_to_2[i]["service"])); + } + } + + XmlRpc::XmlRpcValue services_2_to_1; + std::set services_2_to_1_list; + if ( + ros1_node.getParam(services_2_to_1_parameter_name, services_2_to_1) && + services_2_to_1.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (size_t i = 0; i < static_cast(services_2_to_1.size()); ++i) { + services_2_to_1_list.insert(static_cast(services_2_to_1[i]["service"])); + } + } + + // setup polling of ROS 1 master + auto ros1_poll = [ + &ros1_node, ros2_node, + &ros1_publishers, &ros1_subscribers, + &ros2_publishers, &ros2_subscribers, + &bridges_1to2, &bridges_2to1, + &ros1_services, &ros2_services, + &service_bridges_1_to_2, &service_bridges_2_to_1, + &output_topic_introspection, + &bridge_all_1to2_topics, &bridge_all_2to1_topics, + &topic_list, &services_2_to_1_list + ](const ros::TimerEvent &) -> void + { + // collect all topics names which have at least one publisher or subscriber beside this bridge + std::set active_publishers; + std::set active_subscribers; + + XmlRpc::XmlRpcValue args, result, payload; + args[0] = ros::this_node::getName(); + if (!ros::master::execute("getSystemState", args, result, payload, true)) { + fprintf(stderr, "failed to get system state from ROS 1 master\n"); + return; + } + // check publishers + if (payload.size() >= 1) { + for (int j = 0; j < payload[0].size(); ++j) { + std::string topic_name = payload[0][j][0]; + if ( topic_list.find(topic_name) == topic_list.end()) { + continue; + } + for (int k = 0; k < payload[0][j][1].size(); ++k) { + std::string node_name = payload[0][j][1][k]; + // ignore publishers from the bridge itself + if (node_name == ros::this_node::getName()) { + continue; + } + active_publishers.insert(topic_name); + break; + } + } + } + // check subscribers + if (payload.size() >= 2) { + for (int j = 0; j < payload[1].size(); ++j) { + std::string topic_name = payload[1][j][0]; + if ( topic_list.find(topic_name) == topic_list.end()) { + continue; + } + for (int k = 0; k < payload[1][j][1].size(); ++k) { + std::string node_name = payload[1][j][1][k]; + // ignore subscribers from the bridge itself + if (node_name == ros::this_node::getName()) { + continue; + } + active_subscribers.insert(topic_name); + break; + } + } + } + + // check services + std::map> active_ros1_services; + if (payload.size() >= 3) { + for (int j = 0; j < payload[2].size(); ++j) { + if (payload[2][j][0].getType() == XmlRpc::XmlRpcValue::TypeString) { + std::string name = payload[2][j][0]; + if ( services_2_to_1_list.find(name) == services_2_to_1_list.end()) { + continue; + } + get_ros1_service_info(name, active_ros1_services); + } + } + } + { + std::lock_guard lock(g_bridge_mutex); + ros1_services = active_ros1_services; + } + + // get message types for all topics + ros::master::V_TopicInfo topics; + bool success = ros::master::getTopics(topics); + if (!success) { + fprintf(stderr, "failed to poll ROS 1 master\n"); + return; + } + + std::map current_ros1_publishers; + std::map current_ros1_subscribers; + for (auto topic : topics) { + auto topic_name = topic.name; + bool has_publisher = active_publishers.find(topic_name) != active_publishers.end(); + bool has_subscriber = active_subscribers.find(topic_name) != active_subscribers.end(); + if (!has_publisher && !has_subscriber) { + // skip inactive topics + continue; + } + if (has_publisher) { + current_ros1_publishers[topic_name] = topic.datatype; + } + if (has_subscriber) { + current_ros1_subscribers[topic_name] = topic.datatype; + } + if (output_topic_introspection) { + printf( + " ROS 1: %s (%s) [%s pubs, %s subs]\n", + topic_name.c_str(), topic.datatype.c_str(), + has_publisher ? ">0" : "0", has_subscriber ? ">0" : "0"); + } + } + + // since ROS 1 subscribers don't report their type they must be added anyway + for (auto active_subscriber : active_subscribers) { + if (current_ros1_subscribers.find(active_subscriber) == current_ros1_subscribers.end()) { + current_ros1_subscribers[active_subscriber] = ""; + if (output_topic_introspection) { + printf(" ROS 1: %s () sub++\n", active_subscriber.c_str()); + } + } + } + + if (output_topic_introspection) { + printf("\n"); + } + + { + std::lock_guard lock(g_bridge_mutex); + ros1_publishers = current_ros1_publishers; + ros1_subscribers = current_ros1_subscribers; + } + + update_bridge( + ros1_node, ros2_node, + ros1_publishers, ros1_subscribers, + ros2_publishers, ros2_subscribers, + ros1_services, ros2_services, + bridges_1to2, bridges_2to1, + service_bridges_1_to_2, service_bridges_2_to_1, + bridge_all_1to2_topics, bridge_all_2to1_topics); + }; + + auto ros1_poll_timer = ros1_node.createTimer(ros::Duration(1.0), ros1_poll); + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // setup polling of ROS 2 + std::set already_ignored_topics; + std::set already_ignored_services; + auto ros2_poll = [ + &ros1_node, ros2_node, + &ros1_publishers, &ros1_subscribers, + &ros2_publishers, &ros2_subscribers, + &ros1_services, &ros2_services, + &bridges_1to2, &bridges_2to1, + &service_bridges_1_to_2, &service_bridges_2_to_1, + &output_topic_introspection, + &bridge_all_1to2_topics, &bridge_all_2to1_topics, + &already_ignored_topics, &already_ignored_services, + &topic_list, &services_1_to_2_list + ]() -> void + { + auto ros2_topics = ros2_node->get_topic_names_and_types(); + + std::set ignored_topics; + ignored_topics.insert("parameter_events"); + + std::map current_ros2_publishers; + std::map current_ros2_subscribers; + for (auto topic_and_types : ros2_topics) { + // ignore some common ROS 2 specific topics + if (ignored_topics.find(topic_and_types.first) != ignored_topics.end()) { + continue; + } + if (topic_list.find(topic_and_types.first) == topic_list.end()) { + continue; + } + + auto & topic_name = topic_and_types.first; + auto & topic_type = topic_and_types.second[0]; // explicitly take the first + + // explicitly avoid topics with more than one type + if (topic_and_types.second.size() > 1) { + if (already_ignored_topics.count(topic_name) == 0) { + std::string types = ""; + for (auto type : topic_and_types.second) { + types += type + ", "; + } + fprintf( + stderr, + "warning: ignoring topic '%s', which has more than one type: [%s]\n", + topic_name.c_str(), + types.substr(0, types.length() - 2).c_str() + ); + already_ignored_topics.insert(topic_name); + } + continue; + } + + auto publisher_count = ros2_node->count_publishers(topic_name); + auto subscriber_count = ros2_node->count_subscribers(topic_name); + + // ignore publishers from the bridge itself + if (bridges_1to2.find(topic_name) != bridges_1to2.end()) { + if (publisher_count > 0) { + --publisher_count; + } + } + // ignore subscribers from the bridge itself + if (bridges_2to1.find(topic_name) != bridges_2to1.end()) { + if (subscriber_count > 0) { + --subscriber_count; + } + } + + if (publisher_count) { + current_ros2_publishers[topic_name] = topic_type; + } + + if (subscriber_count) { + current_ros2_subscribers[topic_name] = topic_type; + } + + if (output_topic_introspection) { + printf( + " ROS 2: %s (%s) [%zu pubs, %zu subs]\n", + topic_name.c_str(), topic_type.c_str(), publisher_count, subscriber_count); + } + } + + // collect available services (not clients) + std::set service_names; + std::vector> node_names_and_namespaces = + ros2_node->get_node_graph_interface()->get_node_names_and_namespaces(); + for (auto & pair : node_names_and_namespaces) { + if (pair.first == ros2_node->get_name() && pair.second == ros2_node->get_namespace()) { + continue; + } + + std::map> services_and_types = + ros2_node->get_service_names_and_types_by_node(pair.first, pair.second); + for (auto & it : services_and_types) { + if (services_1_to_2_list.find(it.first) == services_1_to_2_list.end()) { + continue; + } + service_names.insert(it.first); + } + } + + auto ros2_services_and_types = ros2_node->get_service_names_and_types(); + std::map> active_ros2_services; + for (const auto & service_and_types : ros2_services_and_types) { + auto & service_name = service_and_types.first; + auto & service_type = service_and_types.second[0]; // explicitly take the first + + // explicitly avoid services with more than one type + if (service_and_types.second.size() > 1) { + if (already_ignored_services.count(service_name) == 0) { + std::string types = ""; + for (auto type : service_and_types.second) { + types += type + ", "; + } + fprintf( + stderr, + "warning: ignoring service '%s', which has more than one type: [%s]\n", + service_name.c_str(), + types.substr(0, types.length() - 2).c_str() + ); + already_ignored_services.insert(service_name); + } + continue; + } + + // TODO(wjwwood): this should be common functionality in the C++ rosidl package + size_t separator_position = service_type.find('/'); + if (separator_position == std::string::npos) { + fprintf(stderr, "invalid service type '%s', skipping...\n", service_type.c_str()); + continue; + } + + // only bridge if there is a service, not for a client + if (service_names.find(service_name) != service_names.end()) { + auto service_type_package_name = service_type.substr(0, separator_position); + auto service_type_srv_name = service_type.substr(separator_position + 1); + active_ros2_services[service_name]["package"] = service_type_package_name; + active_ros2_services[service_name]["name"] = service_type_srv_name; + } + } + + { + std::lock_guard lock(g_bridge_mutex); + ros2_services = active_ros2_services; + } + + if (output_topic_introspection) { + printf("\n"); + } + + { + std::lock_guard lock(g_bridge_mutex); + ros2_publishers = current_ros2_publishers; + ros2_subscribers = current_ros2_subscribers; + } + + update_bridge( + ros1_node, ros2_node, + ros1_publishers, ros1_subscribers, + ros2_publishers, ros2_subscribers, + ros1_services, ros2_services, + bridges_1to2, bridges_2to1, + service_bridges_1_to_2, service_bridges_2_to_1, + bridge_all_1to2_topics, bridge_all_2to1_topics); + }; + + auto ros2_poll_timer = ros2_node->create_wall_timer( + std::chrono::seconds(1), ros2_poll); + + + // 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); + } + + return 0; +} diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index d6fcd7b8..c9f1e2fc 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include @@ -30,17 +32,213 @@ #include "ros1_bridge/bridge.hpp" +rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) +{ + auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + printf("Qos("); + + if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) { + if (qos_params.hasMember("history")) { + auto history = static_cast(qos_params["history"]); + printf("history: "); + if (history == "keep_all") { + ros2_publisher_qos.keep_all(); + printf("keep_all, "); + } else if (history == "keep_last") { + if (qos_params.hasMember("depth")) { + auto depth = static_cast(qos_params["depth"]); + ros2_publisher_qos.keep_last(depth); + printf("keep_last(%i), ", depth); + } else { + fprintf( + stderr, + "history: keep_last requires that also a depth is set\n"); + } + } else { + fprintf( + stderr, + "invalid value for 'history': '%s', allowed values are 'keep_all'," + "'keep_last' (also requires 'depth' to be set)\n", + history.c_str()); + } + } + + if (qos_params.hasMember("reliability")) { + auto reliability = static_cast(qos_params["reliability"]); + printf("reliability: "); + if (reliability == "best_effort") { + ros2_publisher_qos.best_effort(); + printf("best_effort, "); + } else if (reliability == "reliable") { + ros2_publisher_qos.reliable(); + printf("reliable, "); + } else { + fprintf( + stderr, + "invalid value for 'reliability': '%s', allowed values are 'best_effort', 'reliable'\n", + reliability.c_str()); + } + } + + if (qos_params.hasMember("durability")) { + auto durability = static_cast(qos_params["durability"]); + printf("durability: "); + if (durability == "transient_local") { + ros2_publisher_qos.transient_local(); + printf("transient_local, "); + } else if (durability == "volatile") { + ros2_publisher_qos.durability_volatile(); + printf("volatile, "); + } else { + fprintf( + stderr, + "invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n", + durability.c_str()); + } + } + + if (qos_params.hasMember("deadline")) { + try { + rclcpp::Duration dur = rclcpp::Duration( + static_cast(qos_params["deadline"]["secs"]), + static_cast(qos_params["deadline"]["nsecs"])); + ros2_publisher_qos.deadline(dur); + printf("deadline: Duration(nsecs: %ld), ", dur.nanoseconds()); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to parse deadline: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { + fprintf( + stderr, + "failed to parse deadline: '%s'\n", + e.getMessage().c_str()); + } + } + + if (qos_params.hasMember("lifespan")) { + try { + rclcpp::Duration dur = rclcpp::Duration( + static_cast(qos_params["lifespan"]["secs"]), + static_cast(qos_params["lifespan"]["nsecs"])); + ros2_publisher_qos.lifespan(dur); + printf("lifespan: Duration(nsecs: %ld), ", dur.nanoseconds()); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to parse lifespan: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { + fprintf( + stderr, + "failed to parse lifespan: '%s'\n", + e.getMessage().c_str()); + } + } + + if (qos_params.hasMember("liveliness")) { + if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) { + try { + auto liveliness = static_cast(qos_params["liveliness"]); + ros2_publisher_qos.liveliness(static_cast(liveliness)); + printf("liveliness: %i, ", static_cast(liveliness)); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to parse liveliness: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { + fprintf( + stderr, + "failed to parse liveliness: '%s'\n", + e.getMessage().c_str()); + } + } else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) { + try { + rmw_qos_liveliness_policy_t liveliness = + rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + auto liveliness_str = static_cast(qos_params["liveliness"]); + if (liveliness_str == "LIVELINESS_SYSTEM_DEFAULT" || + liveliness_str == "liveliness_system_default") + { + liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + } else if (liveliness_str == "LIVELINESS_AUTOMATIC" || // NOLINT + liveliness_str == "liveliness_automatic") + { + liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + } else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || // NOLINT + liveliness_str == "liveliness_manual_by_topic") + { + liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + } else { + fprintf( + stderr, + "invalid value for 'liveliness': '%s', allowed values are " + "LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", + liveliness_str.c_str()); + } + + ros2_publisher_qos.liveliness(liveliness); + printf("liveliness: %s, ", liveliness_str.c_str()); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to parse liveliness: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { + fprintf( + stderr, + "failed to parse liveliness: '%s'\n", + e.getMessage().c_str()); + } + } else { + fprintf( + stderr, + "failed to parse liveliness, parameter was not a string or int \n"); + } + } + + if (qos_params.hasMember("liveliness_lease_duration")) { + try { + rclcpp::Duration dur = rclcpp::Duration( + static_cast(qos_params["liveliness_lease_duration"]["secs"]), + static_cast(qos_params["liveliness_lease_duration"]["nsecs"])); + ros2_publisher_qos.liveliness_lease_duration(dur); + printf("liveliness_lease_duration: Duration(nsecs: %ld), ", dur.nanoseconds()); + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to parse liveliness_lease_duration: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { + fprintf( + stderr, + "failed to parse liveliness_lease_duration: '%s'\n", + e.getMessage().c_str()); + } + } + } else { + fprintf( + stderr, + "QoS parameters could not be read\n"); + } + + printf(")"); + return ros2_publisher_qos; +} 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"); + // ROS 1 node + ros::init(argc, argv, "ros_bridge"); + ros::NodeHandle ros1_node; + std::list all_handles; std::list service_bridges_1_to_2; std::list service_bridges_2_to_1; @@ -52,6 +250,8 @@ int main(int argc, char * argv[]) // type: the type of the topic to bridge (e.g. 'pkgname/msg/MsgName') // queue_size: the queue size to use (default: 100) const char * topics_parameter_name = "topics"; + const char * topics_1_to_2_parameter_name = "topics_1_to_2"; + const char * topics_2_to_1_parameter_name = "topics_2_to_1"; // the services parameters need to be arrays // and each item needs to be a dictionary with the following keys; // topic: the name of the topic to bridge (e.g. '/service_name') @@ -62,10 +262,16 @@ int main(int argc, char * argv[]) topics_parameter_name = argv[1]; } if (argc > 2) { - services_1_to_2_parameter_name = argv[2]; + topics_1_to_2_parameter_name = argv[2]; } if (argc > 3) { - services_2_to_1_parameter_name = argv[3]; + topics_2_to_1_parameter_name = argv[3]; + } + if (argc > 4) { + services_1_to_2_parameter_name = argv[4]; + } + if (argc > 5) { + services_2_to_1_parameter_name = argv[5]; } // Topics @@ -87,9 +293,18 @@ int main(int argc, char * argv[]) topic_name.c_str(), type_name.c_str()); try { - ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( - ros1_node, ros2_node, "", type_name, topic_name, queue_size); - all_handles.push_back(handles); + if (topics[i].hasMember("qos")) { + printf("Setting up QoS for '%s': ", topic_name.c_str()); + auto qos_settings = qos_from_params(topics[i]["qos"]); + printf("\n"); + ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); + all_handles.push_back(handles); + } else { + ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + ros1_node, ros2_node, "", type_name, topic_name, queue_size); + all_handles.push_back(handles); + } } catch (std::runtime_error & e) { fprintf( stderr, @@ -104,6 +319,112 @@ int main(int argc, char * argv[]) "The parameter '%s' either doesn't exist or isn't an array\n", topics_parameter_name); } + // Topics 1 to 2 + XmlRpc::XmlRpcValue topics_1_to_2; + if ( + ros1_node.getParam(topics_1_to_2_parameter_name, topics_1_to_2) && + topics_1_to_2.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (size_t i = 0; i < static_cast(topics_1_to_2.size()); ++i) { + std::string topic_name = static_cast(topics_1_to_2[i]["topic"]); + std::string type_name = static_cast(topics_1_to_2[i]["type"]); + size_t queue_size = static_cast(topics_1_to_2[i]["queue_size"]); + if (!queue_size) { + queue_size = 100; + } + printf( + "Trying to create bidirectional bridge for topic '%s' " + "with ROS 2 type '%s'\n", + topic_name.c_str(), type_name.c_str()); + + try { + RCLCPP_INFO(ros2_node->get_logger(), "create topic_1_to_2 bridge for topic " + topic_name); + ros1_bridge::BridgeHandles handles; + if (topics_1_to_2[i].hasMember("qos")) { + printf("Setting up QoS for '%s': ", topic_name.c_str()); + auto qos_settings = qos_from_params(topics_1_to_2[i]["qos"]); + printf("\n"); + // ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + // ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); + handles.bridge1to2 = ros1_bridge::create_bridge_from_1_to_2( + ros1_node, ros2_node, + "", topic_name, queue_size, type_name, topic_name, qos_settings); + all_handles.push_back(handles); + } else { + // ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + // ros1_node, ros2_node, "", type_name, topic_name, queue_size); + handles.bridge1to2 = ros1_bridge::create_bridge_from_1_to_2( + ros1_node, ros2_node, + "", topic_name, queue_size, type_name, topic_name, queue_size); + all_handles.push_back(handles); + } + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to create topics_1_to_2 bridge for topic '%s' " + "with ROS 2 type '%s': %s\n", + topic_name.c_str(), type_name.c_str(), e.what()); + } + } + } else { + fprintf( + stderr, + "The parameter '%s' either doesn't exist or isn't an array\n", topics_1_to_2_parameter_name); + } + + // Topics 2 to 1 + XmlRpc::XmlRpcValue topics_2_to_1; + if ( + ros1_node.getParam(topics_2_to_1_parameter_name, topics_2_to_1) && + topics_2_to_1.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (size_t i = 0; i < static_cast(topics_2_to_1.size()); ++i) { + std::string topic_name = static_cast(topics_2_to_1[i]["topic"]); + std::string type_name = static_cast(topics_2_to_1[i]["type"]); + size_t queue_size = static_cast(topics_2_to_1[i]["queue_size"]); + if (!queue_size) { + queue_size = 100; + } + printf( + "Trying to create bidirectional bridge for topic '%s' " + "with ROS 2 type '%s'\n", + topic_name.c_str(), type_name.c_str()); + + try { + RCLCPP_INFO(ros2_node->get_logger(), "create topic_2_to_1 bridge for topic " + topic_name); + ros1_bridge::BridgeHandles handles; + if (topics_2_to_1[i].hasMember("qos")) { + printf("Setting up QoS for '%s': ", topic_name.c_str()); + auto qos_settings = qos_from_params(topics_2_to_1[i]["qos"]); + printf("\n"); + // ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + // ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); + handles.bridge2to1 = ros1_bridge::create_bridge_from_2_to_1( + ros2_node, ros1_node, + type_name, topic_name, queue_size, "", topic_name, queue_size); + all_handles.push_back(handles); + } else { + // ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + // ros1_node, ros2_node, "", type_name, topic_name, queue_size); + handles.bridge2to1 = ros1_bridge::create_bridge_from_2_to_1( + ros2_node, ros1_node, + type_name, topic_name, queue_size, "", topic_name, queue_size); + all_handles.push_back(handles); + } + } catch (std::runtime_error & e) { + fprintf( + stderr, + "failed to create topics_2_to_1 bridge for topic '%s' " + "with ROS 2 type '%s': %s\n", + topic_name.c_str(), type_name.c_str(), e.what()); + } + } + } else { + fprintf( + stderr, + "The parameter '%s' either doesn't exist or isn't an array\n", topics_2_to_1_parameter_name); + } + // ROS 1 Services in ROS 2 XmlRpc::XmlRpcValue services_1_to_2; if (