nodo 0.18.5

A realtime framework for robotics
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

// Copyright 2023 David Weikersdorfer

use crate::core::{
    AcqtimeMarker, AppMonotonicClock, Clock, Pubtime, PubtimeMarker, SysMonotonicClock,
};

/// Task clocks used internally
#[derive(Clone)]
pub struct Clocks {
    /// Application-wide mononotic clock starting when the application starts
    pub app_mono: AppMonotonicClock<PubtimeMarker>,

    /// System-wide monotonic clock (probably) starting when the system boots
    pub sys_mono: SysMonotonicClock<AcqtimeMarker>,
}

impl Clocks {
    pub fn new() -> Self {
        Self {
            app_mono: AppMonotonicClock::new(),
            sys_mono: SysMonotonicClock::new(),
        }
    }
}

/// Clocks interface exposed to codelet
#[derive(Clone)]
pub struct TaskClocks {
    /// Application-wide mononotic clock starting when the application starts
    pub app_mono: AppMonotonicClock<PubtimeMarker>,

    /// System-wide monotonic clock (probably) starting when the system boots
    pub sys_mono: SysMonotonicClock<AcqtimeMarker>,

    /// Codelet-specific timings
    pub codelet: CodeletClock,
}

impl TaskClocks {
    pub fn from(clocks: Clocks) -> Self {
        Self {
            app_mono: clocks.app_mono.clone(),
            sys_mono: clocks.sys_mono.clone(),
            codelet: CodeletClock::new(clocks.app_mono.now()),
        }
    }

    pub(crate) fn on_codelet_start(&mut self) {
        let now = self.app_mono.now();
        self.codelet.last = now;
    }

    pub(crate) fn on_codelet_stop(&mut self) {}

    pub(crate) fn on_codelet_step(&mut self) {
        let now = self.app_mono.now();
        self.codelet.update_dt(now);
    }
}

#[derive(Clone)]
pub struct CodeletClock {
    last: Pubtime,
    dt: f64,
}

impl CodeletClock {
    pub fn new(now: Pubtime) -> Self {
        Self { last: now, dt: 0.0 }
    }

    pub fn update_dt(&mut self, now: Pubtime) {
        let dt = self.last.abs_diff(now).as_secs_f64();
        self.last = now;
        self.dt = dt;
    }

    /// Time when the current step started. `step_time` is set at the beginning of start/step/stop
    /// functions and stays constant throughout the current step. Use `real_time` for a continuously
    /// updating time.
    pub fn step_time(&self) -> Pubtime {
        self.last
    }

    /// Time elapsed in seconds since last step.
    pub fn dt_secs_f32(&self) -> f32 {
        self.dt as f32
    }

    /// Time elapsed in seconds since last step.
    pub fn dt_secs_f64(&self) -> f64 {
        self.dt
    }
}