Apollo学习之rosbag_storage bag实现

adsdriver

于 2024-05-31 16:25:34 发布

阅读量287

点赞数 6

文章标签：学习 Apollo rosbagstorage 实现 bag

本文链接：https://blog.csdn.net/liuphahaha/article/details/139355980

版权

Apollo学习之rosbag_storage bag实现

附赠自动驾驶最全的学习资料和量产经验：链接

实现

bag包的实现在ros_comm/tools/rosbag_storage/src/bag.cpp中，实现了Bag类。它主要用来读写bag文件，有以下一些接口。

// 1. 打开和关闭文件
void open(std::string const& filename, uint32_t mode = bagmode::Read);
void close();
// 2. 获取文件名、读写模式、版本号和大小
    std::string     getFileName()     const;                      //!< Get the filename of the bag
    BagMode         getMode()         const;                      //!< Get the mode the bag is in
    uint32_t        getMajorVersion() const;                      //!< Get the major-version of the open bag file
    uint32_t        getMinorVersion() const;                      //!< Get the minor-version of the open bag file
    uint64_t        getSize()         const;                      //!< Get the current size of the bag file (a lower bound)
// 3. 设置压缩类型，块阈值
    void            setCompression(CompressionType compression);  //!< Set the compression method to use for writing chunks
    CompressionType getCompression() const;                       //!< Get the compression method to use for writing chunks
    void            setChunkThreshold(uint32_t chunk_threshold);  //!< Set the threshold for creating new chunks
    uint32_t        getChunkThreshold() const;     
// 4. 设置加密插件
void setEncryptorPlugin(const std::string& plugin_name, const std::string& plugin_param = std::string());
// 5. 写消息
    template<class T>
    void write(std::string const& topic, ros::MessageEvent<T> const& event);
// 6. 交换
    void swap(Bag&);
// 7. 是否打开
    bool isOpen() const;

接下来我们主要分析读写接口

读接口

open

通过open接口打开bag包，并且可以设置模式：读、写或者追加写（默认为读）。

void Bag::open(string const& filename, uint32_t mode) {
    mode_ = (BagMode) mode;
    // 1. 根据不同的模式做不同的操作
    if (mode_ & bagmode::Append)
        openAppend(filename);
    else if (mode_ & bagmode::Write)
        openWrite(filename);
    else if (mode_ & bagmode::Read)
        openRead(filename);
    else
        throw BagException((format("Unknown mode: %1%") % (int) mode).str());

    // 2. 设置文件大小和偏移
    uint64_t offset = file_.getOffset();
    seek(0, std::ios::end);
    file_size_ = file_.getOffset();
    seek(offset);
}

因为我们主要是分析读取文件，因此我们接下来看openRead接口。其中读取版本实际上就是读取bag包第一行，前面已经介绍bag第一行的字符串"#ROSBAG V2.0"，代表2.0。

void Bag::openRead(string const& filename) {
    // 打开文件（通过ChunkedFile类）
    file_.openRead(filename);
    // 读取版本
    readVersion();
    // 根据不同版本选择不同的读取函数
    switch (version_) {
    case 102: startReadingVersion102(); break;
    case 200: startReadingVersion200(); break;
    default:
        throw BagException((format("Unsupported bag file version: %1%.%2%") % getMajorVersion() % getMinorVersion()).str());
    }
}

startReadingVersion200

接着我们看如何读取2.0的版本。

void Bag::startReadingVersion200() {
    // 读取文件包头record
    readFileHeaderRecord();

    // Seek to the end of the chunks
    seek(index_data_pos_);

    // 读取connection records
    for (uint32_t i = 0; i < connection_count_; i++)
        readConnectionRecord();

    // 读取chunk info records
    for (uint32_t i = 0; i < chunk_count_; i++)
        readChunkInfoRecord();

    // 读取connection indexes for each chunk
    for (ChunkInfo const& chunk_info : chunks_) {
        curr_chunk_info_ = chunk_info;

        seek(curr_chunk_info_.pos);

        // Skip over the chunk data
        ChunkHeader chunk_header;
        readChunkHeader(chunk_header);
        seek(chunk_header.compressed_size, std::ios::cur);

        // Read the index records after the chunk
        for (unsigned int i = 0; i < chunk_info.connection_counts.size(); i++)
            readConnectionIndexRecord200();
    }

    // At this point we don't have a curr_chunk_info anymore so we reset it
    curr_chunk_info_ = ChunkInfo();
}

写接口

write

写接口做了重载，我们以其中一个为例。

template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> connection_header) {
    doWrite(topic, time, msg, connection_header);
}

doWrite

doWrite主要实现了ConnectionInfo、MessageDataRecord等的写入

template<class T>
void Bag::doWrite(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> const& connection_header) {
    // Whenever we write we increment our revision
    bag_revision_++;

    // Get ID for connection header
    ConnectionInfo* connection_info = NULL;
    uint32_t conn_id = 0;
    if (!connection_header) {
        // 没有connection header: 我们将制造一个，并按topic存储
        std::map<std::string, uint32_t>::iterator topic_connection_ids_iter = topic_connection_ids_.find(topic);
        if (topic_connection_ids_iter == topic_connection_ids_.end()) {
            conn_id = connections_.size();
            topic_connection_ids_[topic] = conn_id;
        }
        else {
            conn_id = topic_connection_ids_iter->second;
            connection_info = connections_[conn_id];
        }
    }
    else {
        // Store the connection info by the address of the connection header

        // Add the topic name to the connection header, so that when we later search by 
        // connection header, we can disambiguate connections that differ only by topic name (i.e.,
        // same callerid, same message type), #3755.  This modified connection header is only used
        // for our bookkeeping, and will not appear in the resulting .bag.
        ros::M_string connection_header_copy(*connection_header);
        connection_header_copy["topic"] = topic;

        std::map<ros::M_string, uint32_t>::iterator header_connection_ids_iter = header_connection_ids_.find(connection_header_copy);
        if (header_connection_ids_iter == header_connection_ids_.end()) {
            conn_id = connections_.size();
            header_connection_ids_[connection_header_copy] = conn_id;
        }
        else {
            conn_id = header_connection_ids_iter->second;
            connection_info = connections_[conn_id];
        }
    }

    {
        // 移动到文件尾
        seek(0, std::ios::end);
        file_size_ = file_.getOffset();

        // Write the chunk header if we're starting a new chunk
        if (!chunk_open_)
            startWritingChunk(time);

        // Write connection info record, if necessary
        if (connection_info == NULL) {
            connection_info = new ConnectionInfo();
            connection_info->id       = conn_id;
            connection_info->topic    = topic;
            connection_info->datatype = std::string(ros::message_traits::datatype(msg));
            connection_info->md5sum   = std::string(ros::message_traits::md5sum(msg));
            connection_info->msg_def  = std::string(ros::message_traits::definition(msg));
            if (connection_header != NULL) {
                connection_info->header = connection_header;
            }
            else {
                connection_info->header = boost::make_shared<ros::M_string>();
                (*connection_info->header)["type"]               = connection_info->datatype;
                (*connection_info->header)["md5sum"]             = connection_info->md5sum;
                (*connection_info->header)["message_definition"] = connection_info->msg_def;
            }
            connections_[conn_id] = connection_info;
            // No need to encrypt connection records in chunks
            writeConnectionRecord(connection_info, false);
            appendConnectionRecordToBuffer(outgoing_chunk_buffer_, connection_info);
        }

        // Add to topic indexes
        IndexEntry index_entry;
        index_entry.time      = time;
        index_entry.chunk_pos = curr_chunk_info_.pos;
        index_entry.offset    = getChunkOffset();

        std::multiset<IndexEntry>& chunk_connection_index = curr_chunk_connection_indexes_[connection_info->id];
        chunk_connection_index.insert(chunk_connection_index.end(), index_entry);

        if (mode_ != BagMode::Write) {
          std::multiset<IndexEntry>& connection_index = connection_indexes_[connection_info->id];
          connection_index.insert(connection_index.end(), index_entry);
        }

        // Increment the connection count
        curr_chunk_info_.connection_counts[connection_info->id]++;

        // Write the message data
        writeMessageDataRecord(conn_id, time, msg);

        // Check if we want to stop this chunk
        uint32_t chunk_size = getChunkOffset();
        CONSOLE_BRIDGE_logDebug("  curr_chunk_size=%d (threshold=%d)", chunk_size, chunk_threshold_);
        if (chunk_size > chunk_threshold_) {
            // Empty the outgoing chunk
            stopWritingChunk();
            outgoing_chunk_buffer_.setSize(0);

            // We no longer have a valid curr_chunk_info
            curr_chunk_info_.pos = -1;
        }
    }
}