cu29_logviz/
lib.rs

1use clap::Parser;
2use cu_sensor_payloads::{CuImage, ImuPayload, PointCloud, PointCloudSoa};
3use cu_spatial_payloads::Transform3D as CuTransform3D;
4use cu29::clock::Tov;
5use cu29::prelude::{
6    CopperListTuple, CuError, CuResult, UnifiedLogType, UnifiedLogger, UnifiedLoggerBuilder,
7    UnifiedLoggerIOReader, UnifiedLoggerRead,
8};
9use cu29_export::copperlists_reader;
10pub use rerun::RecordingStream;
11use rerun::{Points3D, Scalars, TextDocument};
12use serde::Serialize;
13use std::any::{Any, TypeId};
14use std::path::{Path, PathBuf};
15use uom::si::thermodynamic_temperature::degree_celsius;
16
17mod fallback;
18
19pub use fallback::{extract_scalars, flatten_json};
20
21pub fn tov_to_secs(tov: &Tov) -> Option<f64> {
22    match tov {
23        Tov::Time(t) => Some(t.0 as f64 / 1_000_000_000.0),
24        Tov::Range(r) => Some(r.start.0 as f64 / 1_000_000_000.0),
25        Tov::None => None,
26    }
27}
28
29pub fn apply_tov(rec: &RecordingStream, tov: &Tov) {
30    if let Some(secs) = tov_to_secs(tov) {
31        rec.set_duration_secs("tov", secs);
32    } else {
33        rec.reset_time();
34    }
35}
36
37pub trait LogvizDataSet {
38    fn logviz_emit(&self, rec: &RecordingStream) -> CuResult<()>;
39}
40
41fn log_scalar(rec: &RecordingStream, path: &str, value: f64) -> CuResult<()> {
42    rec.log(path, &Scalars::new([value]))
43        .map_err(|e| CuError::new_with_cause("Failed to log scalar", e))
44}
45
46pub fn log_image<A>(rec: &RecordingStream, path: &str, image: &CuImage<A>) -> CuResult<()>
47where
48    A: cu29::prelude::ArrayLike<Element = u8>,
49{
50    log_as_components(rec, path, image)
51}
52
53pub fn log_pointcloud<const N: usize>(
54    rec: &RecordingStream,
55    path: &str,
56    pc: &PointCloudSoa<N>,
57) -> CuResult<()> {
58    log_as_components(rec, path, pc)
59}
60
61pub fn log_transform<T: Copy + std::fmt::Debug + 'static>(
62    rec: &RecordingStream,
63    path: &str,
64    transform: &CuTransform3D<T>,
65) -> CuResult<()>
66where
67    CuTransform3D<T>: rerun::AsComponents,
68{
69    log_as_components(rec, path, transform)
70}
71
72pub fn log_imu(rec: &RecordingStream, base: &str, imu: &ImuPayload) -> CuResult<()> {
73    log_scalar(rec, &format!("{}/accel_x", base), imu.accel_x.value as f64)?;
74    log_scalar(rec, &format!("{}/accel_y", base), imu.accel_y.value as f64)?;
75    log_scalar(rec, &format!("{}/accel_z", base), imu.accel_z.value as f64)?;
76    log_scalar(rec, &format!("{}/gyro_x", base), imu.gyro_x.value as f64)?;
77    log_scalar(rec, &format!("{}/gyro_y", base), imu.gyro_y.value as f64)?;
78    log_scalar(rec, &format!("{}/gyro_z", base), imu.gyro_z.value as f64)?;
79    log_scalar(
80        rec,
81        &format!("{}/temperature_c", base),
82        imu.temperature.get::<degree_celsius>() as f64,
83    )?;
84    Ok(())
85}
86
87pub fn log_as_components<A: rerun::AsComponents>(
88    rec: &RecordingStream,
89    path: &str,
90    value: &A,
91) -> CuResult<()> {
92    rec.log(path, value)
93        .map_err(|e| CuError::new_with_cause("Failed to log rerun components", e))
94}
95
96pub fn log_payload_auto<T>(rec: &RecordingStream, path: &str, payload: &T) -> CuResult<()>
97where
98    T: Serialize + 'static,
99{
100    let any_payload = payload as &dyn Any;
101    let payload_id = any_payload.type_id();
102
103    if payload_id == TypeId::of::<CuImage<Vec<u8>>>() {
104        let image = any_payload
105            .downcast_ref::<CuImage<Vec<u8>>>()
106            .expect("downcast must match TypeId");
107        return log_as_components(rec, path, image);
108    }
109
110    if payload_id == TypeId::of::<PointCloud>() {
111        let point = any_payload
112            .downcast_ref::<PointCloud>()
113            .expect("downcast must match TypeId");
114        return log_as_components(rec, path, point);
115    }
116
117    if payload_id == TypeId::of::<CuTransform3D<f32>>() {
118        let transform = any_payload
119            .downcast_ref::<CuTransform3D<f32>>()
120            .expect("downcast must match TypeId");
121        return log_as_components(rec, path, transform);
122    }
123
124    if payload_id == TypeId::of::<CuTransform3D<f64>>() {
125        let transform = any_payload
126            .downcast_ref::<CuTransform3D<f64>>()
127            .expect("downcast must match TypeId");
128        return log_as_components(rec, path, transform);
129    }
130
131    if payload_id == TypeId::of::<ImuPayload>() {
132        let imu = any_payload
133            .downcast_ref::<ImuPayload>()
134            .expect("downcast must match TypeId");
135        return log_imu(rec, path, imu);
136    }
137
138    if is_pointcloud_soa_type_name(core::any::type_name::<T>())
139        && let Some(points) = extract_pointcloud_soa_positions(payload)?
140    {
141        return rec
142            .log(path, &Points3D::new(points))
143            .map_err(|e| CuError::new_with_cause("Failed to log point cloud", e));
144    }
145
146    log_fallback_payload(rec, path, payload)
147}
148
149fn is_pointcloud_soa_type_name(type_name: &str) -> bool {
150    type_name.contains("PointCloudSoa<")
151}
152
153fn extract_pointcloud_soa_positions<T: Serialize>(
154    payload: &T,
155) -> CuResult<Option<Vec<rerun::Position3D>>> {
156    let value = serde_json::to_value(payload)
157        .map_err(|e| CuError::new_with_cause("Failed to serialize payload", e))?;
158    let object = match value.as_object() {
159        Some(object) => object,
160        None => return Ok(None),
161    };
162
163    let xs = match object.get("x").and_then(|v| v.as_array()) {
164        Some(values) => values,
165        None => return Ok(None),
166    };
167    let ys = match object.get("y").and_then(|v| v.as_array()) {
168        Some(values) => values,
169        None => return Ok(None),
170    };
171    let zs = match object.get("z").and_then(|v| v.as_array()) {
172        Some(values) => values,
173        None => return Ok(None),
174    };
175
176    let default_len = xs.len().min(ys.len()).min(zs.len());
177    let len = object
178        .get("len")
179        .and_then(|v| v.as_u64())
180        .map_or(default_len, |value| value as usize)
181        .min(default_len);
182
183    let mut points = Vec::with_capacity(len);
184    for i in 0..len {
185        let Some(x) = xs[i].as_f64() else {
186            return Ok(None);
187        };
188        let Some(y) = ys[i].as_f64() else {
189            return Ok(None);
190        };
191        let Some(z) = zs[i].as_f64() else {
192            return Ok(None);
193        };
194        points.push(rerun::Position3D::new(x as f32, y as f32, z as f32));
195    }
196
197    Ok(Some(points))
198}
199
200pub fn log_fallback_payload<T: Serialize>(
201    rec: &RecordingStream,
202    base: &str,
203    payload: &T,
204) -> CuResult<()> {
205    let value = serde_json::to_value(payload)
206        .map_err(|e| CuError::new_with_cause("Failed to serialize payload", e))?;
207    let flat = flatten_json(base, &value);
208    for (path, value) in flat {
209        if let Some(num) = value.as_f64() {
210            log_scalar(rec, &path, num)?;
211        } else if let Some(value_bool) = value.as_bool() {
212            log_scalar(rec, &path, if value_bool { 1.0 } else { 0.0 })?;
213        } else if let Some(text) = value.as_str() {
214            rec.log(path.as_str(), &TextDocument::new(text))
215                .map_err(|e| CuError::new_with_cause("Failed to log text", e))?;
216        } else if !value.is_null() {
217            rec.log(path.as_str(), &TextDocument::new(value.to_string()))
218                .map_err(|e| CuError::new_with_cause("Failed to log text", e))?;
219        }
220    }
221    Ok(())
222}
223
224#[derive(Debug, Parser)]
225#[command(author, version, about)]
226struct LogVizCli {
227    /// Base path to the unified log (e.g. logs/my_log.copper)
228    #[arg(value_name = "UNIFIEDLOG_BASE")]
229    unifiedlog_base: PathBuf,
230
231    #[command(flatten)]
232    rerun: rerun::clap::RerunArgs,
233}
234
235fn build_read_logger(unifiedlog_base: &Path) -> CuResult<UnifiedLoggerRead> {
236    let logger = UnifiedLoggerBuilder::new()
237        .file_base_name(unifiedlog_base)
238        .build()
239        .map_err(|e| CuError::new_with_cause("Failed to create logger", e))?;
240    match logger {
241        UnifiedLogger::Read(dl) => Ok(dl),
242        UnifiedLogger::Write(_) => Err(CuError::from(
243            "Expected read-only unified logger in logviz CLI",
244        )),
245    }
246}
247
248pub fn logviz_emit_dataset<P: LogvizDataSet>(dataset: &P, rec: &RecordingStream) -> CuResult<()> {
249    dataset.logviz_emit(rec)
250}
251
252pub fn run_cli<P>() -> CuResult<()>
253where
254    P: CopperListTuple + LogvizDataSet,
255{
256    let args = LogVizCli::parse();
257    let (rec, _guard) = args
258        .rerun
259        .init("cu29-logviz")
260        .map_err(|e| CuError::from(format!("Failed to init rerun: {e}")))?;
261    let dl = build_read_logger(&args.unifiedlog_base)?;
262    let mut reader = UnifiedLoggerIOReader::new(dl, UnifiedLogType::CopperList);
263    for culist in copperlists_reader::<P>(&mut reader) {
264        logviz_emit_dataset(&culist.msgs, &rec)?;
265    }
266    Ok(())
267}
268
269#[cfg(test)]
270mod tests {
271    use super::*;
272    use cu_sensor_payloads::{PointCloud, PointCloudSoa};
273    use cu29::clock::CuTime;
274    use rerun::Position3D;
275    use serde::Serialize;
276
277    #[test]
278    fn tov_time_to_secs() {
279        let tov = Tov::Time(CuTime::from(1_500_000_000u64));
280        assert_eq!(tov_to_secs(&tov), Some(1.5));
281    }
282
283    #[test]
284    fn tov_none_returns_none() {
285        assert_eq!(tov_to_secs(&Tov::None), None);
286    }
287
288    #[derive(Serialize)]
289    struct Inner {
290        c: f64,
291    }
292
293    #[derive(Serialize)]
294    struct Outer {
295        a: i32,
296        b: Inner,
297        arr: [u8; 2],
298    }
299
300    #[test]
301    fn flatten_json_paths() {
302        let value = serde_json::to_value(Outer {
303            a: 1,
304            b: Inner { c: 2.5 },
305            arr: [3, 4],
306        })
307        .unwrap();
308        let flat = flatten_json("root", &value);
309        assert_eq!(flat["root/a"], serde_json::json!(1));
310        assert_eq!(flat["root/b/c"], serde_json::json!(2.5));
311        assert_eq!(flat["root/arr/0"], serde_json::json!(3));
312        assert_eq!(flat["root/arr/1"], serde_json::json!(4));
313    }
314
315    #[test]
316    fn pointcloud_soa_type_name_is_detected() {
317        assert!(is_pointcloud_soa_type_name(core::any::type_name::<
318            PointCloudSoa<64>,
319        >()));
320        assert!(!is_pointcloud_soa_type_name(core::any::type_name::<
321            PointCloud,
322        >()));
323    }
324
325    #[test]
326    fn extract_pointcloud_soa_positions_uses_len_and_xyz_arrays() {
327        let payload = serde_json::json!({
328            "len": 2,
329            "x": [1.0, 4.0, 9.0],
330            "y": [2.0, 5.0, 9.0],
331            "z": [3.0, 6.0, 9.0],
332        });
333        let points = extract_pointcloud_soa_positions(&payload)
334            .expect("serialize")
335            .expect("pointcloud payload");
336        assert_eq!(points.len(), 2);
337        assert_eq!(points[0], Position3D::new(1.0, 2.0, 3.0));
338        assert_eq!(points[1], Position3D::new(4.0, 5.0, 6.0));
339    }
340}
cu29_logviz/lib.rs

cu29_logviz/
lib.rs
