Scepter914 Website
Table of contents

RustでRosbag2から画像を抽出するcrate

概要

  • rosbag2のSQLite DBから直接画像を読み込むcrate を作った
  • rosを立ち上げなくてもrosbagの画像を読み込むことができるので、オフライン処理に使える
  • どちらかというとrosbag2の仕様と、RustでのSQLiteの扱い方についての記事

作ったもの

fn main() {
    let args: Vec<String> = std::env::args().collect();

    let file_name: String = args[1].to_string();
    let mut interfaces: Vec<Rosbag2Images> = load_images_from_rosbag2(file_name).unwrap();
    let mut frame_index = 0;
    if !interfaces.is_empty() {
        loop {
            frame_index += 1;
            let input_image = interfaces[0].get_frame();
            if input_image.is_none() {
                break;
            }
            my_image_proc(&input_image.unwrap(), frame_index);
        }
    }
}

作るにあたって

rosbag2について

The first plugin, sqlite3 is chosen by default. If not specified otherwise, rosbag2 will store and replay all recorded data in an SQLite3 database.
  • なのでSQLite3のデータベースとして直接扱うことができる

rosbag2のschema

  • rosbag2の中身は以下の通りになっている
- messages
  - TABLE
    - id INTEGER PRIMARY KEY
    - topic_id INTEGER NOT NULL
    - timestamp INTEGER NOT NULL
    - data BLOB NOT NULL
  - INDEX:  timestamp_idx ON messages (timestamp ASC);
- topics
  - TABLE
    - id INTEGER PRIMARY KEY
    - name TEXT NOT NULL
    - type TEXT NOT NULL
    - serialization_format TEXT NOT NULL
    - offered_qos_profiles TEXT NOT NULL
  • これをRustのstructにすると以下の通り
/// Topic の定義
#[derive(Debug)]
pub struct Topic {
    pub id: u16,
    pub name: String,
    pub topic_type: String,
    pub serialization_format: String,
    pub offered_qos_profiles: String,
}

/// message の定義
pub struct TopicMessage {
    pub message_id: u64,
    pub topic_id: u16,
    pub timestamp: u64,
    pub data: Option<Vec<u8>>,
}
  • 仕様として
    • message はtopic_idを持っていて、topic_idからtopicのtypeとかわかる
    • dataはu8にserializeされている

SQLiteをRustで扱う

message data の desrialize

    /// Deserialize sensor_msgs/msg/Image to image vector.
    ///
    /// ROS2 image message has data as below.
    ///
    /// | the index of topic data              | topic data             |
    /// | ------------------------------------ | ---------------------- |
    /// | 0..(header.size - 1)                 | std_msgs/Header header |
    /// | header.size..(header.size + 3)       | uint32 height          |
    /// | (header.size + 4)..(header.size + 7) | uint32 width           |
    /// |                                      | string encoding        |
    /// |                                      | uint8 is_bigendian     |
    /// |                                      | uint32 step            |
    /// | (header.datasize + 28)               | uint8[] data           |
    pub fn deserialize_image_message(&self) -> RgbImage {
        let topic_data: Vec<u8> = self.data.as_ref().unwrap().to_vec();
        let header = get_header(&topic_data);
        let height = get_u32(&topic_data, header.size);
        let width = get_u32(&topic_data, header.size + 4);
        RgbImage::from_vec(width, height, topic_data[(header.size + 28)..].to_vec()).unwrap()
    }

pub fn get_u32(topic_data: &[u8], index: usize) -> u32 {
    let u8_array = u32_slice_to_array(&topic_data[index..(index + 4)]);
    unsafe { std::mem::transmute(u8_array) }
}

pub fn u32_slice_to_array(slice: &[u8]) -> [u8; 4] {
    slice.try_into().expect("slice with incorrect length")
}
  • string の deserialize
    • 仕様をあんまり把握できてない、一旦自分の理解で
    • 最初のu8 * 4つのdata bufferのうち、さらに最初のu8 * 1にstringの長さが入っている
    • その後はutf8からStringに復元可能
/// Get string information.
///
/// Return.
///
/// - String: string data
/// - usize: end_index
pub fn get_string(topic_data: &[u8], start_index: usize) -> (String, usize) {
    let end_index = start_index + 4 + topic_data[start_index] as usize + 1;
    let string =
        String::from_utf8(topic_data[(start_index + 4)..(end_index - 1)].to_vec()).unwrap();
    (string, end_index)
}

最後に

  • crateの利用、PRお待ちしております