rosbag2_rs/
reader.rs

1use crate::*;
2use anyhow::{anyhow, Result};
3use std::collections::HashMap;
4use std::path::Path;
5use std::{fs, vec};
6
7// Define other structs like Metadata, FileInformation, Connection, etc.
8
9pub struct Reader {
10    pub metadata: Metadata,
11    pub connections: Vec<TopicConnection>,
12    storage: Sqlite3Reader,
13}
14
15/// The `Reader` struct provides an interface for reading message data from a ROS bag file.
16///
17/// The `Reader` initializes with the path to a ROS bag directory and reads metadata
18/// and message data from the storage. It supports filtering messages by time and handling
19/// each message through a user-defined function.
20
21///
22/// # Errors
23///
24/// - Returns an error if the ROS bag version is not supported.
25/// - Returns an error if the compression mode is not supported.
26/// - Returns an error if a non-CDR serialization format is found in any topic.
27/// - Returns an error if the storage identifier is not 'sqlite3'.
28///
29/// # Note
30///
31/// - This struct assumes that the ROS bag files are in `sqlite3` format.
32/// - The `handle_messages` method allows for processing of individual messages.
33
34impl Reader {
35    pub fn new(path: impl AsRef<Path>) -> Result<Self> {
36        let path = path.as_ref().to_path_buf();
37        let metapath = path.join("metadata.yaml");
38
39        let metadata_contents = fs::read_to_string(metapath).unwrap();
40        let bag_info: BagFileInfo = serde_yaml::from_str(&metadata_contents).unwrap();
41
42        let metadata = bag_info.rosbag2_bagfile_information;
43
44        // Check version and storage identifier
45        if metadata.version > 5 {
46            return Err(anyhow!("Not supported version: {}", metadata.version));
47        }
48
49        if !metadata.compression_mode.is_empty() {
50            return Err(anyhow!(
51                "Not supported compression mode: {}",
52                metadata.compression_mode
53            ));
54        }
55
56        // metadata.topics_with_message_count.iter().map(|topi)
57        if let Some(topic_info) = metadata
58            .topics_with_message_count
59            .iter()
60            .find(|t| t.topic_metadata.serialization_format != "cdr")
61        {
62            return Err(anyhow!(
63                "Only CDR serialization format is supported: {:?}",
64                topic_info.topic_metadata.serialization_format
65            ));
66        }
67
68        if metadata.storage_identifier != "sqlite3" {
69            return Err(anyhow!(
70                "Not supported storage identifier: {}",
71                metadata.storage_identifier
72            ));
73        }
74
75        // Initialize connections
76        let connections = metadata
77            .topics_with_message_count
78            .iter()
79            .enumerate()
80            .map(|(idx, topic_info)| TopicConnection {
81                id: idx as i32 + 1,
82                topic: topic_info.topic_metadata.name.clone(),
83                msgtype: topic_info.topic_metadata.type_.clone(),
84                msgcount: topic_info.message_count,
85                ext: ConnectionExt {
86                    serialization_format: topic_info.topic_metadata.serialization_format.clone(),
87                    offered_qos_profiles: topic_info.topic_metadata.offered_qos_profiles.clone(),
88                },
89            })
90            .collect::<Vec<_>>();
91
92        let paths: Vec<String> = metadata
93            .relative_file_paths
94            .iter()
95            .map(|relative_path| path.join(relative_path).to_string_lossy().into_owned())
96            .collect();
97        let mut storage = Sqlite3Reader::new(paths);
98
99        println!("Opening storage");
100        storage.open()?;
101        println!("Opening storage Done");
102
103        Ok(Self {
104            metadata,
105            connections,
106            storage,
107        })
108    }
109
110    pub fn open(&mut self) -> Result<()> {
111        self.storage.open()?;
112
113        Ok(())
114    }
115
116    pub fn handle_messages(
117        &mut self,
118        handle_func: impl Fn((i64, i64, Vec<u8>)) -> Result<()>,
119        start: Option<i64>,
120        stop: Option<i64>,
121    ) -> Result<()> {
122        let statement = self
123            .storage
124            .messages_statement(&self.connections, start, stop)?;
125        handle_messages(statement, handle_func)?;
126        Ok(())
127    }
128
129    pub fn duration(&self) -> i64 {
130        let nsecs = self.metadata.duration.nanoseconds;
131        if self.message_count() > 0 {
132            nsecs + 1
133        } else {
134            0
135        }
136    }
137
138    pub fn start_time(&self) -> i64 {
139        let nsecs = self.metadata.starting_time.nanoseconds_since_epoch;
140        if self.message_count() > 0 {
141            nsecs
142        } else {
143            i64::MAX
144        }
145    }
146
147    pub fn end_time(&self) -> i64 {
148        self.start_time() + self.duration()
149    }
150
151    pub fn message_count(&self) -> i32 {
152        self.metadata.message_count
153    }
154
155    pub fn compression_format(&self) -> String {
156        self.metadata.compression_format.clone()
157    }
158
159    pub fn compression_mode(&self) -> Option<String> {
160        let mode = self.metadata.compression_mode.to_lowercase();
161        if mode != "none" {
162            Some(mode)
163        } else {
164            None
165        }
166    }
167
168    pub fn topics(&self) -> HashMap<String, TopicInfo> {
169        // Assuming TopicInfo is already defined
170        self.connections
171            .iter()
172            .map(|conn| {
173                (
174                    conn.topic.clone(),
175                    TopicInfo::new(conn.msgtype.clone(), conn.msgcount, vec![conn.clone()]),
176                )
177            })
178            .collect()
179    }
180
181    pub fn ros_distro(&self) -> String {
182        self.metadata.ros_distro.clone()
183    }
184}
rosbag2_rs/reader.rs

rosbag2_rs/
reader.rs
