Fix systemtest

systemtests/mcap_handler.py

@@ -34,15 +34,19 @@ def typename(topic_name):
 
  def write_mcap_to_csv(self, inputbag:str, outputfile:str):
  '''A method which translates an .mcap rosbag file format to a .csv file. 
+ Also modifies the timestamp to start at 0.0 instead of the wall time.
  Only written to translate the /tf topic but could easily be extended to other topics'''
 
+ t_start = None
  try:
  print("Translating .mcap to .csv")
  f = open(outputfile, 'w+')
  writer = csv.writer(f)
  for topic, msg, timestamp in self.read_messages(inputbag):
  if topic =="/tf":
- t = msg.transforms[0].header.stamp.sec + msg.transforms[0].header.stamp.nanosec *10**(-9) 
+ if t_start == None:
+ t_start = msg.transforms[0].header.stamp.sec + msg.transforms[0].header.stamp.nanosec *10**(-9) 
+ t = msg.transforms[0].header.stamp.sec + msg.transforms[0].header.stamp.nanosec *10**(-9) - t_start
  writer.writerow([t, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z])
  f.close()
  except FileNotFoundError:

systemtests/plotter_class.py

@@ -23,11 +23,13 @@ def __init__(self, sim_backend = False):
  self.test_name = None
 
  self.SIM = sim_backend #indicates if we are plotting data from real life test or from a simulated test. Default is false (real life test)
- self.EPSILON = 0.1 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test (NB : epsilon is doubled for multi_trajectory test)
+ self.EPSILON = 0.15 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test (NB : epsilon is doubled for multi_trajectory test)
  self.ALLOWED_DEV_POINTS = 0.05 #allowed percentage of datapoints whose deviation > EPSILON while still passing test (currently % for fig8 and 10% for mt)
- self.DELAY_CONST_FIG8 = 4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. 
+ self.DELAY_CONST_FIG8 = 1.3 #4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. 
+ self.DELAY_CONST_MT = 5.5
  if self.SIM : #It allows to temporally adjust the ideal and real trajectories on the graph. Could this be implemented in a better (not hardcoded) way ?
- self.DELAY_CONST_FIG8 = -0.18 #for an unknown reason, the delay constant with the sim_backend is different
+ self.DELAY_CONST_FIG8 = -0.45 #for an unknown reason, the delay constants with the sim_backend is different
+ self.DELAY_CONST_MT = -0.3
  self.ALTITUDE_CONST_FIG8 = 1 #this is the altitude given for the takeoff in figure8.py. I should find a better solution than a symbolic constant ?
 
  def file_guard(self, pdf_path):
@@ -86,7 +88,7 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile):
  delay = 0
  if self.test_name == "fig8":
  delay = self.DELAY_CONST_FIG8
- elif self.test_name == "m_t":
+ elif self.test_name == "mt":
  delay = self.DELAY_CONST_MT
 
  for i in range(bag_arrays_size):