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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

// Copyright 2023 David Weikersdorfer

use crate::core::Timestamp;
use core::marker::PhantomData;
use std::time::{Duration, Instant};

const DEFAULT_CLOCK_ID: u64 = 0;

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ClockId(u64);

impl Default for ClockId {
    fn default() -> Self {
        ClockId(DEFAULT_CLOCK_ID)
    }
}

impl ClockId {
    pub fn as_u64(&self) -> u64 {
        self.0
    }
}

impl ClockId {
    pub fn is_default(&self) -> bool {
        self.0 == DEFAULT_CLOCK_ID
    }
}

pub trait Clock<M> {
    fn now(&self) -> Timestamp<M>;
}

/// A monotonic clock which starts when the application starts
#[derive(Clone)]
pub struct AppMonotonicClock<M> {
    reference: Instant,
    _marker: PhantomData<M>,
}

impl<M> Clock<M> for AppMonotonicClock<M> {
    fn now(&self) -> Timestamp<M> {
        Timestamp::new(self.reference.elapsed())
    }
}

impl<M> AppMonotonicClock<M> {
    pub fn new() -> Self {
        Self {
            reference: Instant::now(),
            _marker: PhantomData,
        }
    }
}

impl<M> Default for AppMonotonicClock<M> {
    fn default() -> Self {
        AppMonotonicClock::new()
    }
}

/// A monotonic clock which starts when the computer boots
///
/// TODO Currently uses nix::time::clock_gettime but it is unclear if that works under Windows and
///      or Mac.
#[derive(Clone)]
pub struct SysMonotonicClock<M> {
    _marker: PhantomData<M>,
}

impl<M> Clock<M> for SysMonotonicClock<M> {
    fn now(&self) -> Timestamp<M> {
        get_monotonic_time().into()
    }
}

impl<M> SysMonotonicClock<M> {
    pub fn new() -> Self {
        Self {
            _marker: PhantomData,
        }
    }
}

impl<M> Default for SysMonotonicClock<M> {
    fn default() -> Self {
        SysMonotonicClock::new()
    }
}

fn get_monotonic_time() -> Duration {
    #[cfg(unix)]
    {
        use nix::time::{clock_gettime, ClockId};
        // SAFETY: According to the error values listed in the reference for clock_gettime
        //         (see https://man7.org/linux/man-pages/man3/clock_gettime.3.html)
        //         the get function should not return any errors for CLOCK_MONOTONIC.
        let time = clock_gettime(ClockId::CLOCK_MONOTONIC).unwrap();
        Duration::from(time)
    }

    #[cfg(windows)]
    {
        use std::{mem::MaybeUninit, sync::Once};
        use windows_sys::Win32::System::Performance::{
            QueryPerformanceCounter, QueryPerformanceFrequency,
        };

        static mut FREQUENCY: i64 = 0;
        static INIT: Once = Once::new();

        unsafe {
            INIT.call_once(|| {
                let mut freq = MaybeUninit::<i64>::uninit();
                if QueryPerformanceFrequency(freq.as_mut_ptr()) == 0 {
                    panic!("QueryPerformanceFrequency failed");
                }
                FREQUENCY = freq.assume_init();
            });

            let mut counter = MaybeUninit::<i64>::uninit();
            if QueryPerformanceCounter(counter.as_mut_ptr()) == 0 {
                panic!("QueryPerformanceCounter failed");
            }
            let count = counter.assume_init();
            Duration::from_secs_f64(count as f64 / FREQUENCY as f64)
        }
    }
}