logReader.py
Go to the documentation of this file.
1 import numpy as np
2 import sys
3 import glob
4 import serial
5 import os
6 import subprocess
7 from os.path import expanduser
8 from scipy.interpolate import interp1d
9 
10 import sys
11 # Add the ptdraft folder path to the sys.path list
12 import sys
13 sys.path.append('..')
14 from log_reader import LogReader
15 import yaml
16 # from ci_hdw.data_sets import *
17 from pylib.data_sets import *
18 from pylib.pose import *
19 import datetime
20 from pylib.ISToolsDataSorted import refLla, getTimeFromTowMs, getTimeFromTow, setGpsWeek, getTimeFromGTime
21 
22 RAD2DEG = 180.0 / np.pi
23 DEG2RAD = np.pi / 180.0
24 
25 RED = '\u001b[31m'
26 RESET = '\u001b[0m'
27 
28 class Log:
29  def __init__(self):
30  self.c_log = LogReader()
31  self.data = []
32  self.serials = []
33  self.passRMS = 0 # 1 = pass, -1 = fail, 0 = unknown
34 
35  def load(self, directory, serials=['ALL']):
36  self.data = []
37  self.c_log.init(self, directory, serials)
38  self.c_log.load()
39  self.sanitize()
40  self.data = np.array(self.data)
41  self.directory = directory
42  self.numDev = self.data.shape[0]
43  if self.numDev == 0:
44  raise ValueError("No devices found in log")
45  self.serials = [self.data[d, DID_DEV_INFO]['serialNumber'][0] for d in range(self.numDev)]
46  if 10101 in self.serials:
47  self.refINS = True
48  refIdx = self.serials.index(10101)
49  self.truth = self.data[refIdx].copy()
50  else:
51  self.refINS = False
52  self.refdata = []
53  self.compassing = 'Cmp' in str(self.data[0, DID_DEV_INFO]['addInfo'][-1])
54  self.rtk = 'Rov' in str(self.data[0, DID_DEV_INFO]['addInfo'][-1])
55  self.navMode = (self.data[0, DID_INS_2]['insStatus'][-1] & 0x1000) == 0x1000
56  # except:
57  # print(RED + "error loading log" + sys.exc_info()[0] + RESET)
58 
59  def exitHack(self):
60  self.c_log.exitHack()
61 
62  def did_callback(self, did, arr, dev_id):
63  if did >= NUM_DIDS:
64  return
65  while dev_id >= len(self.data):
66  self.data.append([[] for i in range(NUM_DIDS+1)])
67  self.data[dev_id][did] = arr
68 
69  def gps_raw_data_callback(self, did, arr, dev_id, msg_type):
70  if did >= NUM_DIDS:
71  return
72  if self.data[dev_id][did] == []:
73  self.data[dev_id][did] = [[] for i in range(6)]
74  if dev_id < len(self.data) and did < len(self.data[dev_id]) and 6 <= len(self.data[dev_id][did]):
75  if msg_type == eRawDataType.raw_data_type_observation.value:
76  self.data[dev_id][did][0].append(arr)
77  elif msg_type == eRawDataType.raw_data_type_ephemeris.value:
78  self.data[dev_id][did][1] = arr
79  elif msg_type == eRawDataType.raw_data_type_glonass_ephemeris.value:
80  self.data[dev_id][did][2] = arr
81  elif msg_type == eRawDataType.raw_data_type_sbas.value:
82  self.data[dev_id][did][3] = arr
83  elif msg_type == eRawDataType.raw_data_type_ionosphere_model_utc_alm.value:
84  self.data[dev_id][did][4] = arr
85  elif msg_type == eRawDataType.raw_data_type_base_station_antenna_position.value:
86  self.data[dev_id][did][5] = arr
87 
88 
89  def sanitize(self):
90  return
91  GPS_start_Time = datetime.datetime.strptime('6/Jan/1980', "%d/%b/%Y")
92 
93  # Use DID_INS_1 if necessary
94  if np.size(self.data[0][DID_INS_2])==0 and np.size(self.data[0][DID_INS_1])!=0:
95  self.data[0][DID_INS_2].resize(np.size(self.data[0][DID_INS_1]))
96  self.data[0][DID_INS_2]['timeOfWeek'] = self.data[0][DID_INS_1]['timeOfWeek']
97  self.data[0][DID_INS_2]['week'] = self.data[0][DID_INS_1]['week']
98  self.data[0][DID_INS_2]['insStatus'] = self.data[0][DID_INS_1]['insStatus']
99  self.data[0][DID_INS_2]['hdwStatus'] = self.data[0][DID_INS_1]['hdwStatus']
100  self.data[0][DID_INS_2]['qn2b'] = euler2quatArray(self.data[0][DID_INS_1]['theta'])
101  self.data[0][DID_INS_2]['uvw'] = self.data[0][DID_INS_1]['uvw']
102  self.data[0][DID_INS_2]['lla'] = self.data[0][DID_INS_1]['lla']
103 
104  week_time = GPS_start_Time + (datetime.timedelta(weeks=int(self.data[0][DID_INS_2]['week'][-1])))
105 
106  for d, dev in enumerate(self.data):
107  if len(dev[DID_INS_2]) == 0:
108  print("\033[93m" + "missing DID_INS_2 data: removing device\033[0m")
109  del self.data[d]
110  break
111 
112  if len(dev[DID_DEV_INFO]) == 0:
113  print("\033[93m" + "missing DID_DEV_INFO data: making some up\033[0m")
114  self.data[d][DID_DEV_INFO] = np.resize(self.data[d][DID_DEV_INFO], 1)
115  self.data[d][DID_DEV_INFO]['serialNumber'][0] = d
116 
117  for d, dev in enumerate(self.data):
118  for did in range(len(self.data[0])):
119  if isinstance(dev[did], list):
120  continue
121  for field in ['towMs', 'timeOfWeekMs', 'tow', 'timeOfWeek']:
122  if field in dev[did].dtype.names:
123  if (np.diff(dev[did][field].astype(np.int64)) < 0).any():
124  idx = np.argmin(np.diff(dev[did][field].astype(np.int64)))
125  if 'Ms' in field:
126  t1 = week_time + datetime.timedelta(milliseconds=int(dev[did][field][idx]))
127  t2 = week_time + datetime.timedelta(milliseconds=int(dev[did][field][idx+1]))
128  else:
129  t1 = week_time + datetime.timedelta(seconds=int(dev[did][field][idx]))
130  t2 = week_time + datetime.timedelta(seconds=int(dev[did][field][idx+1]))
131  print("\033[93m" + "Time went backwards in " + did_name_lookup[did] + r"!!!, " +
132  " Time went from " + str(t1) + " to " + str(t2) + ". removing all data "
133  + ("before" if idx < len(dev[did][field])/2.0 else "after") + "\033[0m")
134  if idx < len(dev[did][field])/2.0:
135  self.data[d][did] = dev[did][idx+1:]
136  else:
137  self.data[d][did] = dev[did][:idx]
138  ms_multiplier = 1000.0 if 'Ms' in field else 1.0
139  if (np.diff(dev[did][field]) > 3600 * ms_multiplier).any():
140  print("\033[93m" + "greater than 1 minute gap in " + did_name_lookup[did]
141  + " data, assuming GPS fix was acquired during data set, and chopping data"+ "\033[0m")
142  idx = np.argmax(np.diff(dev[did][field])) + did
143  self.data[d][did] = dev[did][idx:]
144 
145  def getRMSArray(self):
146  if self.numDev > 1 or self.refINS:
147  print("\nComputing RMS Accuracies: (%d devices)" % (self.numDev))
148 
149  # Build a 3D array of the data. idx 0 = Device, idx 1 = t, idx 2 = [t, lla, uvw, log(q)]
150  data = [np.hstack((self.data[i, DID_INS_2]['timeOfWeek'][:, None],
151  self.data[i, DID_INS_2]['lla'],
152  self.data[i, DID_INS_2]['uvw'],
153  self.data[i, DID_INS_2]['qn2b'])) for i in range(self.numDev)]
154 
155  # Make sure that the time stamps are realistic
156  for dev in range(self.numDev):
157  if (np.diff(data[dev][:, 0]) > 10.0).any():
158  print("\033[93m" + "large gaps in data for dev" + str(dev)
159  + "chopping off data before gap".format(dev) + "\033[0m")
160  idx = np.argmax(np.diff(data[dev][:, 0])) + 1
161  data[dev] = data[dev][idx:, :]
162 
163  self.min_time = max([np.min(data[i][:, 0]) for i in range(self.numDev)])
164  self.max_time = min([np.max(data[i][:, 0]) for i in range(self.numDev)])
165 
166  # If we are in compassing mode, then only calculate RMS after all devices have fix
167  if self.compassing:
168  # time_of_fix_ms = [self.data[dev, DID_GPS1_RTK_CMP_REL]['timeOfWeekMs'][np.argmax(self.data[dev, DID_GPS1_RTK_CMP_REL]['arRatio'] > 3.0)] / 1000.0 for dev in range(self.numDev)]
169  time_of_fix_ms = [self.data[dev, DID_GPS1_POS]['timeOfWeekMs'][np.argmax(self.data[dev, DID_GPS1_POS]['status'] & 0x08000000)] / 1000.0 for dev in range(self.numDev)]
170  # print time_of_fix_ms
171  self.min_time = max(time_of_fix_ms)
172 
173  # Use middle third of data
174  self.min_time = self.max_time - 2.0*(self.max_time - self.min_time)/3.0
175  self.max_time = self.max_time - (self.max_time - self.min_time)/3.0
176 
177  # Resample at a steady 100 Hz
178  dt = 0.01
179  t = np.arange(1.0, self.max_time - self.min_time - 1.0, dt)
180  for i in range(self.numDev):
181  # Chop off extra data at beginning and end
182  data[i] = data[i][data[i][:, 0] > self.min_time]
183  data[i] = data[i][data[i][:, 0] < self.max_time]
184 
185  # Chop off the min time so everything is wrt to start
186  data[i][:, 0] -= self.min_time
187 
188  # Interpolate data so that it has all the same timestamps
189  fi = interp1d(data[i][:, 0], data[i][:, 1:].T, kind='cubic', fill_value='extrapolate',
190  bounds_error=False)
191  data[i] = np.hstack((t[:, None], fi(t).T))
192 
193  # Normalize Quaternions
194  data[i][:, 7:] /= norm(data[i][:, 7:], axis=1)[:, None]
195 
196  # Make a big 3D numpy array we can work with [dev, sample, data]
197  data = np.array(data)
198 
199  # Convert lla to ned using first device lla at center of data as reference
200  refLla = data[0, int(round(len(t) / 2.0)), 1:4].copy()
201  for i in range(self.numDev):
202  data[i, :, 1:4] = lla2ned(refLla, data[i, :, 1:4])
203 
204  self.stateArray = data
205 
206  def getRMSTruth(self):
207  if not self.refINS:
208  # Find Mean Data
209  means = np.empty((len(self.stateArray[0]), 10))
210  means[:, :6] = np.mean(self.stateArray[:, :, 1:7], axis=0) # calculate mean position and velocity across devices
211  means[:, 6:] = meanOfQuatArray(self.stateArray[:, :, 7:].transpose((1, 0, 2))) # Calculate mean attitude of all devices at each timestep
212  self.truth = means
213  else:
214  self.refIdx = self.serials.index(10101)
215  self.truth = self.stateArray[self.refIdx, :, 1:]
216  self.stateArray = np.delete(self.stateArray, self.refIdx, 0)
217 
218 
219  def calcAttitudeError(self):
220  att_error = np.array([qboxminus(self.stateArray[dev, :, 7:], self.truth[:, 6:]) for dev in range(len(self.stateArray))])
221  self.att_error = att_error
222 
223 
224  def calculateRMS(self):
225  self.data = np.array(self.data)
226  self.getRMSArray()
227  self.getRMSTruth()
228  self.calcAttitudeError()
229 
230  # Calculate the Mounting Bias for all devices (assume the mounting bias is the mean of the attitude error)
231  self.mount_bias = np.mean(self.att_error, axis=1)
232  if self.compassing:
233  # When in compassing, assume all units are sharing the same GPS antennas and should therefore have
234  # no mounting bias in heading
235  self.mount_bias[:, 2] = 0
236  self.att_error = self.att_error - self.mount_bias[:,None,:]
237 
238  # RMS = sqrt ( 1/N sum(e^2) )
239  self.RMS = np.empty((len(self.stateArray), 9))
240  self.RMS[:,:6] = np.sqrt(np.mean(np.square(self.stateArray[:, :, 1:7] - self.truth[:,0:6]), axis=1)) # [ pos, vel ]
241  self.RMS[:,6:] = np.sqrt(np.mean(np.square(self.att_error[:, :, :]), axis=1)) # [ att }
242  self.RMS_euler = self.RMS[:, 6:] # quat2eulerArray(qexp(RMS[:,6:]))
243 
244  # Average RMS across devices
245  self.averageRMS = np.mean(self.RMS, axis=0)
246  self.averageRMS_euler = self.averageRMS[6:] # quat2eulerArray(qexp(averageRMS[None,6:]))[0]
247  self.mount_bias_euler = self.mount_bias # quat2eulerArray(qexp(mount_bias))
248 
249  def pass_fail(self, ratio):
250  if ratio > 1.0:
251  self.tmpPassRMS = -1
252  return 'FAIL'
253  else:
254  # self.tmpPassRMS = -1 # Debug
255  return 'PASS'
256 
257  def printRMSReport(self):
258  self.tmpPassRMS = 1
259  filename = os.path.join(self.directory, 'RMS_report_new_logger.txt')
260  thresholds = np.array([0.35, 0.35, 0.8, # (m) NED
261  0.2, 0.2, 0.2, # (m/s) UVW
262  0.11, 0.11, 2.0]) # (deg) ATT (roll, pitch, yaw)
263  if self.navMode or self.compassing:
264  thresholds[8] = 0.3 # Higher heading accuracy
265  else:
266  thresholds[:6] = np.inf
267 
268  if self.compassing:
269  thresholds[0] = 1.0
270  thresholds[1] = 1.0
271  thresholds[2] = 1.0
272 
273  thresholds[6:] *= DEG2RAD # convert degrees threshold to radians
274 
275  self.specRatio = self.averageRMS / thresholds
276 
277  uINS_device_idx = [n for n in range(self.numDev) if self.serials[n] != 10101]
278 
279  f = open(filename, 'w')
280  f.write('***** Performance Analysis Report - %s *****\n' % (self.directory))
281  f.write('\n')
282  f.write('Directory: %s\n' % (self.directory))
283  mode = "AHRS"
284  if self.navMode: mode = "NAV"
285  if self.compassing: mode = "DUAL GNSS"
286  if self.refINS: mode += " With NovaTel Reference"
287  f.write("\n")
288 
289  # Print Table of RMS accuracies
290  line = 'Device '
291  if self.navMode:
292  f.write(
293  '--------------------------------------------------- RMS Accuracy -------------------------------------------\n')
294  line = line + 'UVW[ (m/s) (m/s) (m/s) ], NED[ (m) (m) (m) ],'
295  else: # AHRS mode
296  f.write('-------------- RMS Accuracy --------------\n')
297  line = line + ' Att [ (deg) (deg) (deg) ]\n'
298  f.write(line)
299 
300  for n, dev in enumerate(uINS_device_idx):
301  devInfo = self.data[dev,DID_DEV_INFO][0]
302  line = '%2d SN%d ' % (n, devInfo['serialNumber'])
303  if self.navMode:
304  line = line + '[ %6.4f %6.4f %6.4f ], ' % (self.RMS[n, 3], self.RMS[n, 4], self.RMS[n, 5])
305  line = line + '[ %6.4f %6.4f %6.4f ], ' % (self.RMS[n, 0], self.RMS[n, 1], self.RMS[n, 2])
306  line = line + '[ %6.4f %6.4f %6.4f ]\n' % (
307  self.RMS_euler[n, 0] * RAD2DEG, self.RMS_euler[n, 1] * RAD2DEG, self.RMS_euler[n, 2] * RAD2DEG)
308  f.write(line)
309 
310  line = 'AVERAGE: '
311  if self.navMode:
312  f.write(
313  '------------------------------------------------------------------------------------------------------------\n')
314  line = line + '[%7.4f %7.4f %7.4f ], ' % (self.averageRMS[3], self.averageRMS[4], self.averageRMS[5])
315  line = line + '[%7.4f %7.4f %7.4f ], ' % (self.averageRMS[0], self.averageRMS[1], self.averageRMS[2])
316  else: # AHRS mode
317  f.write('------------------------------------------\n')
318  line = line + '[%7.4f %7.4f %7.4f ]\n' % (
319  self.averageRMS_euler[0] * RAD2DEG, self.averageRMS_euler[1] * RAD2DEG, self.averageRMS_euler[2] * RAD2DEG)
320  f.write(line)
321 
322  line = 'THRESHOLD: '
323  if self.navMode:
324  line = line + '[%7.4f %7.4f %7.4f ], ' % (thresholds[3], thresholds[4], thresholds[5])
325  line = line + '[%7.4f %7.4f %7.4f ], ' % (thresholds[0], thresholds[1], thresholds[2])
326  line = line + '[%7.4f %7.4f %7.4f ]\n' % (
327  thresholds[6] * RAD2DEG, thresholds[7] * RAD2DEG, thresholds[8] * RAD2DEG)
328  f.write(line)
329 
330  line = 'RATIO: '
331  if self.navMode:
332  f.write(
333  '------------------------------------------------------------------------------------------------------------\n')
334  line = line + '[%7.4f %7.4f %7.4f ], ' % (self.specRatio[3], self.specRatio[4], self.specRatio[5])
335  line = line + '[%7.4f %7.4f %7.4f ], ' % (self.specRatio[0], self.specRatio[1], self.specRatio[2])
336  else: # AHRS mode
337  f.write('------------------------------------------\n')
338  line = line + '[%7.4f %7.4f %7.4f ]\n' % (self.specRatio[6], self.specRatio[7], self.specRatio[8])
339  f.write(line)
340 
341  line = 'PASS/FAIL: '
342  if self.navMode:
343  line = line + '[ %s %s %s ], ' % (
344  self.pass_fail(self.specRatio[3]), self.pass_fail(self.specRatio[4]), self.pass_fail(self.specRatio[5])) # LLA
345  line = line + '[ %s %s %s ], ' % (
346  self.pass_fail(self.specRatio[0]), self.pass_fail(self.specRatio[1]), self.pass_fail(self.specRatio[2])) # UVW
347  line = line + '[ %s %s %s ]\n' % (
348  self.pass_fail(self.specRatio[6]), self.pass_fail(self.specRatio[7]), self.pass_fail(self.specRatio[8])) # ATT
349  f.write(line)
350 
351  if self.navMode:
352  f.write(' ')
353  else: # AHRS mode
354  f.write(' ')
355  f.write('(' + mode + ')\n\n')
356 
357  # Print Mounting Biases
358  f.write('--------------- Angular Mounting Biases ----------------\n')
359  f.write('Device Euler Biases[ (deg) (deg) (deg) ]\n')
360  for n, dev in enumerate(uINS_device_idx):
361  devInfo = self.data[dev, DID_DEV_INFO][0]
362  f.write('%2d SN%d [ %7.4f %7.4f %7.4f ]\n' % (
363  n, devInfo['serialNumber'], self.mount_bias_euler[n, 0] * RAD2DEG, self.mount_bias_euler[n, 1] * RAD2DEG,
364  self.mount_bias_euler[n, 2] * RAD2DEG))
365  f.write('\n')
366 
367  f.write("----------------- Average Attitude ---------------------\n")
368  f.write("Dev: \t[ Roll\t\tPitch\t\tYaw ]\n")
369  for i in range(self.numDev):
370  qavg = meanOfQuat(self.stateArray[i, :, 7:])
371  euler = quat2euler(qavg.T) * 180.0 / np.pi
372  f.write("%d\t%f\t%f\t%f\n" % (self.serials[i], euler[0], euler[1], euler[2]))
373 
374  # Print Device Version Information
375  f.write(
376  '\n\n------------------------------------------- Device Info -------------------------------------------------\n')
377  for n, dev in enumerate(uINS_device_idx):
378  devInfo = self.data[dev, DID_DEV_INFO][0]
379  hver = devInfo['hardwareVer']
380  cver = devInfo['protocolVer']
381  fver = devInfo['firmwareVer']
382  buld = devInfo['buildNumber']
383  repo = devInfo['repoRevision']
384  date = devInfo['buildDate']
385  time = devInfo['buildTime']
386  addi = devInfo['addInfo']
387  f.write(
388  '%2d SN%d HW: %d.%d.%d.%d FW: %d.%d.%d.%d build %d repo %d Proto: %d.%d.%d.%d Date: %04d-%02d-%02d %02d:%02d:%02d %s\n' % (
389  n, devInfo['serialNumber'],
390  hver[3], hver[2], hver[1], hver[0],
391  fver[3], fver[2], fver[1], fver[0], buld, repo,
392  cver[3], cver[2], cver[1], cver[0],
393  2000 + date[2], date[1], date[0],
394  time[3], time[2], time[1],
395  addi))
396  f.write('\n')
397 
398  f.close()
399 
400  # Report if RMS passed all
401  self.passRMS = self.tmpPassRMS
402  return self.passRMS
403 
404  def debugPlot(self):
405  import matplotlib.pyplot as plt
406  colors = ['r', 'g', 'b', 'm']
407  plt.figure()
408  plt.subplot(3,1,1) # Position
409  plt.title("position error")
410  for m in range(3):
411  for n in range(len(self.stateArray)):
412  plt.plot(self.stateArray[n,:,0], self.stateArray[n, :, m+1], color = colors[m])
413  plt.plot(self.stateArray[0,:,0], self.truth[:, m], linewidth=2, color = colors[m])
414  plt.subplot(3,1,2)
415  plt.title("velocity error")
416  for m in range(3):
417  for n in range(len(self.stateArray)):
418  plt.plot(self.stateArray[n,:,0], self.stateArray[n, :, m+4], color = colors[m] )
419  plt.plot(self.stateArray[0,:,0], self.truth[:, m+3], linewidth=2, color = colors[m])
420  plt.subplot(3,1,3)
421  plt.title("attitude")
422  for m in range(4):
423  for n in range(len(self.stateArray)):
424  plt.plot(self.stateArray[n,:,0], self.stateArray[n, :, m+7], color = colors[m])
425  plt.plot(self.stateArray[0,:,0], self.truth[:, m+6], linewidth=2, color = colors[m])
426 
427  plt.figure()
428  for m in range(3):
429  plt.subplot(3, 1, m +1)
430  for n in range(len(self.stateArray)):
431  plt.plot(self.att_error[n, :, m])
432  plt.show()
433 
434  def openRMSReport(self):
435  filename = os.path.join(self.directory, 'RMS_report_new_logger.txt')
436  if 'win' in sys.platform:
437  subprocess.Popen(["notepad.exe", filename])
438  if 'linux' in sys.platform:
439  subprocess.Popen(['gedit', filename])
440 
441 
442 if __name__ == '__main__':
443  np.set_printoptions(linewidth=200)
444 
445  home = expanduser("~")
446 
447  # 2nd argument: Log Directory
448  if len(sys.argv) >= 2:
449  directory = sys.argv[1]
450  serials = ["ALL"]
451 
452  if 'directory' not in locals():
453  print("First parameter must be directory!")
454  exit()
455 
456  # Load from config.yaml
457  file = open(home + "/Documents/Inertial_Sense/config.yaml", 'r')
458  config = yaml.load(file)
459  directory = config["directory"]
460  serials = ["ALL"]
461 
462  log = Log()
463  log.load(directory)
464 
465  # Compute and output RMS Report
466  log.calculateRMS()
467  # log.debugPlot()
468  log.printRMSReport()
469  log.openRMSReport()
python.logInspector.logReader.Log.did_callback
def did_callback(self, did, arr, dev_id)
Definition: logReader.py:62
python.pylib.pose.lla2ned
def lla2ned(llaRef, lla)
Definition: pose.py:429
python.pylib.pose.meanOfQuatArray
def meanOfQuatArray(q)
Definition: pose.py:796
init
bool init(test_data_t &t)
Definition: test_com_manager.cpp:280
Catch::Generators::range
GeneratorWrapper< T > range(T const &start, T const &end, T const &step)
Definition: catch.hpp:4141
max
#define max(a, b)
Takes the maximal value of a and b.
Definition: compiler.h:823
python.pylib.pose.qboxminus
def qboxminus(q1, q2)
Definition: pose.py:778
python.logInspector.logReader.Log.gps_raw_data_callback
def gps_raw_data_callback(self, did, arr, dev_id, msg_type)
Definition: logReader.py:69
python.pylib.pose.norm
def norm(v, axis=None)
Definition: pose.py:695
python.logInspector.logReader.Log.load
def load(self, directory, serials=['ALL'])
Definition: logReader.py:35
python.pylib.pose.quat2euler
def quat2euler(q)
Definition: pose.py:126
python.pylib.pose.euler2quatArray
def euler2quatArray(euler)
Definition: pose.py:168
python.pylib.pose.meanOfQuat
def meanOfQuat(q)
Definition: pose.py:784

inertial_sense_ros
Author(s):
autogenerated on Sun Feb 28 2021 03:17:57