From 57b8418324d71b9952907772e2a583b6ad3363f5 Mon Sep 17 00:00:00 2001 From: David Revay Date: Wed, 11 Dec 2024 11:07:32 +1100 Subject: [PATCH] feat: add `bestEffortQosTopicWhiteList` --- README.md | 6 +---- .../include/foxglove_bridge/param_utils.hpp | 3 +-- .../foxglove_bridge/ros2_foxglove_bridge.hpp | 2 +- ros2_foxglove_bridge/src/param_utils.cpp | 21 ++++++++++-------- .../src/ros2_foxglove_bridge.cpp | 22 ++++++++----------- 5 files changed, 24 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index d72cf5b..e1d581a 100644 --- a/README.md +++ b/README.md @@ -84,11 +84,7 @@ Parameters are provided to configure the behavior of the bridge. These parameter * (ROS 2) __num_threads__: The number of threads to use for the ROS node executor. This controls the number of subscriptions that can be processed in parallel. 0 means one thread per CPU core. Defaults to `0`. * (ROS 2) __min_qos_depth__: Minimum depth used for the QoS profile of subscriptions. Defaults to `1`. This is to set a lower limit for a subscriber's QoS depth which is computed by summing up depths of all publishers. See also [#208](https://github.com/foxglove/ros-foxglove-bridge/issues/208). * (ROS 2) __max_qos_depth__: Maximum depth used for the QoS profile of subscriptions. Defaults to `25`. - * (ROS 2) __qos_reliability__: The default QoS reliability setting for subscriptions the bridge creates. Defaults to `automatic`. - * `reliable`: ALWAYS subscribe with a "reliable" QoS profile. - * `best_effort`: ALWAYS subscribe with a "best effort" QoS profile. - * `best_effort_if_volatile`: subscribe as "best effort" if all the participants are "volatile". If any are "transient_local", subscribe as "reliable". - * `automatic`: Subscribes as "reliable" if all publishers are reliable. Otherwise, if mixed reliability, subscribes as "best effort" to maxmize compatibility. + * (ROS 2) __best_effort_qos_topic_whitelist__: List of regular expressions (ECMAScript) for topics that should use be forced to use 'best_effort' QoS. Unmatched topics will use 'reliable' QoS if ALL publishers are 'reliable', 'best_effort' if any publishers are 'best_effort'. Defaults to `["(?!)"]` (match nothing). * (ROS 2) __include_hidden__: Include hidden topics and services. Defaults to `false`. * (ROS 2) __disable_load_message__: Do not publish as loaned message when publishing a client message. Defaults to `true`. diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/param_utils.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/param_utils.hpp index b513ac5..b56ed11 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/param_utils.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/param_utils.hpp @@ -16,7 +16,7 @@ constexpr char PARAM_CERTFILE[] = "certfile"; constexpr char PARAM_KEYFILE[] = "keyfile"; constexpr char PARAM_MIN_QOS_DEPTH[] = "min_qos_depth"; constexpr char PARAM_MAX_QOS_DEPTH[] = "max_qos_depth"; -constexpr char PARAM_QOS_RELIABILITY[] = "qos_reliability"; +constexpr char PARAM_BEST_EFFORT_QOS_TOPIC_WHITELIST[] = "best_effort_qos_topic_whitelist"; constexpr char PARAM_TOPIC_WHITELIST[] = "topic_whitelist"; constexpr char PARAM_SERVICE_WHITELIST[] = "service_whitelist"; constexpr char PARAM_PARAMETER_WHITELIST[] = "param_whitelist"; @@ -32,7 +32,6 @@ constexpr char DEFAULT_ADDRESS[] = "0.0.0.0"; constexpr int64_t DEFAULT_SEND_BUFFER_LIMIT = 10000000; constexpr int64_t DEFAULT_MIN_QOS_DEPTH = 1; constexpr int64_t DEFAULT_MAX_QOS_DEPTH = 25; -constexpr char DEFAULT_QOS_RELIABILITY[] = "automatic"; void declareParameters(rclcpp::Node* node); diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp index 55b0c67..b2091e7 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp @@ -62,6 +62,7 @@ class FoxgloveBridge : public rclcpp::Node { std::vector _topicWhitelistPatterns; std::vector _serviceWhitelistPatterns; std::vector _assetUriAllowlistPatterns; + std::vector _bestEffortQosTopicWhiteListPatterns; std::shared_ptr _paramInterface; std::unordered_map _advertisedTopics; std::unordered_map _advertisedServices; @@ -77,7 +78,6 @@ class FoxgloveBridge : public rclcpp::Node { std::unique_ptr _rosgraphPollThread; size_t _minQosDepth = DEFAULT_MIN_QOS_DEPTH; size_t _maxQosDepth = DEFAULT_MAX_QOS_DEPTH; - std::string _qosReliability = DEFAULT_QOS_RELIABILITY; std::shared_ptr> _clockSubscription; bool _useSimTime = false; std::vector _capabilities; diff --git a/ros2_foxglove_bridge/src/param_utils.cpp b/ros2_foxglove_bridge/src/param_utils.cpp index 7674309..8fb4f60 100644 --- a/ros2_foxglove_bridge/src/param_utils.cpp +++ b/ros2_foxglove_bridge/src/param_utils.cpp @@ -84,15 +84,18 @@ void declareParameters(rclcpp::Node* node) { maxQosDepthDescription.integer_range[0].step = 1; node->declare_parameter(PARAM_MAX_QOS_DEPTH, DEFAULT_MAX_QOS_DEPTH, maxQosDepthDescription); - auto qosReliabilityDescription = rcl_interfaces::msg::ParameterDescriptor{}; - qosReliabilityDescription.name = PARAM_QOS_RELIABILITY; - qosReliabilityDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - qosReliabilityDescription.description = - "The default QoS reliability setting for subscriptions the bridge " - "creates. Can be 'reliable', 'best_effort', 'best_effort_if_volatile', or 'automatic'."; - qosReliabilityDescription.read_only = true; - node->declare_parameter(PARAM_QOS_RELIABILITY, DEFAULT_QOS_RELIABILITY, - qosReliabilityDescription); + auto bestEffortQosTopicWhiteListDescription = rcl_interfaces::msg::ParameterDescriptor{}; + bestEffortQosTopicWhiteListDescription.name = PARAM_BEST_EFFORT_QOS_TOPIC_WHITELIST; + bestEffortQosTopicWhiteListDescription.type = + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + bestEffortQosTopicWhiteListDescription.description = + "List of regular expressions (ECMAScript) for topics that should use be forced to use " + "'best_effort' QoS. Unmatched topics will use 'reliable' QoS if ALL publishers are 'reliable', " + "'best_effort' if any publishers are 'best_effort'."; + bestEffortQosTopicWhiteListDescription.read_only = true; + node->declare_parameter(PARAM_BEST_EFFORT_QOS_TOPIC_WHITELIST, std::vector({"(?!)"}), + bestEffortQosTopicWhiteListDescription); + auto topicWhiteListDescription = rcl_interfaces::msg::ParameterDescriptor{}; topicWhiteListDescription.name = PARAM_TOPIC_WHITELIST; topicWhiteListDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 4cf8300..0c313f5 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -36,7 +36,9 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options) const auto keyfile = this->get_parameter(PARAM_KEYFILE).as_string(); _minQosDepth = static_cast(this->get_parameter(PARAM_MIN_QOS_DEPTH).as_int()); _maxQosDepth = static_cast(this->get_parameter(PARAM_MAX_QOS_DEPTH).as_int()); - _qosReliability = this->get_parameter(PARAM_QOS_RELIABILITY).as_string(); + const auto bestEffortQosTopicWhiteList = + this->get_parameter(PARAM_BEST_EFFORT_QOS_TOPIC_WHITELIST).as_string_array(); + _bestEffortQosTopicWhiteListPatterns = parseRegexStrings(this, bestEffortQosTopicWhiteList); const auto topicWhiteList = this->get_parameter(PARAM_TOPIC_WHITELIST).as_string_array(); _topicWhitelistPatterns = parseRegexStrings(this, topicWhiteList); const auto serviceWhiteList = this->get_parameter(PARAM_SERVICE_WHITELIST).as_string_array(); @@ -496,17 +498,9 @@ void FoxgloveBridge::subscribe(foxglove::ChannelId channelId, ConnectionHandle c rclcpp::QoS qos{rclcpp::KeepLast(depth)}; - // Handle the QoS reliability setting - if (_qosReliability == "reliable") { - qos.reliable(); - } else if (_qosReliability == "best_effort") { + // Ignore the topic if it is not on the topic whitelist + if (isWhitelisted(topic, _bestEffortQosTopicWhiteListPatterns)) { qos.best_effort(); - } else if (_qosReliability == "best_effort_if_volatile") { - if (durabilityTransientLocalEndpointsCount > 0) { - qos.reliable(); - } else { - qos.best_effort(); - } } else { // If all endpoints are reliable, ask for reliable if (!publisherInfo.empty() && reliabilityReliableEndpointsCount == publisherInfo.size()) { @@ -538,8 +532,10 @@ void FoxgloveBridge::subscribe(foxglove::ChannelId channelId, ConnectionHandle c } if (firstSubscription) { - RCLCPP_INFO(this->get_logger(), "Subscribing to topic \"%s\" (%s) on channel %d", topic.c_str(), - datatype.c_str(), channelId); + RCLCPP_INFO( + this->get_logger(), "Subscribing to topic \"%s\" (%s) on channel %d with reliablity \"%s\"", + topic.c_str(), datatype.c_str(), channelId, + qos.reliability() == rclcpp::ReliabilityPolicy::Reliable ? "reliable" : "best_effort"); } else { RCLCPP_INFO(this->get_logger(), "Adding subscriber #%zu to topic \"%s\" (%s) on channel %d",