折腾笔记[18]-使用rust读取rosbag数据
摘要
使用rust读取rosbag数据(ROS1格式).
关键词
rust;ros;rosbag;
关键信息
[workspace]
version = "0.0.1"
authors = ["qsbye <2557877116@qq.com"]
members = [ "crates/rosette_cli","crates/rosette_core", "crates/rosette_derive", "crates/rosette_playground"]
resolver = "3"
[workspace.package]
name = "bye_rosette_rs"
version = "0.0.1"
authors = ["qsbye <2557877116@qq.com"]
repository = "https://github.com/ByeIO/bye.rosette.rs"
edition = "2024"
license = "Apache-2.0"
[workspace.dependencies]
# 错误处理
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格式
[https://blog.csdn.net/lu_embedded/article/details/132451852]
[https://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序列化/反序列化简介
[https://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<Point>,
}
fn parse_custom_msg(data: &[u8]) -> Result<CustomMsg> {
let mut cursor = Cursor::new(data);
let seq = cursor.read_u32::<LittleEndian>()?;
let secs = cursor.read_i32::<LittleEndian>()?;
let nsecs = cursor.read_i32::<LittleEndian>()?;
let frame_id_len = cursor.read_u32::<LittleEndian>()? 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::<LittleEndian>()?;
let point_num = cursor.read_u32::<LittleEndian>()?;
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::<LittleEndian>()?,
x: cursor.read_f32::<LittleEndian>()?,
y: cursor.read_f32::<LittleEndian>()?,
z: cursor.read_f32::<LittleEndian>()?,
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
-
浙公网安备 33010602011771号