WIP adaptive publish

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -44,22 +44,23 @@ namespace
  * Determine which QoS to offer for a topic.
  * The priority of the profile selected is:
  * 1. The override specified in play_options (if one exists for the topic).
- * 2. The default RMW QoS profile.
+ * 2. A profile automatically adapted to the recorded QoS profiles of publishers on the topic.
  *
  * \param topic_name The full name of the topic, with namespace (ex. /arm/joint_status).
  * \param topic_qos_profile_overrides A map of topic to QoS profile overrides.
  * @return The QoS profile to be used for subscribing.
  */
 rclcpp::QoS publisher_qos_for_topic(
  const std::string & topic_name,
- std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides)
+ const std::unordered_map<std::string, rclcpp::QoS> & topic_qos_profile_overrides,
+ const std::unordered_map<std::string, std::vector<Rosbag2QoS>> & recorded_qos_profiles)
 {
  auto qos_it = topic_qos_profile_overrides.find(topic_name);
  if (qos_it != topic_qos_profile_overrides.end()) {
  ROSBAG2_TRANSPORT_LOG_INFO_STREAM("Overriding QoS profile for topic " << topic_name);
  return rosbag2_transport::Rosbag2QoS{qos_it->second};
  }
- return rosbag2_transport::Rosbag2QoS{};
+ return Rosbag2QoS::adapt_offer_to_recorded_offers(recorded_qos_profiles.at(topic_name));
 }
 } // namespace
 
@@ -70,9 +71,17 @@ const std::chrono::milliseconds
 Player::queue_read_wait_period_ = std::chrono::milliseconds(100);
 
 Player::Player(
- std::shared_ptr<rosbag2_cpp::Reader> reader, std::shared_ptr<Rosbag2Node> rosbag2_transport)
-: reader_(std::move(reader)), rosbag2_transport_(rosbag2_transport)
-{}
+ std::shared_ptr<rosbag2_cpp::Reader> reader,
+ std::shared_ptr<Rosbag2Node> rosbag2_transport,
+ const rosbag2_storage::BagMetadata & metadata)
+: reader_(std::move(reader)), rosbag2_transport_(rosbag2_transport), metadata_(metadata)
+{
+ for (auto topic_information : metadata.topics_with_message_count) {
+ const auto & topic_metadata = topic_information.topic_metadata;
+ const auto & loaded_profiles = YAML::Load(topic_metadata.offered_qos_profiles);
+ recorded_qos_profiles_[topic_metadata.name] = loaded_profiles.as<std::vector<Rosbag2QoS>>();
+ }
+}
 
 bool Player::is_storage_completely_loaded() const
 {
@@ -189,7 +198,8 @@ void Player::prepare_publishers(const PlayOptions & options)
 
  auto topics = reader_->get_all_topics_and_types();
  for (const auto & topic : topics) {
- auto topic_qos = publisher_qos_for_topic(topic.name, topic_qos_profile_overrides_);
+ auto topic_qos = publisher_qos_for_topic(
+ topic.name, topic_qos_profile_overrides_, recorded_qos_profiles_);
  publishers_.insert(
  std::make_pair(
  topic.name, rosbag2_transport_->create_generic_publisher(

rosbag2_transport/src/rosbag2_transport/player.hpp

@@ -28,6 +28,7 @@
 
 #include "rosbag2_transport/play_options.hpp"
 
+#include "qos.hpp"
 #include "replayable_message.hpp"
 
 using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock>;
@@ -70,6 +71,7 @@ class Player
  std::shared_ptr<Rosbag2Node> rosbag2_transport_;
  std::unordered_map<std::string, std::shared_ptr<GenericPublisher>> publishers_;
  std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides_;
+ std::unordered_map<std::string, std::vector<Rosbag2QoS>> recorded_qos_profiles_;
 };
 
 } // namespace rosbag2_transport