From b8f8a4f5bfa2d5bf646c260f69775aea76a1a61c Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Wed, 22 Jun 2022 02:16:23 +0200 Subject: [PATCH] Parametrize Quality of Service in `parameter_bridge`. (#331) * Extend create_bidirectional_bridge to take qos param for ROS2 publisher Signed-off-by: Loy van Beek * Busy setting up a way to read QoS parameters from ROS1 params Signed-off-by: Loy van Beek * Parse history qos params Signed-off-by: Loy van Beek * Call qos_from_params when setting up topics Signed-off-by: Loy van Beek * Configure deadline, lifespan, liveliness_lease_durations Signed-off-by: Loy van Beek * Configure liveliness Signed-off-by: Loy van Beek * Add some basic debug text Signed-off-by: Loy van Beek * Print the QoS settings to stdout when setting them up Signed-off-by: Loy van Beek * Catch XmlRpc::XmlRpcExceptions when constructing QoS from parameters Signed-off-by: Loy van Beek * Parse liveliness as either int enum value or upper/lower case string representation of liveliness enum values Signed-off-by: Loy van Beek * Fix formatting with uncrustify Signed-off-by: Loy van Beek * Fix cpplint formatting In 2 cases the formatting conflicts with what uncrustify wants; // NOLINT got rid of the complaints of cpplint, favouring uncrustify Signed-off-by: Loy van Beek * Clearer logging as suggested by code review Co-authored-by: Geoffrey Biggs Signed-off-by: Loy van Beek * Clarify keep_last vs keep_all setting for history Signed-off-by: Loy van Beek Co-authored-by: Geoffrey Biggs --- CMakeLists.txt | 1 + README.md | 44 +++++++ include/ros1_bridge/bridge.hpp | 10 ++ package.xml | 1 + src/bridge.cpp | 24 ++++ src/parameter_bridge.cpp | 213 ++++++++++++++++++++++++++++++++- 6 files changed, 290 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e133278..f329e41f 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 14ce2efc..b48a53fe 100644 --- a/README.md +++ b/README.md @@ -458,3 +458,47 @@ source ${ROS2_INSTALL_PATH}/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. 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 39146ed6..025b57b0 100644 --- a/package.xml +++ b/package.xml @@ -29,6 +29,7 @@ rcutils rmw_implementation_cmake std_msgs + xmlrpcpp pkg-config diff --git a/src/bridge.cpp b/src/bridge.cpp index b8f0fa83..def61466 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -139,4 +139,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 79531a8f..a95d49a4 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[]) { @@ -89,9 +287,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,