openrr_tracing/
de.rs

Help
1//! Deserialize tracing log in JSON format
2
3use std::time::{Duration, SystemTime};
4
5use arci::{nalgebra, BaseVelocity};
6use serde::Deserialize;
7
8pub type Timestamp = chrono::DateTime<chrono::Utc>;
9
10pub fn from_str(lines: &str) -> Result<Vec<TracingLog>, arci::Error> {
11    let mut res = vec![];
12    for line in lines.lines() {
13        let value: serde_json::Value =
14            serde_json::from_str(line).map_err(|e| arci::Error::Other(e.into()))?;
15        // ignore unrelated line and log for other target (library/binary/module).
16        if !matches!(value.get("target"), Some(target) if target == "openrr_tracing") {
17            continue;
18        }
19        let Some(fields) = value.get("fields") else {
20            continue;
21        };
22        match fields.get("method") {
23            Some(v) if v == "arci::Localization::current_pose" => {
24                let log: CurrentPoseLog =
25                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
26                res.push(TracingLog::CurrentPose(log));
27            }
28            Some(v) if v == "arci::MotorDriveEffort::set_motor_effort" => {
29                let log: SetMotorEffortLog =
30                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
31                res.push(TracingLog::SetMotorEffort(log));
32            }
33            Some(v) if v == "arci::MotorDriveEffort::get_motor_effort" => {
34                let log: GetMotorEffortLog =
35                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
36                res.push(TracingLog::GetMotorEffort(log));
37            }
38            Some(v) if v == "arci::MotorDrivePosition::set_motor_position" => {
39                let log: SetMotorPositionLog =
40                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
41                res.push(TracingLog::SetMotorPosition(log));
42            }
43            Some(v) if v == "arci::MotorDrivePosition::get_motor_position" => {
44                let log: GetMotorPositionLog =
45                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
46                res.push(TracingLog::GetMotorPosition(log));
47            }
48            Some(v) if v == "arci::MotorDriveVelocity::set_motor_velocity" => {
49                let log: SetMotorVelocityLog =
50                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
51                res.push(TracingLog::SetMotorVelocity(log));
52            }
53            Some(v) if v == "arci::MotorDriveVelocity::get_motor_velocity" => {
54                let log: GetMotorVelocityLog =
55                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
56                res.push(TracingLog::GetMotorVelocity(log));
57            }
58            Some(v) if v == "arci::MoveBase::send_velocity" => {
59                let log: SendVelocityLog =
60                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
61                res.push(TracingLog::SendVelocity(log));
62            }
63            Some(v) if v == "arci::MoveBase::current_velocity" => {
64                let log: CurrentVelocityLog =
65                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
66                res.push(TracingLog::CurrentVelocity(log));
67            }
68            Some(v) if v == "arci::Navigation::send_goal_pose" => {
69                let log: SendGoalPoseLog =
70                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
71                res.push(TracingLog::SendGoalPose(log));
72            }
73            Some(v) if v == "arci::Navigation::cancel" => {
74                let log: NavigationCancelLog =
75                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
76                res.push(TracingLog::NavigationCancel(log));
77            }
78            Some(v) if v == "arci::Speaker::speak" => {
79                let log: SpeakLog =
80                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
81                res.push(TracingLog::Speak(log));
82            }
83            Some(v) if v == "arci::TransformResolver::resolve_transformation" => {
84                let log: ResolveTransformationLog =
85                    serde_json::from_value(value).map_err(|e| arci::Error::Other(e.into()))?;
86                res.push(TracingLog::ResolveTransformation(log));
87            }
88            _ => continue, // TODO
89        }
90    }
91    Ok(res)
92}
93
94#[derive(Debug)]
95#[non_exhaustive]
96pub enum TracingLog {
97    /// [`arci::Localization::current_pose`]
98    CurrentPose(CurrentPoseLog),
99
100    /// [`arci::MotorDriveEffort::set_motor_effort`]
101    SetMotorEffort(SetMotorEffortLog),
102    /// [`arci::MotorDriveEffort::get_motor_effort`]
103    GetMotorEffort(GetMotorEffortLog),
104
105    /// [`arci::MotorDrivePosition::set_motor_position`]
106    SetMotorPosition(SetMotorPositionLog),
107    /// [`arci::MotorDrivePosition::get_motor_position`]
108    GetMotorPosition(GetMotorPositionLog),
109
110    /// [`arci::MotorDriveVelocity::set_motor_velocity`]
111    SetMotorVelocity(SetMotorVelocityLog),
112    /// [`arci::MotorDriveVelocity::get_motor_velocity`]
113    GetMotorVelocity(GetMotorVelocityLog),
114
115    /// [`arci::MoveBase::send_velocity`]
116    SendVelocity(SendVelocityLog),
117    /// [`arci::MoveBase::current_velocity`]
118    CurrentVelocity(CurrentVelocityLog),
119
120    /// [`arci::Navigation::send_goal_pose`]
121    SendGoalPose(SendGoalPoseLog),
122    /// [`arci::Navigation::cancel`]
123    NavigationCancel(NavigationCancelLog),
124
125    /// [`arci::Speaker::speak`]
126    Speak(SpeakLog),
127
128    /// [`arci::TransformResolver::resolve_transformation`]
129    ResolveTransformation(ResolveTransformationLog),
130}
131
132#[derive(Deserialize)]
133struct RawTracingLog<Fields> {
134    timestamp: Timestamp,
135    fields: Fields,
136}
137
138// =============================================================================
139// arci::Localization
140
141#[derive(Debug)]
142#[non_exhaustive]
143pub struct CurrentPoseLog {
144    pub timestamp: Timestamp,
145    pub frame_id: String,
146    pub pose: arci::Isometry2<f64>,
147}
148
149impl<'de> Deserialize<'de> for CurrentPoseLog {
150    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
151    where
152        D: serde::Deserializer<'de>,
153    {
154        #[derive(Deserialize)]
155        struct Fields {
156            frame_id: String,
157            pose_rotation_re: f64,
158            pose_rotation_im: f64,
159            pose_translation_x: f64,
160            pose_translation_y: f64,
161        }
162        let v = RawTracingLog::<Fields>::deserialize(deserializer)?;
163        Ok(Self {
164            timestamp: v.timestamp,
165            frame_id: v.fields.frame_id,
166            pose: arci::Isometry2::from_parts(
167                nalgebra::Translation2::new(
168                    v.fields.pose_translation_x,
169                    v.fields.pose_translation_y,
170                ),
171                nalgebra::UnitComplex::from_complex(nalgebra::Complex {
172                    re: v.fields.pose_rotation_re,
173                    im: v.fields.pose_rotation_im,
174                }),
175            ),
176        })
177    }
178}
179
180// =============================================================================
181// arci::MotorDriveEffort
182
183// Currently, both SetMotorEffortLog and GetMotorEffortLog are the same.
184// However, we may want to add result field to SetMotorEffortLog.
185#[derive(Debug)]
186#[non_exhaustive]
187pub struct SetMotorEffortLog {
188    pub timestamp: Timestamp,
189    pub effort: f64,
190}
191#[derive(Debug)]
192#[non_exhaustive]
193pub struct GetMotorEffortLog {
194    pub timestamp: Timestamp,
195    pub effort: f64,
196}
197
198#[derive(Deserialize)]
199struct MotorEffortLogFields {
200    effort: f64,
201}
202impl<'de> Deserialize<'de> for SetMotorEffortLog {
203    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
204    where
205        D: serde::Deserializer<'de>,
206    {
207        let v = RawTracingLog::<MotorEffortLogFields>::deserialize(deserializer)?;
208        Ok(Self {
209            timestamp: v.timestamp,
210            effort: v.fields.effort,
211        })
212    }
213}
214impl<'de> Deserialize<'de> for GetMotorEffortLog {
215    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
216    where
217        D: serde::Deserializer<'de>,
218    {
219        let v = RawTracingLog::<MotorEffortLogFields>::deserialize(deserializer)?;
220        Ok(Self {
221            timestamp: v.timestamp,
222            effort: v.fields.effort,
223        })
224    }
225}
226
227// =============================================================================
228// arci::MotorDrivePosition
229
230// Currently, both SetMotorPositionLog and GetMotorPositionLog are the same.
231// However, we may want to add result field to SetMotorPositionLog.
232#[derive(Debug)]
233#[non_exhaustive]
234pub struct SetMotorPositionLog {
235    pub timestamp: Timestamp,
236    pub position: f64,
237}
238#[derive(Debug)]
239#[non_exhaustive]
240pub struct GetMotorPositionLog {
241    pub timestamp: Timestamp,
242    pub position: f64,
243}
244
245#[derive(Deserialize)]
246struct MotorPositionLogFields {
247    position: f64,
248}
249impl<'de> Deserialize<'de> for SetMotorPositionLog {
250    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
251    where
252        D: serde::Deserializer<'de>,
253    {
254        let v = RawTracingLog::<MotorPositionLogFields>::deserialize(deserializer)?;
255        Ok(Self {
256            timestamp: v.timestamp,
257            position: v.fields.position,
258        })
259    }
260}
261impl<'de> Deserialize<'de> for GetMotorPositionLog {
262    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
263    where
264        D: serde::Deserializer<'de>,
265    {
266        let v = RawTracingLog::<MotorPositionLogFields>::deserialize(deserializer)?;
267        Ok(Self {
268            timestamp: v.timestamp,
269            position: v.fields.position,
270        })
271    }
272}
273
274// =============================================================================
275// arci::MotorDriveVelocity
276
277// Currently, both SetMotorVelocityLog and GetMotorVelocityLog are the same.
278// However, we may want to add result field to SetMotorVelocityLog.
279#[derive(Debug)]
280#[non_exhaustive]
281pub struct SetMotorVelocityLog {
282    pub timestamp: Timestamp,
283    pub velocity: f64,
284}
285#[derive(Debug)]
286#[non_exhaustive]
287pub struct GetMotorVelocityLog {
288    pub timestamp: Timestamp,
289    pub velocity: f64,
290}
291
292#[derive(Deserialize)]
293struct MotorVelocityLogFields {
294    velocity: f64,
295}
296impl<'de> Deserialize<'de> for SetMotorVelocityLog {
297    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
298    where
299        D: serde::Deserializer<'de>,
300    {
301        let v = RawTracingLog::<MotorVelocityLogFields>::deserialize(deserializer)?;
302        Ok(Self {
303            timestamp: v.timestamp,
304            velocity: v.fields.velocity,
305        })
306    }
307}
308impl<'de> Deserialize<'de> for GetMotorVelocityLog {
309    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
310    where
311        D: serde::Deserializer<'de>,
312    {
313        let v = RawTracingLog::<MotorVelocityLogFields>::deserialize(deserializer)?;
314        Ok(Self {
315            timestamp: v.timestamp,
316            velocity: v.fields.velocity,
317        })
318    }
319}
320
321// =============================================================================
322// arci::MoveBase
323
324// Currently, both SendVelocityLog and CurrentVelocityLog are the same.
325// However, we may want to add result field to SendVelocityLog.
326#[derive(Debug)]
327#[non_exhaustive]
328pub struct SendVelocityLog {
329    pub timestamp: Timestamp,
330    pub velocity: BaseVelocity,
331}
332#[derive(Debug)]
333#[non_exhaustive]
334pub struct CurrentVelocityLog {
335    pub timestamp: Timestamp,
336    pub velocity: BaseVelocity,
337}
338
339#[derive(Deserialize)]
340struct VelocityLogFields {
341    velocity_x: f64,
342    velocity_y: f64,
343    velocity_theta: f64,
344}
345impl<'de> Deserialize<'de> for SendVelocityLog {
346    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
347    where
348        D: serde::Deserializer<'de>,
349    {
350        let v = RawTracingLog::<VelocityLogFields>::deserialize(deserializer)?;
351        Ok(Self {
352            timestamp: v.timestamp,
353            velocity: BaseVelocity {
354                x: v.fields.velocity_x,
355                y: v.fields.velocity_y,
356                theta: v.fields.velocity_theta,
357            },
358        })
359    }
360}
361impl<'de> Deserialize<'de> for CurrentVelocityLog {
362    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
363    where
364        D: serde::Deserializer<'de>,
365    {
366        let v = RawTracingLog::<VelocityLogFields>::deserialize(deserializer)?;
367        Ok(Self {
368            timestamp: v.timestamp,
369            velocity: BaseVelocity {
370                x: v.fields.velocity_x,
371                y: v.fields.velocity_y,
372                theta: v.fields.velocity_theta,
373            },
374        })
375    }
376}
377
378// =============================================================================
379// arci::Navigation
380
381#[derive(Debug)]
382#[non_exhaustive]
383pub struct SendGoalPoseLog {
384    pub timestamp: Timestamp,
385    pub goal: arci::Isometry2<f64>,
386    pub frame_id: String,
387    pub timeout: Duration,
388}
389#[derive(Debug)]
390#[non_exhaustive]
391pub struct NavigationCancelLog {
392    pub timestamp: Timestamp,
393}
394
395impl<'de> Deserialize<'de> for SendGoalPoseLog {
396    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
397    where
398        D: serde::Deserializer<'de>,
399    {
400        #[derive(Deserialize)]
401        struct Fields {
402            goal_rotation_re: f64,
403            goal_rotation_im: f64,
404            goal_translation_x: f64,
405            goal_translation_y: f64,
406            frame_id: String,
407            timeout_secs: u64,
408            timeout_nanos: u32,
409        }
410        let v = RawTracingLog::<Fields>::deserialize(deserializer)?;
411        Ok(Self {
412            timestamp: v.timestamp,
413            goal: arci::Isometry2::from_parts(
414                nalgebra::Translation2::new(
415                    v.fields.goal_translation_x,
416                    v.fields.goal_translation_y,
417                ),
418                nalgebra::UnitComplex::from_complex(nalgebra::Complex {
419                    re: v.fields.goal_rotation_re,
420                    im: v.fields.goal_rotation_im,
421                }),
422            ),
423            frame_id: v.fields.frame_id,
424            timeout: Duration::new(v.fields.timeout_secs, v.fields.timeout_nanos),
425        })
426    }
427}
428impl<'de> Deserialize<'de> for NavigationCancelLog {
429    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
430    where
431        D: serde::Deserializer<'de>,
432    {
433        #[derive(Deserialize)]
434        struct Fields {}
435        let v = RawTracingLog::<Fields>::deserialize(deserializer)?;
436        Ok(Self {
437            timestamp: v.timestamp,
438        })
439    }
440}
441
442// =============================================================================
443// arci::Speaker
444
445#[derive(Debug)]
446#[non_exhaustive]
447pub struct SpeakLog {
448    pub timestamp: Timestamp,
449    pub message: String,
450}
451
452impl<'de> Deserialize<'de> for SpeakLog {
453    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
454    where
455        D: serde::Deserializer<'de>,
456    {
457        #[derive(Deserialize)]
458        struct Fields {
459            message: String,
460        }
461        let v = RawTracingLog::<Fields>::deserialize(deserializer)?;
462        Ok(Self {
463            timestamp: v.timestamp,
464            message: v.fields.message,
465        })
466    }
467}
468
469// =============================================================================
470// arci::TransformResolver
471
472#[derive(Debug)]
473#[non_exhaustive]
474pub struct ResolveTransformationLog {
475    pub timestamp: Timestamp,
476    pub from: String,
477    pub to: String,
478    pub time: SystemTime,
479}
480
481impl<'de> Deserialize<'de> for ResolveTransformationLog {
482    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
483    where
484        D: serde::Deserializer<'de>,
485    {
486        #[derive(Deserialize)]
487        struct Fields {
488            from: String,
489            to: String,
490            time_secs: u64,
491            time_nanos: u32,
492        }
493        let v = RawTracingLog::<Fields>::deserialize(deserializer)?;
494        Ok(Self {
495            timestamp: v.timestamp,
496            from: v.fields.from,
497            to: v.fields.to,
498            time: SystemTime::UNIX_EPOCH + Duration::new(v.fields.time_secs, v.fields.time_nanos),
499        })
500    }
501}
openrr_tracing/de.rs

openrr_tracing/
de.rs
