diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index c6e1a4be87..29bffb3583 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -44,7 +44,7 @@ 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. @@ -52,14 +52,15 @@ namespace */ rclcpp::QoS publisher_qos_for_topic( const std::string & topic_name, - std::unordered_map topic_qos_profile_overrides) + const std::unordered_map & topic_qos_profile_overrides, + const std::unordered_map> & 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 reader, std::shared_ptr rosbag2_transport) -: reader_(std::move(reader)), rosbag2_transport_(rosbag2_transport) -{} + std::shared_ptr reader, + std::shared_ptr 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>(); + } +} 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( diff --git a/rosbag2_transport/src/rosbag2_transport/player.hpp b/rosbag2_transport/src/rosbag2_transport/player.hpp index cc7a612ae1..63ebd46923 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.hpp +++ b/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; @@ -70,6 +71,7 @@ class Player std::shared_ptr rosbag2_transport_; std::unordered_map> publishers_; std::unordered_map topic_qos_profile_overrides_; + std::unordered_map> recorded_qos_profiles_; }; } // namespace rosbag2_transport