Move qos utilities to their own compilation unit

rosbag2_transport/src/rosbag2_transport/qos.cpp

@@ -12,6 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <string>
+#include <vector>
+
+#include "rosbag2_transport/logging.hpp"
+
 #include "qos.hpp"
 
 namespace YAML
@@ -68,3 +73,126 @@ bool convert<rosbag2_transport::Rosbag2QoS>::decode(
  return true;
 }
 } // namespace YAML
+
+namespace rosbag2_transport
+{
+Rosbag2QoS Rosbag2QoS::adapt_request_to_offers(
+ const std::string & topic_name, const std::vector<rclcpp::TopicEndpointInfo> & endpoints)
+{
+ static const Rosbag2QoS default_request{};
+ if (endpoints.empty()) {
+ return default_request;
+ }
+ size_t num_endpoints = endpoints.size();
+ size_t reliability_reliable_endpoints_count = 0;
+ size_t durability_transient_local_endpoints_count = 0;
+ for (const auto & endpoint : endpoints) {
+ const auto & profile = endpoint.qos_profile().get_rmw_qos_profile();
+ if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) {
+ reliability_reliable_endpoints_count++;
+ }
+ if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) {
+ durability_transient_local_endpoints_count++;
+ }
+ }
+
+ // We set policies in order as defined in rmw_qos_profile_t
+ Rosbag2QoS request_qos = default_request;
+ // Policy: history, depth
+ // History does not affect compatibility
+
+ // Policy: reliability
+ if (reliability_reliable_endpoints_count == num_endpoints) {
+ request_qos.reliable();
+ } else {
+ if (reliability_reliable_endpoints_count > 0) {
+ ROSBAG2_TRANSPORT_LOG_WARN_STREAM(
+ "Some, but not all, publishers on topic \"" << topic_name << "\" "
+ "are offering RMW_QOS_POLICY_RELIABILITY_RELIABLE. "
+ "Falling back to RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT "
+ "as it will connect to all publishers. "
+ "Some messages from Reliable publishers could be dropped.");
+ }
+ request_qos.best_effort();
+ }
+
+ // Policy: durability
+ // If all publishers offer transient_local, we can request it and receive latched messages
+ if (durability_transient_local_endpoints_count == num_endpoints) {
+ request_qos.transient_local();
+ } else {
+ if (durability_transient_local_endpoints_count > 0) {
+ ROSBAG2_TRANSPORT_LOG_WARN_STREAM(
+ "Some, but not all, publishers on topic \"" << topic_name << "\" "
+ "are offering RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. "
+ "Falling back to RMW_QOS_POLICY_DURABILITY_VOLATILE "
+ "as it will connect to all publishers. "
+ "Previously-published latched messages will not be retrieved.");
+ }
+ request_qos.durability_volatile();
+ }
+ // Policy: deadline
+ // Deadline does not affect delivery of messages,
+ // and we do not record Deadline"Missed events.
+ // We can always use unspecified deadline, which will be compatible with all publishers.
+
+ // Policy: lifespan
+ // Lifespan does not affect compatibiliy
+
+ // Policy: liveliness, liveliness_lease_duration
+ // Liveliness does not affect delivery of messages,
+ // and we do not record LivelinessChanged events.
+ // We can always use unspecified liveliness, which will be compatible with all publishers.
+ return request_qos;
+}
+
+namespace
+{
+bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs)
+{
+ return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec;
+}
+
+bool all_profiles_effectively_same(const std::vector<Rosbag2QoS> & profiles)
+{
+ auto iterator = profiles.begin();
+ const auto p_ref = iterator->get_rmw_qos_profile();
+ iterator++;
+ for (; iterator != profiles.end(); iterator++) {
+ const auto p_next = iterator->get_rmw_qos_profile();
+ bool compatibility_equals_previous = (
+ // excluding history
+ p_ref.reliability == p_next.reliability &&
+ p_ref.durability == p_next.durability &&
+ p_ref.deadline == p_next.deadline &&
+ // excluding lifespan
+ p_ref.liveliness == p_next.liveliness &&
+ p_ref.liveliness_lease_duration == p_next.liveliness_lease_duration
+ );
+ if (!compatibility_equals_previous) {
+ return false;
+ }
+ }
+ return true;
+}
+} // unnamed namespace
+
+Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers(
+ const std::string & topic_name, const std::vector<Rosbag2QoS> & profiles)
+{
+ static const Rosbag2QoS default_offer{};
+ if (profiles.empty()) {
+ return default_offer;
+ }
+ if (all_profiles_effectively_same(profiles)) {
+ auto result = profiles[0];
+ return result.default_history();
+ }
+
+ ROSBAG2_TRANSPORT_LOG_WARN_STREAM(
+ "Not all original publishers on topic " << topic_name << " offered the same QoS profiles. "
+ "Rosbag2 cannot yet choose an adapted profile to offer for this mixed case. "
+ "Falling back to the rosbag2_transport default publisher offer.");
+ return default_offer;
+}
+} // namespace rosbag2_transport

rosbag2_transport/src/rosbag2_transport/qos.hpp

@@ -15,6 +15,10 @@
 #ifndef ROSBAG2_TRANSPORT__QOS_HPP_
 #define ROSBAG2_TRANSPORT__QOS_HPP_
 
+#include <string>
+#include <vector>
+
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/qos.hpp"
 
 #ifdef _WIN32
@@ -46,6 +50,30 @@ class Rosbag2QoS : public rclcpp::QoS
  keep_last(rmw_qos_profile_default.depth);
  return *this;
  }
+
+ // Create an adaptive QoS profile to use for subscribing to a set of offers from publishers.
+ /**
+ * - Uses rosbag2_transport defaults for History since they do not affect compatibility.
+ * - Adapts Durability and Reliability, falling back to the least strict publisher when there
+ * is a mixed offer. This behavior errs on the side of forming connections with all publishers.
+ * - Does not specify Lifespan, Deadline, or Liveliness to be maximally compatible, because
+ * these policies do not affect message delivery.
+ */
+ static Rosbag2QoS adapt_request_to_offers(
+ const std::string & topic_name,
+ const std::vector<rclcpp::TopicEndpointInfo> & endpoints);
+
+ // Create a QoS profile to offer for playback.
+ /**
+ * This logic exists because rosbag2 does not record on a per-publisher basis, so we try to
+ * get as close as possible to the original system's behavior, given a single publisher.
+ * If all profiles are the same (excepting History & Lifespan, which are purely local),
+ * that exact value is returned.
+ * Otherwise, fall back to the rosbag2 default and emit a warning.
+ */
+ static Rosbag2QoS adapt_offer_to_recorded_offers(
+ const std::string & topic_name,
+ const std::vector<Rosbag2QoS> & profiles);
 };
 } // namespace rosbag2_transport
 

rosbag2_transport/src/rosbag2_transport/recorder.cpp

@@ -201,77 +201,9 @@ rclcpp::QoS Recorder::subscription_qos_for_topic(const std::string & topic_name)
  if (topic_qos_profile_overrides_.count(topic_name)) {
  ROSBAG2_TRANSPORT_LOG_INFO_STREAM("Overriding subscription profile for " << topic_name);
  return topic_qos_profile_overrides_.at(topic_name);
- } else {
- return adapt_qos_to_publishers(topic_name);
  }
- return adapt_qos_to_publishers(topic_name);
-}
-
-rclcpp::QoS Recorder::adapt_qos_to_publishers(const std::string & topic_name) const
-{
- auto endpoints = node_->get_publishers_info_by_topic(topic_name);
- size_t num_endpoints = endpoints.size();
- size_t reliability_reliable_endpoints_count = 0;
- size_t durability_transient_local_endpoints_count = 0;
- for (const auto & endpoint : endpoints) {
- const auto & profile = endpoint.qos_profile().get_rmw_qos_profile();
- if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) {
- reliability_reliable_endpoints_count++;
- }
- if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) {
- durability_transient_local_endpoints_count++;
- }
- }
-
- // We set policies in order as defined in rmw_qos_profile_t
- Rosbag2QoS request_qos;
- // Policy: history, depth
- // History does not affect compatibility
- request_qos.default_history();
-
- // Policy: reliability
- if (reliability_reliable_endpoints_count == num_endpoints) {
- request_qos.reliable();
- } else {
- if (reliability_reliable_endpoints_count > 0) {
- ROSBAG2_TRANSPORT_LOG_WARN_STREAM(
- "Some, but not all, publishers on topic \"" << topic_name << "\" "
- "are offering RMW_QOS_POLICY_RELIABILITY_RELIABLE. "
- "Falling back to RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT "
- "as it will connect to all publishers. "
- "Some messages from Reliable publishers could be dropped.");
- }
- request_qos.best_effort();
- }
-
- // Policy: durability
- // If all publishers offer transient_local, we can request it and receive latched messages
- if (durability_transient_local_endpoints_count == num_endpoints) {
- request_qos.transient_local();
- } else {
- if (durability_transient_local_endpoints_count > 0) {
- ROSBAG2_TRANSPORT_LOG_WARN_STREAM(
- "Some, but not all, publishers on topic \"" << topic_name << "\" "
- "are offering RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. "
- "Falling back to RMW_QOS_POLICY_DURABILITY_VOLATILE "
- "as it will connect to all publishers. "
- "Previously-published latched messages will not be retrieved.");
- }
- request_qos.durability_volatile();
- }
- // Policy: deadline
- // Deadline does not affect delivery of messages,
- // and we do not record Deadline"Missed events.
- // We can always use unspecified deadline, which will be compatible with all publishers.
-
- // Policy: lifespan
- // Lifespan does not affect compatibiliy
-
- // Policy: liveliness, liveliness_lease_duration
- // Liveliness does not affect delivery of messages,
- // and we do not record LivelinessChanged events.
- // We can always use unspecified liveliness, which will be compatible with all publishers.
- return request_qos;
+ return Rosbag2QoS::adapt_request_to_offers(
+ topic_name, node_->get_publishers_info_by_topic(topic_name));
 }
 
 void Recorder::warn_if_new_qos_for_subscribed_topic(const std::string & topic_name)

rosbag2_transport/src/rosbag2_transport/recorder.hpp

@@ -88,21 +88,13 @@ class Recorder
  /**
  * Find the QoS profile that should be used for subscribing.
  *
- * Profiles are prioritized by:
- * 1. The override specified in the record_options, if one exists for the topic.
- * 2. Adapted subscription via `adapt_qos_to_publishers`
+ * Uses the override from record_options, if it is specified for this topic.
+ * Otherwise, falls back to Rosbag2QoS::adapt_request_to_offers
  *
  * \param topic_name The full name of the topic, with namespace (ex. /arm/joint_status).
  * \return The QoS profile to be used for subscribing.
  */
  rclcpp::QoS subscription_qos_for_topic(const std::string & topic_name) const;
- /**
- * Try to subscribe using publishers' offered QoS policies.
- *
- * Fall back to sensible defaults when we can't adapt robustly,
- * erring in favor of creating compatible connections.
- */
- rclcpp::QoS adapt_qos_to_publishers(const std::string & topic_name) const;
 
  // Serialize all currently offered QoS profiles for a topic into a YAML list.
  std::string serialized_offered_qos_profiles_for_topic(const std::string & topic_name);