使用rust读取rosbag数据ROS1格式

Shadowlover
143 阅读2分钟

使用rust读取rosbag数据(ROS1格式).

使用rust读取rosbag数据(ROS1格式).

rust;ros;rosbag;

关键信息

项目地址[github.com/ByeIO/bye.r…]

version = "0.0.1" authors = ["qsbye <2557877116@qq.com"] members = [ "crates/rosette_cli","crates/rosette_core", "crates/rosette_derive", "crates/rosette_playground"] resolver = "3"

name = "bye_rosette_rs" version = "0.0.1" authors = ["qsbye <2557877116@qq.com"] repository = "github.com/ByeIO/bye.r…" edition = "2024" license = "Apache-2.0"

错误处理

anyhow = "1.0.97"

时间格式化

chrono = "0.4.40"

Model Context Protocol协议定义

rust-mcp-schema = { version = "0.2.2", path = "./static/rust-mcp-schema" }

Model Context Protocol开发工具

rust-mcp-sdk = { version = "0.1.2", path = "./static/rust-mcp-sdk/crates/rust-mcp-sdk" }

多线程框架

tokio = { version = "1.44.1", features = ["full"] }

ros2接口(针对humble)

ros2-interfaces-humble = { version = "0.0.1", path = "./static/ros2-interfaces-humble" }

日志后端

log = { version = "0.4.27", features = ["std"] }

ros2数据包bag处理

rosbag2-rs = { version = "0.2.1", path = "./static/rosbag2-rs" }

ros1数据包bag处理

rosbag = { version = "0.6.3", path = "./static/rosbag-rs" }

临时文件

tempfile = { version = "3.19.1" }

ros2消息收发

ros2-client = { version = "0.8.1", features = ["pre-iron-gid"] }

DDS分布式通信

rustdds = { version = "0.11.4", path = "./static/RustDDS" }

MQTT协议栈(纯Rust)

rumqttc = { version = "0.24.0", path = "./static/rumqttc" } rumqttd = { version = "0.19.0", path = "./static/rumqttd" }

静态变量

static_cell = "2.1.0"

全局变量

once_cell = "1.21.3"

通用数据表达序列化/反序列化

cdr = { version = "0.2.4", path = "./static/cdr-rs" }

大小端字节序

byteorder = "1.5.0"

原理简介

rosbag1格式对比rosbag2格式

[blog.csdn.net/lu_embedded…]
[zhuanlan.zhihu.com/p/494474804]
以下是 ROS1 和 ROS2 中 rosbag 格式的对比：

  • 文件结构
    ROS1 rosbag：以单个 .bag 文件存储所有数据，文件中包含消息的二进制数据、时间戳、主题信息等。
    ROS2 rosbag：采用目录结构，包含多个文件，如 .db3 数据库文件和 .yaml 配置文件。.db3 文件基于 SQLite，用于存储消息数据，而 .yaml 文件则存储元数据。
  • 存储格式
    ROS1 rosbag：基于二进制格式存储数据，消息以二进制形式写入文件，每个消息包含时间戳和消息体。这种格式存储效率高，但在跨平台兼容性和扩展性方面存在限制。
    ROS2 rosbag：基于 SQLite 数据库格式，将消息存储为表格形式，每个表格包含时间戳、消息类型和消息数据。这种格式支持更灵活的查询和过滤操作。
  • 兼容性
    ROS1 rosbag：由于其二进制格式的固定性，不支持跨平台的消息兼容性。
    ROS2 rosbag：具有更好的跨平台兼容性，支持多种操作系统和架构。
  • 扩展性
    ROS1 rosbag：格式相对固定，不支持动态扩展。
    ROS2 rosbag：格式灵活，支持动态扩展，能够适应不同类型和结构的消息。
  • 查询与过滤
    ROS1 rosbag：查询和过滤功能相对简单，主要通过主题名称、时间范围等进行过滤。
    ROS2 rosbag：由于基于数据库，支持更复杂的查询和过滤操作，用户可以更灵活地检索特定消息。
  • 转换工具
    ROS1 rosbag：需要借助工具如 rosbag_migration_tool 或 rosbags-convert 将其转换为 ROS2 格式。
    ROS2 rosbag：同样可以使用上述工具将其转换为 ROS1 格式。

cdr序列化/反序列化简介

[linuxcpp.0voice.com/?id=232477]
CDR（Common Data Representation）是一种用于序列化和反序列化数据的格式，主要在分布式系统中应用，以便于不同平台和编程语言之间的数据交换。在RTPS（Real-Time Publish-Subscribe Protocol）以及DDS（Data Distribution Service）等协议中，CDR 被广泛使用来处理消息的传输。

CDR（Common Data Representation）序列化和反序列化是一种用于数据交换的技术，广泛应用于分布式系统和通信协议中，特别是在DDS（Data Distribution Service）和RTPS（Real-Time Publish-Subscribe Protocol）等场景。

CDR序列化

CDR序列化是将数据结构（如结构体、类等）转换为字节流的过程，以便通过网络传输或存储到文件中。CDR定义了一种平台无关的二进制格式，支持多种数据类型（如整数、浮点数、字符串、结构体等），并提供相应的编码规则。在序列化过程中，数据按照预定格式写入字节流，同时需要考虑字节对齐和大小端转换等问题。

CDR反序列化

CDR反序列化是序列化的逆过程，即将字节流还原为原始数据结构。接收方根据CDR规范解析字节流，按照数据类型和格式逐步读取字段，并重新构造出原始数据。

CDR的特点
  • 跨平台和跨语言：CDR支持多种编程语言（如C++、Java、Python等）之间的数据交换。
  • 高效性：CDR提供高效的序列化和反序列化方法，适合高性能通信场景。
  • 灵活性：支持复杂数据类型，包括嵌套结构和变长数据类型。
  • 兼容性：CDR支持版本管理，允许在不破坏现有二进制兼容性的情况下更改数据格式。
应用场景

CDR广泛应用于以下领域：

  • 实时通信系统：如自动驾驶、实时监控等，需要高效数据传输。
  • 分布式系统：用于不同组件之间的数据交换。
  • 物联网：在嵌入式设备之间传输数据。

通过CDR序列化和反序列化，可以实现跨平台、跨语言的数据交换，满足分布式系统和实时通信的需求。

#![allow(unused)]

//! 读取fast_livo2示例rosbag

// 错误处理 use anyhow::{Result, Ok};

// rosbag1处理 use rosbag::{ChunkRecord, MessageRecord, RosBag, record_types::{Chunk, IndexData}};

// 字节处理 use byteorder::{LittleEndian, ReadBytesExt};

// 标准库 use std::path::Path; use std::env; use std::collections::HashMap; use std::io::Cursor; use std::io::prelude::*;

#[derive(Debug)] struct Stamp { secs: i32, nsecs: i32,

#[derive(Debug)] struct Header { seq: u32, stamp: Stamp, frame_id: String,

#[derive(Debug)] struct Point { offset_time: u32, x: f32, y: f32, z: f32, reflectivity: u8, tag: u8, line: u8,

#[derive(Debug)] struct CustomMsg { header: Header, timebase: u64, point_num: u32, lidar_id: u8, rsvd: [u8; 3], points: Vec,

fn parse_custom_msg(data: &[u8]) -> Result { let mut cursor = Cursor::new(data); let seq = cursor.read_u32::()?; let secs = cursor.read_i32::()?; let nsecs = cursor.read_i32::()?; let frame_id_len = cursor.read_u32::()? as usize; let mut frame_id_bytes = vec![0u8; frame_id_len]; cursor.read_exact(&mut frame_id_bytes)?; let frame_id = String::from_utf8(frame_id_bytes)?; let timebase = cursor.read_u64::()?; let point_num = cursor.read_u32::()?; let lidar_id = cursor.read_u8()?; let mut rsvd = [0u8; 3]; cursor.read_exact(&mut rsvd)?; let mut points = Vec::with_capacity(point_num as usize); for _ in 0..point_num { points.push(Point { offset_time: cursor.read_u32::()?, x: cursor.read_f32::()?, y: cursor.read_f32::()?, z: cursor.read_f32::()?, reflectivity: cursor.read_u8()?, tag: cursor.read_u8()?, line: cursor.read_u8()?, Ok(CustomMsg { header: Header { seq, stamp: Stamp { secs, nsecs }, frame_id }, timebase, point_num, lidar_id, rsvd, points,

/// 处理单个数据块中的消息记录 fn process_chunk( chunk: Chunk, conn_id_to_topic: &mut HashMap<u32, String>, message_count: &mut usize, ) -> Result<()> { for msg_record in chunk.messages() { match msg_record? { MessageRecord::Connection(conn) => { conn_id_to_topic.insert(conn.id, conn.topic.to_ascii_lowercase()); MessageRecord::MessageData(msg_data) => { if let Some(topic) = conn_id_to_topic.get(&msg_data.conn_id) { if topic == "/livox/lidar" { let custom_msg = parse_custom_msg(&msg_data.data)?;

                    // 结构化输出
                    println!("header: ");
                    println!("  seq: {}", custom_msg.header.seq);
                    println!("  stamp: ");
                    println!("    secs: {}", custom_msg.header.stamp.secs);
                    println!("    nsecs: {}", custom_msg.header.stamp.nsecs);
                    println!("  frame_id: \"{}\"", custom_msg.header.frame_id);
                    println!("timebase: {}", custom_msg.timebase);
                    println!("point_num: {}", custom_msg.point_num);
                    println!("lidar_id: {}", custom_msg.lidar_id);
                    println!("rsvd: [{}, {}, {}]", custom_msg.rsvd[0], custom_msg.rsvd[1], custom_msg.rsvd[2]);
                    println!("points: ");
                    for point in &custom_msg.points {
                        println!("  - ");
                        println!("    offset_time: {}", point.offset_time);
                        println!("    x: {:.15}", point.x);
                        println!("    y: {:.15}", point.y);
                        println!("    z: {:.15}", point.z);
                        println!("    reflectivity: {}", point.reflectivity);
                        println!("    tag: {}", point.tag);
                        println!("    line: {}", point.line);
                    println!("\n-----------------------------");
                    
                    *message_count += 1;
                    if *message_count >= 5 {
                        return Ok(());
Ok(())

fn main() -> Result<()> { let bag_relative_path = "../../assets/Retail_Street.bag"; let current_dir = env::current_dir().expect("获取当前目录失败"); let bag_abs_path = current_dir.join(bag_relative_path); println!("正在打开ROS bag文件: {}", bag_abs_path.display());

let bag = RosBag::new(bag_abs_path)?;
let mut conn_id_to_topic = HashMap::new();
let mut message_count = 0;

for chunk_record in bag.chunk_records() {
    match chunk_record? {
        ChunkRecord::Chunk(chunk) => {
            // 处理当前数据块
            process_chunk(chunk, &mut conn_id_to_topic, &mut message_count)?;
            if message_count >= 5 {
                break;
        ChunkRecord::IndexData(_) => {}

println!("成功读取前5条消息");
Ok(())

cargo run --example rosbag1_read_fast_livo2

效果

原始rostopic输出:

header: seq: 4068 stamp: secs: 946685438 nsecs: 699309945 frame_id: "livox_frame" timebase: 946685278699310000 point_num: 24000 lidar_id: 0 rsvd: [0, 0, 0] points: offset_time: 0 x: 15.404000282287598 y: 5.798999786376953 z: 5.0370001792907715 reflectivity: 15 tag: 16 line: 0 offset_time: 4167 x: 15.5 y: 5.89300012588501 z: 5.381999969482422 reflectivity: 17 tag: 16 line: 1 offset_time: 8334 x: 15.468000411987305 y: 5.941999912261963 z: 5.690999984741211 reflectivity: 19 tag: 16 line: 2 offset_time: 12501 x: 15.496999740600586 y: 6.019000053405762 z: 6.0320000648498535 reflectivity: 19 tag: 16 line: 3 offset_time: 16668 x: 15.512999534606934 y: 6.0980000495910645 z: 6.377999782562256 reflectivity: 18 tag: 16 line: 4

代码输出:

header: seq: 4056 stamp: secs: 946685437 nsecs: 499511003 frame_id: "livox_frame" timebase: 946685277499511000 point_num: 24000 lidar_id: 0 rsvd: [0, 0, 0] points: offset_time: 83886080 x: 0.000000000000000 y: 12.005999565124512 z: 2.029999971389771 reflectivity: 25 tag: 4 line: 210 offset_time: 1056064 x: 0.000000000000000 y: 12.024000167846680 z: 2.036999940872192 reflectivity: 225 tag: 122 line: 220 offset_time: 17834048 x: 0.000000000000000 y: 11.967000007629395 z: 2.033999919891357 reflectivity: 25 tag: 4 line: 230

avatar
文章
阅读
粉丝