From 4e75c63db6ba5a52d438c1b649c37f3fa62a84ca Mon Sep 17 00:00:00 2001 From: Lucyanno Frota Date: Thu, 4 May 2023 19:31:00 +0100 Subject: [PATCH] Port of the commit (ec44770) to foxy branch Just reproduces the changes made in https://github.com/ros2/ros1_bridge/pull/331 in ros foxy. Parametrize Quality of Service in `parameter_bridge` Signed-off-by: Lucyanno Frota --- CMakeLists.txt | 1 + README.md | 117 ++++++++++++ bin/ros1_bridge_generate_factories | 0 include/ros1_bridge/bridge.hpp | 10 + include/ros1_bridge/factory.hpp | 0 include/ros1_bridge/factory_interface.hpp | 0 package.xml | 1 + src/bridge.cpp | 24 +++ src/parameter_bridge.cpp | 213 +++++++++++++++++++++- 9 files changed, 363 insertions(+), 3 deletions(-) mode change 100755 => 100644 bin/ros1_bridge_generate_factories mode change 100755 => 100644 include/ros1_bridge/bridge.hpp mode change 100755 => 100644 include/ros1_bridge/factory.hpp mode change 100755 => 100644 include/ros1_bridge/factory_interface.hpp mode change 100755 => 100644 src/bridge.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b303dc77..46e073ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(xmlrpcpp REQUIRED) # find ROS 1 packages set(cmake_extras_files cmake/find_ros1_package.cmake cmake/find_ros1_interface_packages.cmake) 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 old mode 100755 new mode 100644 index c0485df9..8809852f --- 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/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp old mode 100755 new mode 100644 diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp old mode 100755 new mode 100644 diff --git a/package.xml b/package.xml index 2be7aade..94b43b9a 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ rcutils rmw_implementation_cmake std_msgs + xmlrpcpp pkg-config diff --git a/src/bridge.cpp b/src/bridge.cpp old mode 100755 new mode 100644 index ec682acc..2186b1e7 --- 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/parameter_bridge.cpp b/src/parameter_bridge.cpp index d6fcd7b8..4a296020 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,6 +32,202 @@ #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[]) { @@ -87,9 +285,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,