Fix systemtest (#464)

* tuning delay

Author: julienthevenoz

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):