diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 00000000..1d6aa37b --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,19 @@ +name: CI + +on: [push, pull_request] # on all pushes and PRs + +jobs: + ros1_bridge: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@v0.1 + with: + required-ros-distributions: "noetic rolling" + - name: Build and test ros1-bridge + uses: ros-tooling/action-ros-ci@v0.2 + with: + package-name: ros1_bridge + target-ros1-distro: noetic + target-ros2-distro: rolling + diff --git a/CHANGELOG.rst b/CHANGELOG.rst index bcad06cb..2c495407 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,52 @@ Changelog for package ros1_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.10.1 (2021-01-25) +------------------- +* Fix logging for updated rclcpp interface (`#303 `_) +* Fix typo in comments (`#297 `_) +* Contributors: Michael Carroll, Vicidel + +0.9.5 (2020-12-08) +------------------ +* Update to use rosidl_parser and .idl files rather than rosidl_adapter and .msg files (`#296 `_) +* Update maintainers (`#286 `_) +* Contributors: Jacob Perron, William Woodall + +0.9.4 (2020-09-10) +------------------ +* use hardcoded QoS (keep all, transient local) for /tf_static topic in dynamic_bridge (`#282 `_) +* document explicitly passing the topic type to 'ros2 topic echo' (`#279 `_) + +0.9.3 (2020-07-07) +------------------ +* Fix multiple definition if message with same name as service exists (`#272 `_) +* Contributors: Dirk Thomas + +0.9.2 (2020-06-01) +------------------ +* When generating service mappings cast pair to set to avoid duplicate pairs (`#268 `_) +* Contributors: Gavin Suddrey + +0.9.1 (2020-05-27) +------------------ +* Deprecate package key for service parameters, use full type instead (`#263 `_) +* Fix removing obsolete ROS 2 service bridges (`#267 `_) +* Gracefully handle invalid ROS 1 publishers (`#266 `_) +* Use reliable publisher in simple bridge (`#264 `_) +* Remove outdated information on Fast RTPS bug (`#260 `_) +* Contributors: Dirk Thomas, Thom747 + +0.9.0 (2020-05-18) +------------------ +* Avoid new deprecations (`#255 `_) +* Updates since changes to message_info in rclcpp (`#253 `_) +* Assert ROS 1 nodes' stdout (`#247 `_) +* Ignore actionlib_msgs deprecation warning (`#245 `_) +* Drop workaround for https://github.com/ros2/rmw_fastrtps/issues/265. (`#233 `_) +* Code style only: wrap after open parenthesis if not in one line (`#238 `_) +* Contributors: Dirk Thomas, Jacob Perron, Michel Hidalgo, William Woodall + 0.8.2 (2020-01-17) ------------------ * fix building test when ROS 1 diagnostic_msgs is isolated from roscpp (`#236 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index b9e6a8ff..b303dc77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,7 +99,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 diff --git a/README.md b/README.md index 78b14f75..f52db7b0 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,11 @@ If you would like to use a bridge with other interfaces (including your own cust See [the documentation](doc/index.rst) for an example setup. For efficiency reasons, topics will only be bridged when matching publisher-subscriber pairs are active for a topic on either side of the bridge. -Note that before `rostopic echo` would work with bridged topics, a subscriber must already exist, in order for `echo` to determine the message type and then to create its own subscriber. -You can use the `--bridge-all-2to1-topics` option to bridge all ROS 2 topics to ROS 1 so that tools such as `rostopic list` and `rqt` will see the topics even if there are no matching ROS 1 subscribers. +As a result using `ros2 topic echo ` doesn't work but fails with an error message `Could not determine the type for the passed topic` if no other subscribers are present since the dynamic bridge hasn't bridged the topic yet. +As a workaround the topic type can be specified explicitly `ros2 topic echo ` which triggers the bridging of the topic since the `echo` command represents the necessary subscriber. +On the ROS 1 side `rostopic echo` doesn't have an option to specify the topic type explicitly. +Therefore it can't be used with the dynamic bridge if no other subscribers are present. +As an alternative you can use the `--bridge-all-2to1-topics` option to bridge all ROS 2 topics to ROS 1 so that tools such as `rostopic echo`, `rostopic list` and `rqt` will see the topics even if there are no matching ROS 1 subscribers. Run `ros2 run ros1_bridge dynamic_bridge -- --help` for more options. ## Prerequisites @@ -174,9 +177,6 @@ Once you stop either the talker or the listener in *shell B* a line will be stat removed 1to2 bridge for topic '/chatter' ``` -*Note:* When the bridge is run using the default ROS 2 middleware implementation, which uses Fast RTPS, it does not always remove bridges instantly. -See [https://github.com/ros2/ros1_bridge/issues/38](https://github.com/ros2/ros1_bridge/issues/38). - The screenshot shows all the shell windows and their expected content: ![ROS 1 talker and ROS 2 listener](doc/ros1_talker_ros2_listener.png) diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index eb2fccff..c0485df9 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -83,6 +83,17 @@ create_bridge_from_1_to_2( const std::string & ros2_topic_name, size_t publisher_queue_size); +Bridge1to2Handles +create_bridge_from_1_to_2( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string & ros1_type_name, + const std::string & ros1_topic_name, + size_t subscriber_queue_size, + const std::string & ros2_type_name, + const std::string & ros2_topic_name, + const rclcpp::QoS & publisher_qos); + Bridge2to1Handles create_bridge_from_2_to_1( rclcpp::Node::SharedPtr ros2_node, @@ -95,6 +106,18 @@ create_bridge_from_2_to_1( size_t publisher_queue_size, rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); +Bridge2to1Handles +create_bridge_from_2_to_1( + rclcpp::Node::SharedPtr ros2_node, + ros::NodeHandle ros1_node, + const std::string & ros2_type_name, + const std::string & ros2_topic_name, + const rclcpp::QoS & subscriber_qos, + const std::string & ros1_type_name, + const std::string & ros1_topic_name, + size_t publisher_queue_size, + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); + BridgeHandles create_bidirectional_bridge( ros::NodeHandle ros1_node, diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index e41b207d..f2d1e34b 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -70,6 +70,15 @@ class Factory : public FactoryInterface { auto qos = rclcpp::QoS(rclcpp::KeepAll()); qos.get_rmw_qos_profile() = qos_profile; + return create_ros2_publisher(node, topic_name, qos); + } + + rclcpp::PublisherBase::SharedPtr + create_ros2_publisher( + rclcpp::Node::SharedPtr node, + const std::string & topic_name, + const rclcpp::QoS & qos) + { return node->create_publisher(topic_name, qos); } @@ -103,9 +112,8 @@ class Factory : public FactoryInterface ros::Publisher ros1_pub, rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) { - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; - custom_qos_profile.depth = queue_size; - return create_ros2_subscriber(node, topic_name, custom_qos_profile, ros1_pub, ros2_pub); + auto qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + return create_ros2_subscriber(node, topic_name, qos, ros1_pub, ros2_pub); } rclcpp::SubscriptionBase::SharedPtr @@ -115,18 +123,30 @@ class Factory : public FactoryInterface const rmw_qos_profile_t & qos, ros::Publisher ros1_pub, rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) + { + auto rclcpp_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)); + rclcpp_qos.get_rmw_qos_profile() = qos; + return create_ros2_subscriber( + node, topic_name, rclcpp_qos, ros1_pub, ros2_pub); + } + + rclcpp::SubscriptionBase::SharedPtr + create_ros2_subscriber( + rclcpp::Node::SharedPtr node, + const std::string & topic_name, + const rclcpp::QoS & qos, + ros::Publisher ros1_pub, + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) { std::function< void(const typename ROS2_T::SharedPtr msg, const rclcpp::MessageInfo & msg_info)> callback; callback = std::bind( &Factory::ros2_callback, std::placeholders::_1, std::placeholders::_2, ros1_pub, ros1_type_name_, ros2_type_name_, node->get_logger(), ros2_pub); - auto rclcpp_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)); - rclcpp_qos.get_rmw_qos_profile() = qos; rclcpp::SubscriptionOptions options; options.ignore_local_publications = true; return node->create_subscription( - topic_name, rclcpp_qos, callback, options); + topic_name, qos, callback, options); } void convert_1_to_2(const void * ros1_msg, void * ros2_msg) override @@ -214,6 +234,16 @@ class Factory : public FactoryInterface } } + void * ptr = ros1_pub; + if (ptr == 0) { + RCLCPP_WARN_ONCE( + logger, + "Message from ROS 2 %s failed to be passed to ROS 1 %s because the " + "ROS 1 publisher is invalid (showing msg only once per type)", + ros2_type_name.c_str(), ros1_type_name.c_str()); + return; + } + ROS1_T ros1_msg; convert_2_to_1(*ros2_msg, ros1_msg); RCLCPP_INFO_ONCE( diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index 595e559d..ee568d4d 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -67,6 +67,13 @@ class FactoryInterface const std::string & topic_name, const rmw_qos_profile_t & qos_profile) = 0; + virtual + rclcpp::PublisherBase::SharedPtr + create_ros2_publisher( + rclcpp::Node::SharedPtr node, + const std::string & topic_name, + const rclcpp::QoS & qos) = 0; + virtual ros::Subscriber create_ros1_subscriber( @@ -94,6 +101,15 @@ class FactoryInterface ros::Publisher ros1_pub, rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; + virtual + rclcpp::SubscriptionBase::SharedPtr + create_ros2_subscriber( + rclcpp::Node::SharedPtr node, + const std::string & topic_name, + const rclcpp::QoS & qos, + ros::Publisher ros1_pub, + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; + virtual void convert_1_to_2(const void * ros1_msg, void * ros2_msg) = 0; diff --git a/package.xml b/package.xml index c6f27a3a..40fc76b6 100644 --- a/package.xml +++ b/package.xml @@ -2,11 +2,14 @@ ros1_bridge - 0.8.2 + 0.10.1 A simple bridge between ROS 1 and ROS 2 - Dirk Thomas + Jacob Perron + Shane Loretz Apache License 2.0 + Dirk Thomas + ament_cmake ament_index_python python3-catkin-pkg-modules 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 9463b344..1e640823 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 @@ -138,15 +139,19 @@ 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': [], + } + 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': [ + 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], - } + 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' % @@ -283,7 +288,7 @@ def get_ros2_messages(): if all(n not in data for n in ('ros1_service_name', 'ros2_service_name')): try: rules.append(MessageMappingRule(data, package_name)) - except Exception as e: + except Exception as e: # noqa: B902 print('%s' % str(e), file=sys.stderr) return pkgs, msgs, rules @@ -336,7 +341,7 @@ def get_ros2_services(): if all(n not in data for n in ('ros1_message_name', 'ros2_message_name')): try: rules.append(ServiceMappingRule(data, package_name)) - except Exception as e: + except Exception as e: # noqa: B902 print('%s' % str(e), file=sys.stderr) return pkgs, srvs, rules @@ -562,17 +567,20 @@ def determine_common_services( for rule in mapping_rules: for ros1_srv in ros1_srvs: for ros2_srv in ros2_srvs: + pair = (ros1_srv, ros2_srv) + if pair in pairs: + continue if rule.ros1_package_name == ros1_srv.package_name and \ rule.ros2_package_name == ros2_srv.package_name: if rule.ros1_service_name is None and rule.ros2_service_name is None: if ros1_srv.message_name == ros2_srv.message_name: - pairs.append((ros1_srv, ros2_srv)) + pairs.append(pair) else: if ( rule.ros1_service_name == ros1_srv.message_name and rule.ros2_service_name == ros2_srv.message_name ): - pairs.append((ros1_srv, ros2_srv)) + pairs.append(pair) for pair in pairs: ros1_spec = load_ros1_service(pair[0]) @@ -677,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) @@ -745,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 @@ -760,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 @@ -794,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): @@ -840,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 2e154b24..b8f0fa83 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -30,10 +30,32 @@ create_bridge_from_1_to_2( const std::string & ros2_type_name, const std::string & ros2_topic_name, size_t publisher_queue_size) +{ + return create_bridge_from_1_to_2( + ros1_node, + ros2_node, + ros1_type_name, + ros1_topic_name, + subscriber_queue_size, + ros2_type_name, + ros2_topic_name, + rclcpp::QoS(rclcpp::KeepLast(publisher_queue_size))); +} + +Bridge1to2Handles +create_bridge_from_1_to_2( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string & ros1_type_name, + const std::string & ros1_topic_name, + size_t subscriber_queue_size, + const std::string & ros2_type_name, + const std::string & ros2_topic_name, + const rclcpp::QoS & publisher_qos) { auto factory = get_factory(ros1_type_name, ros2_type_name); auto ros2_pub = factory->create_ros2_publisher( - ros2_node, ros2_topic_name, publisher_queue_size); + ros2_node, ros2_topic_name, publisher_qos); auto ros1_sub = factory->create_ros1_subscriber( ros1_node, ros1_topic_name, subscriber_queue_size, ros2_pub, ros2_node->get_logger()); @@ -55,13 +77,38 @@ create_bridge_from_2_to_1( const std::string & ros1_topic_name, size_t publisher_queue_size, rclcpp::PublisherBase::SharedPtr ros2_pub) +{ + auto subscriber_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(subscriber_queue_size)); + return create_bridge_from_2_to_1( + ros2_node, + ros1_node, + ros2_type_name, + ros2_topic_name, + subscriber_qos, + ros1_type_name, + ros1_topic_name, + publisher_queue_size, + ros2_pub); +} + +Bridge2to1Handles +create_bridge_from_2_to_1( + rclcpp::Node::SharedPtr ros2_node, + ros::NodeHandle ros1_node, + const std::string & ros2_type_name, + const std::string & ros2_topic_name, + const rclcpp::QoS & subscriber_qos, + const std::string & ros1_type_name, + const std::string & ros1_topic_name, + size_t publisher_queue_size, + rclcpp::PublisherBase::SharedPtr ros2_pub) { auto factory = get_factory(ros1_type_name, ros2_type_name); auto ros1_pub = factory->create_ros1_publisher( ros1_node, ros1_topic_name, publisher_queue_size); auto ros2_sub = factory->create_ros2_subscriber( - ros2_node, ros2_topic_name, subscriber_queue_size, ros1_pub, ros2_pub); + ros2_node, ros2_topic_name, subscriber_qos, ros1_pub, ros2_pub); Bridge2to1Handles handles; handles.ros2_subscriber = ros2_sub; @@ -78,7 +125,9 @@ create_bidirectional_bridge( const std::string & topic_name, size_t queue_size) { - RCLCPP_INFO(ros2_node->get_logger(), "create bidirectional bridge for topic " + topic_name); + 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, diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 2d841607..cd2db0c3 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include // include ROS 1 @@ -37,8 +38,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/scope_exit.hpp" -#include "rcutils/get_env.h" - #include "ros1_bridge/bridge.hpp" @@ -182,11 +181,16 @@ void update_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, 10); + bridge.ros2_type_name, topic_name, ros2_publisher_qos); } catch (std::runtime_error & e) { fprintf( stderr, @@ -695,6 +699,21 @@ int main(int argc, char * argv[]) } } + // 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) { + 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) { @@ -725,12 +744,14 @@ int main(int argc, char * argv[]) fprintf(stderr, "invalid service type '%s', skipping...\n", service_type.c_str()); continue; } - auto service_type_package_name = service_type.substr(0, separator_position); - auto service_type_srv_name = service_type.substr(separator_position + 1); - // TODO(wjwwood): fix bug where just a ros2 client will cause a ros1 service to be made - active_ros2_services[service_name]["package"] = service_type_package_name; - active_ros2_services[service_name]["name"] = service_type_srv_name; + // 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; + } } { diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 508e6275..f4f4bd76 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -46,12 +46,16 @@ int main(int argc, char * argv[]) std::list service_bridges_2_to_1; // bridge all topics listed in a ROS 1 parameter - // the parameter needs to be an array + // 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 - // type: the type of the topic to bridge + // 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; + // service: the name of the service to bridge (e.g. '/service_name') + // type: the type of the service 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) { @@ -108,15 +112,33 @@ int main(int argc, char * argv[]) { for (size_t i = 0; i < static_cast(services_1_to_2.size()); ++i) { std::string service_name = static_cast(services_1_to_2[i]["service"]); - std::string package_name = static_cast(services_1_to_2[i]["package"]); std::string type_name = static_cast(services_1_to_2[i]["type"]); + { + // for backward compatibility + std::string package_name = static_cast(services_1_to_2[i]["package"]); + if (!package_name.empty()) { + fprintf( + stderr, + "The service '%s' uses the key 'package' which is deprecated for " + "services. Instead prepend the 'type' value with '/'.\n", + service_name.c_str()); + type_name = package_name + "/" + type_name; + } + } printf( - "Trying to create bridge for ROS 2 service '%s' " - "with package '%s' and type '%s'\n", - service_name.c_str(), package_name.c_str(), type_name.c_str()); + "Trying to create bridge for ROS 2 service '%s' with type '%s'\n", + service_name.c_str(), type_name.c_str()); + const size_t index = type_name.find("/"); + if (index == std::string::npos) { + fprintf( + stderr, + "the service '%s' has a type '%s' without a slash.\n", + service_name.c_str(), type_name.c_str()); + continue; + } auto factory = ros1_bridge::get_service_factory( - "ros2", package_name, type_name); + "ros2", type_name.substr(0, index), type_name.substr(index + 1)); if (factory) { try { service_bridges_1_to_2.push_back( @@ -126,16 +148,14 @@ int main(int argc, char * argv[]) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create bridge ROS 1 service '%s' " - "with package '%s' and type '%s': %s\n", - service_name.c_str(), type_name.c_str(), type_name.c_str(), e.what()); + "failed to create bridge ROS 1 service '%s' with type '%s': %s\n", + service_name.c_str(), type_name.c_str(), e.what()); } } else { fprintf( stderr, - "failed to create bridge ROS 1 service '%s' " - "no conversion for package '%s' and type '%s'\n", - service_name.c_str(), package_name.c_str(), type_name.c_str()); + "failed to create bridge ROS 1 service '%s' no conversion for type '%s'\n", + service_name.c_str(), type_name.c_str()); } } @@ -154,15 +174,34 @@ int main(int argc, char * argv[]) { for (size_t i = 0; i < static_cast(services_2_to_1.size()); ++i) { std::string service_name = static_cast(services_2_to_1[i]["service"]); - std::string package_name = static_cast(services_2_to_1[i]["package"]); std::string type_name = static_cast(services_2_to_1[i]["type"]); + { + // for backward compatibility + std::string package_name = static_cast(services_2_to_1[i]["package"]); + if (!package_name.empty()) { + fprintf( + stderr, + "The service '%s' uses the key 'package' which is deprecated for " + "services. Instead prepend the 'type' value with '/'.\n", + service_name.c_str()); + type_name = package_name + "/" + type_name; + } + } printf( - "Trying to create bridge for ROS 1 service '%s' " - "with package '%s' and type '%s'\n", - service_name.c_str(), package_name.c_str(), type_name.c_str()); + "Trying to create bridge for ROS 1 service '%s' with type '%s'\n", + service_name.c_str(), type_name.c_str()); + + const size_t index = type_name.find("/"); + if (index == std::string::npos) { + fprintf( + stderr, + "the service '%s' has a type '%s' without a slash.\n", + service_name.c_str(), type_name.c_str()); + continue; + } auto factory = ros1_bridge::get_service_factory( - "ros1", package_name, type_name); + "ros1", type_name.substr(0, index), type_name.substr(index + 1)); if (factory) { try { service_bridges_2_to_1.push_back( @@ -171,16 +210,14 @@ int main(int argc, char * argv[]) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create bridge ROS 2 service '%s' " - "with package '%s' and type '%s': %s\n", - service_name.c_str(), type_name.c_str(), type_name.c_str(), e.what()); + "failed to create bridge ROS 2 service '%s' with type '%s': %s\n", + service_name.c_str(), type_name.c_str(), e.what()); } } else { fprintf( stderr, - "failed to create bridge ROS 2 service '%s' " - "no conversion for package '%s' and type '%s'\n", - service_name.c_str(), package_name.c_str(), type_name.c_str()); + "failed to create bridge ROS 2 service '%s' no conversion for type '%s'\n", + service_name.c_str(), type_name.c_str()); } } diff --git a/src/simple_bridge.cpp b/src/simple_bridge.cpp index c0de28a2..94eb97d0 100644 --- a/src/simple_bridge.cpp +++ b/src/simple_bridge.cpp @@ -82,7 +82,7 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); ros2_pub = ros2_node->create_publisher( - "chatter", rclcpp::SensorDataQoS()); + "chatter", 10); // ROS 1 subscriber ros::Subscriber ros1_sub = ros1_node.subscribe(