imu_timestamp_test.py
Go to the documentation of this file.
1 '''
2 
3 '''
4 import matplotlib
5 import matplotlib.pyplot as plt
6 import numpy as np
7 
8 
10  def __init__(self, logName):
11  self.logName = logName
12 
13  def run(self):
14  # try to parse rostopic-echo file and search for timestamps
15  with open(self.logName, "r") as f:
16  data = f.readlines()
17 
18  state = 0
19  secVal = 0.000;
20  nanoVal = 0.0
21 
22  timestamps = np.zeros(1)
23  firstTime = True
24  for line in data:
25  words = line.split()
26  for word in words:
27  if (state == 1):
28  secVal = float(word)
29  state = 2
30  if (state == 3):
31  nanoVal = float(word)
32  timeStampVal = secVal + 1E-9 * nanoVal
33  print("Timestamp: {timestamp}".format(timestamp=timeStampVal))
34  if (firstTime):
35  firstTime = False
36  timestamps[0] = timeStampVal
37  else:
38  timestamps = np.append(timestamps, timeStampVal)
39  state = 0
40 
41  if (word == 'secs:'):
42  state = 1
43 
44  if (word == 'nsecs:'):
45  state = 3
46  print(words)
47 
48  # now we can analyse and plot the result
49 
50  deltaT = timestamps[1:] - timestamps[:-1]
51  X = np.arange(0, timestamps.size, 1)
52 
53  ax = plt.gca()
54  plt.ylabel('Zeitdifferenz t(x)-t(x-1)')
55  plt.xlabel('relativer Rospaketindex x')
56  plt.title('Zeitverhalten der IMU Timestamps nach langen Differenzen')
57  ax.grid(which='major', axis='both', linestyle='--')
58 
59  for i in range(0, deltaT.size - 1):
60  if deltaT[i] > np.mean(deltaT) + 2 * np.std(deltaT):
61  print('ERROR INDEX:' + str(i) + ' Timestamp[INDEX-1]:' + str(timestamps[i]) + ' Timestamp[INDEX]:' + str(
62  timestamps[i + 1]))
63  plt.plot(deltaT)
64  plt.show()
65  plt.savefig('imutimestampsmu+2sigma.png', dpi=300)
66 
67 
68 if __name__ == "__main__":
69  filename = "/tmp/imu.txt" # create such a file by running rostopic echo /imu >/tmp/imu.txt for about 10 secs.
70 
71  imuTest = ImuTimeStampTester(filename)
72  imuTest.run()


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Tue Jul 9 2019 04:55:32