依据ros2 bag play的实现方式实现一个基于QT界面的rosbag播放器

具体代码如下：
1、rosbag_player.h

//
// Created by Yao Guanghua  on 2023/6/2.
//

#ifndef HDMAP_TOOLS_ROSBAG_PLAYER_H
#define HDMAP_TOOLS_ROSBAG_PLAYER_H


#include <QApplication>
#include <QFileDialog>
#include <QHBoxLayout>
#include <QObject>
#include <QProgressBar>
#include <QPushButton>
#include <QSlider>
#include <QVBoxLayout>
#include <QWidget>
#include <QFormLayout>
#include <QLineEdit>
#include <QDateTime>
#include <QLabel>
#include <QKeyEvent>
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/reader.hpp>
#include <rosbag2_cpp/readers/sequential_reader.hpp>
#include <rosbag2_cpp/storage_options.hpp>
#include <rosbag2_cpp/typesupport_helpers.hpp>
#include <rosbag2_cpp/clocks/player_clock.hpp>
#include <rosbag2_cpp/clocks/time_controller_clock.hpp>
#include "rosgraph_msgs/msg/clock.hpp"
#include <chrono>
#include "rcutils/time.h"
#include "rosbag2_transport/play_options.hpp"
#include "rosbag2_transport/player.hpp"
#include "qos.hpp"
#include "yaml-cpp/yaml.h"



template <typename T>
static void DeserializerDb3RosMsg(std::shared_ptr<rosbag2_storage::SerializedBagMessage> & serialized_message, T &msg){
    rclcpp::SerializedMessage serialized_data(*serialized_message->serialized_data);
    rclcpp::Serialization<T> serialization;
    serialization.deserialize_message(&serialized_data, &msg);
}

static QString NanoSecToDateTime(int64_t time_stamp) {
    QDateTime date_time = QDateTime::fromMSecsSinceEpoch(time_stamp / 1000000);
    QString cur_date_time =date_time.date().toString(Qt::ISODate) + " " +date_time.time().toString(Qt::ISODateWithMs);
    return cur_date_time;
}


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. 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 rosbag2_storage::TopicMetadata & topic,
            const std::unordered_map<std::string, rclcpp::QoS> & topic_qos_profile_overrides,
            const rclcpp::Logger & logger)
    {
        using rosbag2_transport::Rosbag2QoS;
        auto qos_it = topic_qos_profile_overrides.find(topic.name);
        if (qos_it != topic_qos_profile_overrides.end()) {
            RCLCPP_INFO_STREAM(
                    logger,
                    "Overriding QoS profile for topic " << topic.name);
            return Rosbag2QoS{qos_it->second};
        } else if (topic.offered_qos_profiles.empty()) {
            return Rosbag2QoS{};
        }

        const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles);
        const auto offered_qos_profiles = profiles_yaml.as<std::vector<Rosbag2QoS>>();
        return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles);
    }
} // namespace




//class BagPlayerNode : public QWidget, public QObject {
class BagPlayerNode : public QWidget {
    Q_OBJECT

public:
    BagPlayerNode(const std::string &nodeName);
    ~BagPlayerNode();




private slots:

    void openBag();

    void seekToTime();

    void pauseAndContinuePlayback();

    void pausePlayback();

    void resumePlayback();

    void startPlayback();

    bool set_rate(double rate);

    void TopicsToPlay();

    void prepare_publishers();

    void publish_clock_update();

    void publish_clock_update(const rclcpp::Time & time);

    void play();

    void load_storage_content();

    void enqueue_up_to_boundary(uint64_t boundary);

    void wait_for_filled_queue() const;

    bool is_storage_completely_loaded() const;

    void play_messages_from_queue();

    bool publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message);

    rosbag2_storage::SerializedBagMessageSharedPtr * peek_next_message_from_queue();

    // Publish the message on each topic.
    void PublishTopic();

    protected:
        std::atomic<bool> playing_messages_from_queue_{false};
        // clcpp::GenericPublisher是ROS 2中的一个类，用于创建通用发布者。
        // 通用发布者可以发布任何类型的消息，而不需要指定具体的消息类型。它是在运行时动态创建的，并且可以根据需要更改其发布的消息类型。
        // 这种灵活性使得通用发布者在某些情况下非常有用，例如在开发测试工具或调试程序时。
        std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericPublisher>> publishers_;
        rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_publisher_;



private:
    std::string bagFilePath_;
    QSlider *progressSlider_;
    QProgressBar *progressBar_;
    QPushButton *pauseButton_;
    QPushButton *openButton_;
    QFormLayout * formLayout_;
    QLineEdit *line_edit_topics_;
    QLabel* q_time_label_;
    QTimer* q_timer_;
    double playbackRate_;
    bool isPlaying_;
    std::unique_ptr<rosbag2_cpp::readers::SequentialReader> reader_;
    rosbag2_cpp::ConverterOptions converter_options{};
    rosbag2_cpp::StorageOptions storage_options_{};
    rosbag2_transport::PlayOptions play_options_;
    std::unique_ptr<rosbag2_cpp::Reader> better_reader_;
    std::shared_ptr<rclcpp::Node> node_;
    rcutils_time_point_value_t rosbag_starting_time_;
    rcutils_time_point_value_t rosbag_ending_time_;
    std::chrono::nanoseconds rosbag_duration_;
    std::shared_ptr<rclcpp::TimerBase> clock_publish_timer_;
    std::unique_ptr<rosbag2_cpp::PlayerClock> clock_;
    bool start_paused_ = false;
    rcutils_time_point_value_t start_offset_ = 0;
    bool h264_decode_enabled_ = false;
    double clock_publish_frequency_ = 0.0;
    bool loop_ = false;
    std::vector<std::string> topics_to_filter_ = {};
    std::string node_prefix_ = "";
    float rate_ = 1.0;
    std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides_;

    const std::chrono::milliseconds queue_read_wait_period_ = std::chrono::milliseconds(100);
    moodycamel::ReaderWriterQueue<rosbag2_storage::SerializedBagMessageSharedPtr> message_queue_;
    mutable std::future<void> storage_loading_future_;
    std::atomic_bool is_in_play_{false};
    std::mutex skip_message_in_main_play_loop_mutex_;
    bool skip_message_in_main_play_loop_ RCPPUTILS_TSA_GUARDED_BY
            (skip_message_in_main_play_loop_mutex_) = false;

    static constexpr double read_ahead_lower_bound_percentage_ = 0.9;




};


#endif//HDMAP_TOOLS_ROSBAG_PLAYER_H

2、rosbag_player.cpp

//
// Created by Yao Guanghua  on 2023/6/2.
//

#include "rosbag_player.h"


BagPlayerNode::BagPlayerNode(const std::string &nodeName)
        : QWidget(nullptr),
          playbackRate_(0.8),
          isPlaying_(false){
    q_time_label_ = new QLabel;
    q_time_label_->setText("00:00:00");
    pauseButton_ = new QPushButton("Pause");
//    pauseButton_->installEventFilter(&event_filter_);
    openButton_ = new QPushButton("Open Bag");
    progressSlider_ = new QSlider(Qt::Horizontal);
    QLabel* topics_label = new QLabel;
    topics_label->setText("topics to play:");
    topics_label->setMinimumWidth(80);
    progressBar_ = new QProgressBar;
    line_edit_topics_ = new QLineEdit;
    line_edit_topics_->setPlaceholderText("topics to play, use space to split each topic");
    line_edit_topics_->setClearButtonEnabled(true);
    node_ = rclcpp::Node::make_shared(nodeName, nodeName);
    reader_ = std::make_unique<rosbag2_cpp::readers::SequentialReader>();
    connect(progressSlider_, &QSlider::sliderMoved, this, &BagPlayerNode::pausePlayback);
    connect(progressSlider_, &QSlider::sliderPressed, this, &BagPlayerNode::pausePlayback);
    connect(progressSlider_, &QSlider::sliderReleased, this, &BagPlayerNode::resumePlayback);
    connect(progressSlider_, &QSlider::valueChanged, progressBar_, &QProgressBar::setValue);
    connect(pauseButton_, &QPushButton::clicked, this, &BagPlayerNode::pauseAndContinuePlayback);
    connect(openButton_, &QPushButton::clicked, this, &BagPlayerNode::openBag);
    connect(line_edit_topics_, &QLineEdit::editingFinished, this, &BagPlayerNode::TopicsToPlay);

    QHBoxLayout *buttonLayout = new QHBoxLayout;
    buttonLayout->addWidget(openButton_);
    buttonLayout->addWidget(pauseButton_);

    QHBoxLayout *progressLayout = new QHBoxLayout;
    progressLayout->addWidget(progressSlider_);
    progressLayout->addWidget(q_time_label_);
    progressLayout->addWidget(progressBar_);

    QHBoxLayout *topics_layout = new QHBoxLayout;
    topics_layout->addWidget(topics_label, 1);
    topics_layout->addWidget(line_edit_topics_, 10);

    QVBoxLayout *layout = new QVBoxLayout;
    layout->addLayout(buttonLayout);
    layout->addLayout(topics_layout);
    layout->addLayout(progressLayout);

    QWidget *window = new QWidget;
    window->setWindowTitle("rosbag player");
    window->resize(1000,500);
    window->setLayout(layout);
    window->show();

}

BagPlayerNode::~BagPlayerNode(){
    if(reader_) {
        reader_->close();
    }
}



void BagPlayerNode::openBag() {
    QString filePath = QFileDialog::getOpenFileName(nullptr, "Open Bag File", "/zdrive", "ROS2 Bag Files (*.db3)");
    if (!filePath.isEmpty()) {
        bagFilePath_ = filePath.toStdString();
        startPlayback();
    }
}

void BagPlayerNode::TopicsToPlay() {

    QString text = this->line_edit_topics_->text();
    if(text !=""){
//        std::cout<<"the following topics will be played: "<<std::endl;
        RCLCPP_INFO_STREAM(node_->get_logger(), "the following topics will be played: ");
        play_options_.topics_to_filter.clear();
        QByteArray byte = text.toLatin1();
        QList<QByteArray> byte_list = byte.split(' ');
        for(int i=0; i<byte_list.length(); ++i){
            play_options_.topics_to_filter.emplace_back(byte_list.at(i).toStdString());
//            std::cout<<"topic to play "<<i<<" = "<<byte_list.at(i).toStdString()<<std::endl;
            const char *topic = byte_list.at(i);
            RCLCPP_INFO_STREAM(node_->get_logger(), topic);
        }
    }
    if(bagFilePath_ != ""){
        prepare_publishers();
}

}


void BagPlayerNode::seekToTime() {
//        qint64 duration = reader_->get_metadata().duration.count();// nanoseconds
    qint64 targetTime = (progressSlider_->value() * rosbag_duration_.count() / progressSlider_->maximum()) + rosbag_starting_time_;
    int64_t closest_time;
    if(reader_) {
        reader_->close();
    };
    reader_->open(storage_options_, converter_options);
    while (reader_->has_next() && closest_time < targetTime) {
        auto message = reader_->read_next();
        closest_time = message->time_stamp;
    }
    clock_->jump(closest_time);


//        PublishTopic();

}

void BagPlayerNode::pauseAndContinuePlayback() {
    if (bagFilePath_ == ""){
        std::cout<<"the rosbag is not exist, please add rosbag first..."<<std::endl;
    } else {
        isPlaying_ = !isPlaying_;
        if(isPlaying_) {
            std::cout<<"continue to play rosbag... "<<std::endl;
            pauseButton_->setText("pause");
            clock_->resume();
            PublishTopic();
        } else {
            pauseButton_->setText("continue");
            clock_->pause();
        }

    }

}


void BagPlayerNode::pausePlayback() {
    if (bagFilePath_ == ""){
        std::cout<<"the rosbag is not exist, please add rosbag first..."<<std::endl;
    } else {
        isPlaying_ = false;
        clock_->pause();
        int64_t cur_nanosec = static_cast<int64_t>(progressSlider_->value()) * 1000000 +
                              static_cast<uint64_t>(rosbag_starting_time_); //ms
        q_time_label_->setText(NanoSecToDateTime(cur_nanosec));
    }



}

void BagPlayerNode::resumePlayback() {
    if (bagFilePath_ == ""){
        std::cout<<"the rosbag is not exist, please add rosbag first..."<<std::endl;
    } else {
        qint64 targetTime = (progressSlider_->value() * rosbag_duration_.count() / progressSlider_->maximum()) +
                            rosbag_starting_time_;
        int64_t closest_time;
        if (reader_) {
            reader_->close();
        };
        reader_->open(storage_options_, converter_options);
        while (reader_->has_next() && closest_time < targetTime) {
            auto message = reader_->read_next();
            closest_time = message->time_stamp;
        }
        clock_->jump(closest_time);

        isPlaying_ = true;
        clock_->resume();
        PublishTopic();
    }
}

void BagPlayerNode::startPlayback() {
    // 创建ROS bag读取器
    isPlaying_ = true;

    storage_options_.uri = bagFilePath_;
//    std::cout << "bagFilePath=" << bagFilePath_ << std::endl;
    RCLCPP_INFO_STREAM(node_->get_logger(), "bagFilePath=" << bagFilePath_ );
    storage_options_.storage_id = "sqlite3";
    converter_options.input_serialization_format = "cdr";
    converter_options.output_serialization_format = "cdr";
    rclcpp::Time start_time = node_->get_clock()->now();
    RCLCPP_INFO_STREAM(node_->get_logger(), "begin to open rosbag ======>");
//    reader_->open(storage_options_, {"", rmw_get_serialization_format()});
    reader_->open(storage_options_, converter_options);
    rclcpp::Time end_time = node_->get_clock()->now();
    auto duration = end_time - start_time;
    RCLCPP_INFO_STREAM(node_->get_logger(), "success open rosbag, const time:"<<duration.seconds());

    auto topics = reader_->get_all_topics_and_types();

    rosbag_starting_time_ = reader_->get_metadata().starting_time.time_since_epoch().count(); // nano
    rosbag_duration_ = reader_->get_metadata().duration;
    rosbag_ending_time_ = rosbag_starting_time_ + rosbag_duration_.count();
    progressSlider_->setRange(0, static_cast<int64_t>(rosbag_duration_.count() / 1000000)); // ms
    progressBar_->setRange(0, static_cast<int64_t>(rosbag_duration_.count() / 1000000));
    std::cout << "this rosbag total duration time is: " << static_cast<int64_t>(rosbag_duration_.count() / 1000000) <<"ms"<< std::endl; //ms
    clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(
            rosbag_starting_time_, std::chrono::steady_clock::now,
            std::chrono::milliseconds{100}, start_paused_);

    set_rate(rate_);
    topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides;
    prepare_publishers();
//        reader_->close();

/*
        for (const auto &topic_name : topics) {
            //            reader_->select_topic(topic.name, topic.type);
            const auto qos = rclcpp::QoS{10};
            if (topic_name.name == "/map/calib_params") {
                //                publishers_.push_back(node_->create_publisher<sdk_msgs::msg::CalibParams>(topic_name.name, qos));
                std::cout<<"topic_name.type = "<<topic_name.type<<std::endl;
                std::cout<<"topic_name.serialization_format = "<<topic_name.serialization_format<<std::endl;
                std::cout<<"topic_name.offered_qos_profiles = "<<topic_name.offered_qos_profiles<<std::endl;
                calib_params_pub_ = node_->create_publisher<sdk_msgs::msg::CalibParams>(topic_name.name, qos);
            } else if (topic_name.name == "/loc/ego_tf") {
                //                publishers_.push_back(node_->create_publisher<interfaces::msg::Localization>(topic_name.name, qos));
                ego_tf_pub_ = node_->create_publisher<interfaces::msg::Localization>(topic_name.name, qos);
//                ego_tf_pub_ = node_->create_publisher<topic_name.type>(topic_name.name, qos);
            } else if (topic_name.name == "/loc/ego_tf_relative") {
                //                publishers_.push_back(node_->create_publisher<interfaces::msg::RelativeLocalization>(topic_name.name, qos));
                ego_tf_relative_pub_ = node_->create_publisher<interfaces::msg::RelativeLocalization>(topic_name.name, qos);
            }
        }
        PublishTopic();
        */
    clock_->jump(rosbag_starting_time_);
    PublishTopic();


}

bool BagPlayerNode::set_rate(double rate)
{
    bool ok = clock_->set_rate(rate);
    if (ok) {
        RCLCPP_INFO_STREAM(node_->get_logger(), "Set rate to " << rate);
    } else {
        RCLCPP_WARN_STREAM(node_->get_logger(), "Failed to set rate to invalid value " << rate);
    }
    return ok;
}


void BagPlayerNode::prepare_publishers()
{
    rosbag2_storage::StorageFilter storage_filter;
    storage_filter.topics = play_options_.topics_to_filter;
    reader_->set_filter(storage_filter);

    // Create /clock publisher
    if (play_options_.clock_publish_frequency > 0.f) {

        const auto publish_period = std::chrono::nanoseconds(
                static_cast<uint64_t>(RCUTILS_S_TO_NS(1) / play_options_.clock_publish_frequency));
        if(clock_publisher_ != nullptr) {
            clock_publisher_ = node_->create_publisher<rosgraph_msgs::msg::Clock>(
                    "/clock", rclcpp::ClockQoS());
        }

        if(clock_publish_timer_ != nullptr) {
            clock_publish_timer_ = node_->create_wall_timer(
                    publish_period, [this]() {
                        auto msg = rosgraph_msgs::msg::Clock();
                        msg.clock = rclcpp::Time(clock_->now());
                        clock_publisher_->publish(msg);
                    });
        }

    }

    // Create topic publishers
    publishers_.clear();
    auto topics = reader_->get_all_topics_and_types();
    std::string topic_without_support_acked;
    for (const auto & topic : topics) {
        if (publishers_.find(topic.name) != publishers_.end()) {
            continue;
        }
        // filter topics to add publishers if necessary
        auto & filter_topics = storage_filter.topics;
        if (!filter_topics.empty()) {
            auto iter = std::find(filter_topics.begin(), filter_topics.end(), topic.name);
            if (iter == filter_topics.end()) {
                continue;
            }
        }

        auto topic_qos = publisher_qos_for_topic(
                topic, topic_qos_profile_overrides_,
                node_->get_logger());
        try {
            publishers_.insert(
                    std::make_pair(
                            topic.name, node_->create_generic_publisher(topic.name, topic.type, topic_qos)));
        } catch (const std::runtime_error & e) {
            // using a warning log seems better than adding a new option
            // to ignore some unknown message type library
            RCLCPP_WARN(
                    node_->get_logger(),
                    "Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what());
        }
    }

}


void BagPlayerNode::publish_clock_update()
{
    publish_clock_update(rclcpp::Time(clock_->now()));
}

void BagPlayerNode::publish_clock_update(const rclcpp::Time & time)
{
    if (clock_publisher_->can_loan_messages()) {
        auto loaned_timestamp{clock_publisher_->borrow_loaned_message()};
        loaned_timestamp.get().clock = time;
        clock_publisher_->publish(std::move(loaned_timestamp));
    } else {
        rosgraph_msgs::msg::Clock timestamp;
        timestamp.clock = time;
        clock_publisher_->publish(timestamp);
    }
}

void BagPlayerNode::play()
{
    is_in_play_ = true;

    float delay;
    if (play_options_.delay >= 0.0) {
        delay = play_options_.delay;
    } else {
        RCLCPP_WARN(
                node_->get_logger(),
                "Invalid delay value: %f. Delay is disabled.",
                play_options_.delay);
        delay = 0.0;
    }

    try {
        do {
            if (delay > 0.0) {
                RCLCPP_INFO_STREAM(node_->get_logger(), "Sleep " << delay << " sec");
                std::chrono::duration<float> duration(delay);
                std::this_thread::sleep_for(duration);
            }
            reader_->open(storage_options_, {"", rmw_get_serialization_format()});
            const auto starting_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
                    reader_->get_metadata().starting_time.time_since_epoch()).count();
            clock_->jump(starting_time);

            storage_loading_future_ = std::async(
                    std::launch::async,
                    [this]() {load_storage_content();});

            wait_for_filled_queue();
            play_messages_from_queue();
            reader_->close();
        } while (rclcpp::ok() && play_options_.loop);
    } catch (std::runtime_error & e) {
        RCLCPP_ERROR(node_->get_logger(), "Failed to play: %s", e.what());
    }
    is_in_play_ = false;
}

void BagPlayerNode::load_storage_content()
{
    auto queue_lower_boundary =
            static_cast<size_t>(play_options_.read_ahead_queue_size * read_ahead_lower_bound_percentage_);
    auto queue_upper_boundary = play_options_.read_ahead_queue_size;

    while (reader_->has_next() && rclcpp::ok()) {
        if (message_queue_.size_approx() < queue_lower_boundary) {
            enqueue_up_to_boundary(queue_upper_boundary);
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
}

void BagPlayerNode::enqueue_up_to_boundary(uint64_t boundary)
{
    rosbag2_storage::SerializedBagMessageSharedPtr message;
    for (size_t i = message_queue_.size_approx(); i < boundary; i++) {
        if (!reader_->has_next()) {
            break;
        }
        message = reader_->read_next();
        message_queue_.enqueue(message);
    }
}

void BagPlayerNode::wait_for_filled_queue() const
{
    while (
            message_queue_.size_approx() < play_options_.read_ahead_queue_size &&
            !is_storage_completely_loaded() && rclcpp::ok())
    {
        std::this_thread::sleep_for(queue_read_wait_period_);
    }
}

bool BagPlayerNode::is_storage_completely_loaded() const
{
    if (storage_loading_future_.valid() &&
        storage_loading_future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) // 检查异步任务是否已完成，但不会阻塞当前线程。使用 std::chrono::seconds(0) 表示在检查时不等待任何时间。
    {
        storage_loading_future_.get();
    }
    return !storage_loading_future_.valid();
}





void BagPlayerNode::play_messages_from_queue()
{
    playing_messages_from_queue_ = true;
    // Note: We need to use message_queue_.peek() instead of message_queue_.try_dequeue(message)
    // to support play_next() API logic.
    rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = peek_next_message_from_queue();
    while (message_ptr != nullptr && rclcpp::ok()) {
        {
            rosbag2_storage::SerializedBagMessageSharedPtr message = *message_ptr;
            // Do not move on until sleep_until returns true
            // It will always sleep, so this is not a tight busy loop on pause
            while (rclcpp::ok() && !clock_->sleep_until(message->time_stamp)) {}
            if (rclcpp::ok()) {
                {
                    std::lock_guard<std::mutex> lk(skip_message_in_main_play_loop_mutex_);
                    if (skip_message_in_main_play_loop_) {
                        skip_message_in_main_play_loop_ = false;
                        message_ptr = peek_next_message_from_queue();
                        continue;
                    }
                }
                publish_message(message);
            }
            message_queue_.pop();
            message_ptr = peek_next_message_from_queue();
        }
    }
    playing_messages_from_queue_ = false;
}

bool BagPlayerNode::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message)
{
    bool message_published = false;
    auto publisher_iter = publishers_.find(message->topic_name);
    if (publisher_iter != publishers_.end()) {
        publisher_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data));
        message_published = true;
    }
    return message_published;
}

rosbag2_storage::SerializedBagMessageSharedPtr * BagPlayerNode::peek_next_message_from_queue()
{
    rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = message_queue_.peek();
    if (message_ptr == nullptr && !is_storage_completely_loaded() && rclcpp::ok()) {
        RCLCPP_WARN(
                node_->get_logger(),
                "Message queue starved. Messages will be delayed. Consider "
                "increasing the --read-ahead-queue-size option.");
        while (message_ptr == nullptr && !is_storage_completely_loaded() && rclcpp::ok()) {
            std::this_thread::sleep_for(std::chrono::microseconds(100));
            message_ptr = message_queue_.peek();
        }
    }
    return message_ptr;
}




// Publish the message on each topic.
/*
void PublishTopic1(){
//        std::cout<<"----------in Publish Topic function-------------------"<<std::endl;
//        if(isPlaying_) {
        rclcpp::Rate r(300);
        while (reader_->has_next() && isPlaying_) {
            std::shared_ptr<rosbag2_storage::SerializedBagMessage> serialized_message = reader_->read_next();
            progressSlider_->setValue(static_cast<int64_t>(serialized_message->time_stamp - rosbag_starting_time_)/1000000000);
            std::cout<<"serialized_message->topic_name="<<serialized_message->topic_name<<std::endl;
            std::cout<<"serialized_message->time_stamp= "<<serialized_message->time_stamp<<", rosbag_starting_time_="<<rosbag_starting_time_<<std::endl;
            std::cout<<"progressSlider_->setValue = " <<(static_cast<int64_t>(serialized_message->time_stamp - rosbag_starting_time_)/1000000000)<<std::endl;
            if (serialized_message->topic_name == "/map/calib_params") {
                sdk_msgs::msg::CalibParams msg;
                DeserializerDb3RosMsg(serialized_message, msg);
                calib_params_pub_->publish(msg);
            } else if (serialized_message->topic_name == "/loc/ego_tf") {
                interfaces::msg::Localization msg;
                DeserializerDb3RosMsg(serialized_message, msg);
                ego_tf_pub_->publish(msg);
            } else if (serialized_message->topic_name == "/loc/ego_tf_relative") {
                interfaces::msg::RelativeLocalization msg;
                DeserializerDb3RosMsg(serialized_message, msg);
                ego_tf_relative_pub_->publish(msg);
            }

            r.sleep();
            QCoreApplication::processEvents();
        }
        std::cout<<"ending to play rosbag ...... "<<std::endl;
//        }
}
 */


// Publish the message on each topic.
void BagPlayerNode::PublishTopic(){
    while (reader_->has_next() && isPlaying_) {
        std::shared_ptr<rosbag2_storage::SerializedBagMessage> serialized_message = reader_->read_next();
        progressSlider_->setValue(static_cast<int64_t>(serialized_message->time_stamp - rosbag_starting_time_)/1000000);
        q_time_label_->setText(NanoSecToDateTime(serialized_message->time_stamp));

//        std::cout<<"progressSlider_->setValue = " <<(static_cast<int64_t>(serialized_message->time_stamp - rosbag_starting_time_)/1000000000)<<std::endl;
        while (rclcpp::ok() && !clock_->sleep_until(serialized_message->time_stamp)) {}
        publish_message(serialized_message);
//            std::cout<<"serialized_message->topic_name="<<serialized_message->topic_name<<std::endl;
//            std::cout<<"serialized_message->time_stamp= "<<serialized_message->time_stamp<<", rosbag_starting_time_="<<rosbag_starting_time_<<std::endl;
        QCoreApplication::processEvents();

    }
    if(!reader_->has_next()){
        std::cout<<"finished to play rosbag... "<<std::endl;
    } else {
        std::cout<<"pause to play rosbag... "<<std::endl;
    }


}

3、rosbag_player_main.cpp

#include "rosbag_player.h"

int main(int argc, char** argv) {
    QApplication app(argc, argv);
    rclcpp::init(argc, argv);
    BagPlayerNode bagPlayerNode("rosbag_player");
    return app.exec();
}

//#include "rosbag_player.moc"

4、CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(rosbag_player)

set(CMAKE_CXX_STANDARD 17)

set(rosbag_player_install_dir /zdrive/hdmap/tools/install/)
# 查找Qt库
find_package(Qt5 COMPONENTS Widgets REQUIRED)

# 查找ROS 2包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(std_msgs REQUIRED)
#if(rosbag2_cpp_FOUND)
#    message(STATUS "rosbag2_cpp_LIBRARIES = " ${rosbag2_cpp_LIBRARIES})
#    message(STATUS "rosbag2_cpp_INCLUDE_DIRS = " ${rosbag2_cpp_INCLUDE_DIRS})
#endif()

# yaml-cpp
include_directories(/zdrive/hdmap/tools/thirdparty/yaml_cpp_vendor/include)
link_directories(t/zdrive/hdmap/tools/hirdparty/yaml_cpp_vendor/lib)

include_directories( ${rosbag2_cpp_INCLUDE_DIRS})

# 添加Qt头文件路径
include_directories(${Qt5Widgets_INCLUDE_DIRS})

# 添加Qt编译选项
add_definitions(${Qt5Widgets_DEFINITIONS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${Qt5Widgets_EXECUTABLE_COMPILE_FLAGS}")
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTORCC ON)
set(CMAKE_AUTOUIC ON)

# 添加可执行文件
add_executable(rosbag_player rosbag_player_main.cpp rosbag_player.cpp qos.cpp test.rc)

target_link_libraries(rosbag_player
        Qt5::Widgets
        ${rosbag2_cpp_LIBRARIES}
        ${rclcpp_LIBRARIES}
        ${std_msgs_LIBRARIES}
        yaml-cpp
        )

# 安装可执行文件
install(TARGETS rosbag_player
        DESTINATION ${rosbag_player_install_dir}/bin)

直接运行rosbag_player:

./rosbag_player

rosbag_player播放器如图所示：