forked from rodrigoqueiroz/geoscenarioserver
-
Notifications
You must be signed in to change notification settings - Fork 0
/
TickSync.py
177 lines (160 loc) · 7.06 KB
/
TickSync.py
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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
#!/usr/bin/env python3
#rqueiroz@uwaterloo.ca
# ---------------------------------------------
# TickSync
# Syncronize simulation loop based on a given frequency (frame-rate).
# Higher rate allows smoother trajectories and more precise metrics and collisions,
# but requires more processing capabilities. Can't avoid drift if hardware is slow.
# --------------------------------------------
import datetime
import time
import math
import csv
from SimConfig import *
from util.Utils import truncate
import glog as log
class TickSync():
def __init__(self, rate = 30, realtime = True, block = False, verbose = False, label = "", sim_start_time = 0.0):
#config
self.timeout = None
self.tick_rate = rate
self.expected_tick_duration = 1.0/rate
self.realtime = realtime
self.block = block
self.verbose = verbose
self.label = label
self.sim_start_time = sim_start_time
#global
self._sim_start_clock = None #clock time when sim started (first tick) [clock]
self.tick_count = 0
self.sim_time = 0 #Total simulation time since start() [s]
#per tick
self._tick_start_clock = None #sim time when tick started [s]
self.delta_time = 0.0 #diff since previous tick [s] (aka frame time)
self.drift = 0.0 #diff between expected tick time and actual time caused by lag
#stats
self.performance_log = []
self.target_performance_stats = [
self.label,
self.timeout,
self.tick_rate,
self.expected_tick_duration]
def get_sim_time(self):
return self.sim_time
def set_timeout(self,timeout):
self.timeout = timeout
def print(self,msg):
if (self.verbose):
print(msg)
def tick(self):
now = datetime.datetime.now()
#First Tick
if (self.tick_count==0):
#First Tick is special:
self._sim_start_clock = now
self.delta_time = 0.0
self._tick_start_clock = now
#Update globals
self.tick_count+=1
self.sim_time = self.sim_start_time #starting time by config
self.print('{:05.2f}s {} Tick {:3}# START'.
format(self.sim_time,self.label,self.tick_count))
return True
else:
#Can tick? Preliminary numbers:
diff_tick = (now - self._tick_start_clock).total_seconds() #diff from previous tick
drift = diff_tick - self.expected_tick_duration #diff from expected time
if (drift<0):
#Too fast. Need to chill.
if (self.block):
time.sleep(-drift) #blocks diff if negative drift
#self.print('sleep {:.3}'.format(drift))
else:
#self.print('skip {:.3}'.format(drift))
return False #return false to skip
#Can proceed tick: on time or late (drift):
now = datetime.datetime.now() #update after wake up
self.delta_time = (now - self._tick_start_clock).total_seconds() #diff from previous tick
self.drift = self.delta_time - self.expected_tick_duration #diff from expected time
self._tick_start_clock = now
#Update globals
passed_time = (now - self._sim_start_clock).total_seconds()
self.sim_time = self.sim_start_time + passed_time
self.tick_count+=1
#stats
self.update_stats()
#Check timeout
if (self.timeout):
if (self.sim_time>=self.timeout):
log.info('{} TIMEOUT: {:.3}s'.format(self.label,self.sim_time))
return False
return True
def update_stats(self):
if LOG_PERFORMANCE:
self.performance_log.append([
self.tick_count,
truncate(self.sim_time,3),
truncate(self.delta_time,3),
truncate(self.drift,3)
])
if self.verbose:
log.info('{:05.2f}s {} Tick {:3}# +{:.3f} e{:.3f} d{:.3f} '.
format(self.sim_time,self.label,self.tick_count, self.delta_time, self.expected_tick_duration, self.drift))
def write_peformance_log(self):
if LOG_PERFORMANCE:
logtime = time.strftime("%Y%m%d-%H%M%S")
filename = "log/{}_performance_log.csv".format(self.label)
log.info('Writting performance log: {}'.format(filename))
with open(filename,mode='w') as csv_file:
csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
titleline =['tickcount', 'sim_time','delta_time', 'drift']
csv_writer.writerow(titleline)
for line in self.performance_log:
csv_writer.writerow(line)
def set_task(self,label,target_t,max_t = None):
#Note: a single task per object.
#Todo: alllow tracking of multiple tasks in the same object
self.task_label = label
self.target_t = target_t
self.next_target_t = target_t
self.max_t = max_t
def get_task_time(self):
return self.target_t
def start_task(self):
self.target_t = self.next_target_t
self.task_start_time = datetime.datetime.now()
def end_task(self, block = True):
self.task_end_time = datetime.datetime.now()
delta_time = (self.task_end_time - self.task_start_time).total_seconds()
diff = self.target_t - delta_time
if diff > 0:
#sleep for the remaining
if block:
time.sleep(diff)
else:
log.error("Task {} took longer than expected. Plan Tick: {}, Expected: {}, Actual: {}".format
(self.task_label, self.tick_count, self.target_t, delta_time))
#if variable taks time, target will be adjusted for next cycle
if self.max_t:
#increase target and cap by max t
new_t = math.ceil((abs(diff)+self.target_t )*100)/100
if new_t < self.max_t:
self.next_target_t = new_t
log.warning("Task '{}' target adjusted from {:3}s to {:3}s".format(self.task_label, self.target_t, self.next_target_t))
else:
self.next_target_t = self.max_t
log.warning("Task '{}' target adjusted to max time {:3}s (consider reducing the tick rate)".format(self.task_label, self.next_target_t))
#returns the last target used
#returns actual time
return delta_time
#For Debug only:
_last_log = None
def clock_log(label):
now = datetime.datetime.now()
if TickSync._last_log is None:
newlog = [label,0.0]
else:
_delta = format((now - TickSync._last_log).total_seconds(),'.4f')
newlog = [label,_delta]
TickSync._last_log = now
log.info(newlog)