forked from ZebraDevs/rosgo
-
Notifications
You must be signed in to change notification settings - Fork 0
/
rate.go
48 lines (41 loc) · 989 Bytes
/
rate.go
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
package rosgo
// Impelement Rate interface
type Rate struct {
actualCycleTime Duration
expectedCycleTime Duration
start Time
}
func NewRate(frequency float64) Rate {
var actualCycleTime, expectedCycleTime Duration
expectedCycleTime.FromSec(1.0 / frequency)
start := Now()
return Rate{actualCycleTime, expectedCycleTime, start}
}
func CycleTime(d Duration) Rate {
var actualCycleTime Duration
start := Now()
return Rate{actualCycleTime, d, start}
}
func (r *Rate) CycleTime() Duration {
return r.actualCycleTime
}
func (r *Rate) ExpectedCycleTime() Duration {
return r.expectedCycleTime
}
func (r *Rate) Reset() {
r.actualCycleTime = NewDuration(0, 0)
r.start = Now()
}
func (r *Rate) Sleep() error {
end := Now()
diff := end.Diff(r.start)
var remaining Duration
if r.expectedCycleTime.Cmp(diff) >= 0 {
remaining = r.expectedCycleTime.Sub(diff)
}
remaining.Sleep()
now := Now()
r.actualCycleTime = now.Diff(r.start)
r.start = now
return nil
}