折腾笔记18-使用rust读取rosbag数据
折腾笔记18-使用rust读取rosbag数据
使用rust读取rosbag数据(ROS1格式). ## 关键词 rust;ros;rosbag; ## 关键信息 项目地址[ github.com/ByeIO/bye.r… ] ``` [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 = "github.com/ByeIO/bye.r…" edition = "2024" license = "Apache-2.0"
[workspace.dependencies]
折腾笔记18-使用rust读取rosbag数据
anyhow = "1.0.97"
折腾笔记18-使用rust读取rosbag数据
chrono = "0.4.40"
折腾笔记18-使用rust读取rosbag数据
rust-mcp-schema = { version = "0.2.2", path = "./static/rust-mcp-schema" }
折腾笔记18-使用rust读取rosbag数据
rust-mcp-sdk = { version = "0.1.2", path = "./static/rust-mcp-sdk/crates/rust-mcp-sdk" }
折腾笔记18-使用rust读取rosbag数据
tokio = { version = "1.44.1", features = ["full"] }
折腾笔记18-使用rust读取rosbag数据
ros2-interfaces-humble = { version = "0.0.1", path = "./static/ros2-interfaces-humble" }
折腾笔记18-使用rust读取rosbag数据
log = { version = "0.4.27", features = ["std"] }
折腾笔记18-使用rust读取rosbag数据
rosbag2-rs = { version = "0.2.1", path = "./static/rosbag2-rs" }
折腾笔记18-使用rust读取rosbag数据
rosbag = { version = "0.6.3", path = "./static/rosbag-rs" }
折腾笔记18-使用rust读取rosbag数据
tempfile = { version = "3.19.1" }
折腾笔记18-使用rust读取rosbag数据
ros2-client = { version = "0.8.1", features = ["pre-iron-gid"] }
折腾笔记18-使用rust读取rosbag数据
rustdds = { version = "0.11.4", path = "./static/RustDDS" }
折腾笔记18-使用rust读取rosbag数据
rumqttc = { version = "0.24.0", path = "./static/rumqttc" } rumqttd = { version = "0.19.0", path = "./static/rumqttd" }
折腾笔记18-使用rust读取rosbag数据
static_cell = "2.1.0"
折腾笔记18-使用rust读取rosbag数据
once_cell = "1.21.3"
折腾笔记18-使用rust读取rosbag数据
cdr = { version = "0.2.4", path = "./static/cdr-rs" }
折腾笔记18-使用rust读取rosbag数据
byteorder = "1.5.0"
## 原理简介
### rosbag1格式对比rosbag2格式
[ [https://blog.csdn.net/lu_embedded/article/details/132451852](https://blog.csdn.net/lu_embedded/article/details/132451852) ] [ [https://zhuanlan.zhihu.com/p/494474804](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](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序列化和反序列化，可以实现跨平台、跨语言的数据交换，满足分布式系统和实时通信的需求。 ## 实现
折腾笔记18-使用rust读取rosbag数据
//! 读取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::*;
折腾笔记18-使用rust读取rosbag数据
struct Stamp { secs: i32, nsecs: i32, }
折腾笔记18-使用rust读取rosbag数据
struct Header { seq: u32, stamp: Stamp, frame_id: String, }
折腾笔记18-使用rust读取rosbag数据
struct Point { offset_time: u32, x: f32, y: f32, z: f32, reflectivity: u8, tag: u8, line: u8, }
折腾笔记18-使用rust读取rosbag数据
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
-
