diff --git a/systemtests/mcap_handler.py b/systemtests/mcap_handler.py index b5945a214..ed68d720c 100644 --- a/systemtests/mcap_handler.py +++ b/systemtests/mcap_handler.py @@ -7,7 +7,7 @@ class McapHandler: def __init__(self): - pass + self.takeoff_time = None def read_messages(self, input_bag: str): reader = rosbag2_py.SequentialReader() @@ -36,18 +36,32 @@ 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 + t_start_bag = None #this is the timestamp of the first message we read in the bag (doesn't mean it's exactly the start time of the bag but close enough ?) try: print("Translating .mcap to .csv") f = open(outputfile, 'w+') writer = csv.writer(f) + writer.writerow(["# t"," x"," y"," z"]) for topic, msg, timestamp in self.read_messages(inputbag): if topic =="/tf": - 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 + if t_start_bag == None: + t_start_bag = 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_bag writer.writerow([t, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z]) + if topic == "/rosout": #and t_start_bag != None : + if msg.name == "crazyflie_server" and msg.function == "takeoff": + t = msg.stamp.sec + msg.stamp.nanosec *10**(-9) - t_start_bag + self.takeoff_time = t + #the takeoff message in simulation has a sligthly different name than IRL, so we need this to record the sim takeoff time + #BUT for some unknown reason the sim tests seem to work perfectly without even recording takeoff and adjusting the offset + #so I will leave this commented here just in case the sim tests have to be worked on + # if msg.name == "crazyflie_server" and msg.function == "_takeoff_callback": + # t = msg.stamp.sec + msg.stamp.nanosec *10**(-9) + # self.takeoff_time = t + # print(f"takeoff at {self.takeoff_time}") + + #write the "takeoff command" time as a comment on the last line + writer.writerow([f"### takeoff time : {self.takeoff_time}"]) f.close() except FileNotFoundError: print(f"McapHandler : file {outputfile} not found") @@ -67,5 +81,4 @@ def write_mcap_to_csv(self, inputbag:str, outputfile:str): args:Namespace = parser.parse_args() translator = McapHandler() - translator.write_mcap_to_csv(args.inputbag,args.outputfile) - + translator.write_mcap_to_csv(args.inputbag,args.outputfile) \ No newline at end of file diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py index 42a8307df..96c632eda 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -2,6 +2,7 @@ import os import sys from matplotlib.backends.backend_pdf import PdfPages +from matplotlib.patches import Rectangle import numpy as np from crazyflie_py.uav_trajectory import Trajectory from pathlib import Path @@ -24,12 +25,12 @@ def __init__(self, sim_backend = False): 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.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.ideal_takeoff = 0.6 + self.ideal_traj_start = 5.6 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 = 1.3 #delay const used to temporally align the ideal traj with the real traj - 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.45 #for an unknown reason, the delay constants with the sim_backend is different - self.DELAY_CONST_MT = -0.3 + # 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#-0.45 #for an unknown reason, the delay constants with the sim_backend is different + # self.DELAY_CONST_MT = 0#-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): @@ -74,6 +75,21 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): self.adjust_arrays() bag_arrays_size = len(self.bag_times) print("number of datapoints in self.bag_*: ",bag_arrays_size) + + + #since the rosbag doesn't start at a reliable time, we need to adjust the ideal time array and the real one so that they aren't offset. For this we compare the time where the "takeoff" command was + # given to the crazyflie with the time of takeoff in the desired trajectory. With this we have the time-delay which we need to correct the offset (NB : empirically modified by 0.15 seconds this + #time-delay because that seems to be the delay between receiving the takeoff command and actually flying off) + with open(rosbag_csvfile) as f: + lines = f.readlines() + lastline = lines[-1] #get last line of csv file, where the takeoff time is written as comment + try: + self.takeoff_time = float(lastline[lastline.find(":") + 1 :]) #get the "takeoff" time from last line of csv + except ValueError: #if no takeoff was issued, there will be a "None" in the lastline which will produce value error when converting to float + print("Warning : No takeoff written in the lastline of the rosbag csv file : offset cannot be corrected.") + self.takeoff_time = 0 + offset1 = (self.ideal_takeoff - self.takeoff_time) - 0.15 + #####calculate ideal trajectory points corresponding to the times of recorded points @@ -81,23 +97,21 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): self.ideal_traj_y = np.empty([bag_arrays_size]) self.ideal_traj_z = np.empty([bag_arrays_size]) self.euclidian_dist = np.empty([bag_arrays_size]) - no_match_in_idealcsv=[] - delay = 0 - if self.test_name == "fig8": - delay = self.DELAY_CONST_FIG8 - elif self.test_name == "mt": - delay = self.DELAY_CONST_MT - + delay = offset1 + # if self.test_name == "fig8" and self.SIM: + # delay = self.DELAY_CONST_FIG8 + # elif self.test_name == "mt" and self.SIM: + # delay = self.DELAY_CONST_MT + for i in range(bag_arrays_size): try: - pos = self.ideal_traj_csv.eval(self.bag_times[i] - delay).pos + pos = self.ideal_traj_csv.eval(self.bag_times[i] + delay).pos except AssertionError: no_match_in_idealcsv.append(i) pos = [0,0,0] #for all recorded datapoints who cannot be matched to a corresponding ideal position we assume the drone is on its ground start position (ie those datapoints are before takeoff or after landing) - self.ideal_traj_x[i], self.ideal_traj_y[i], self.ideal_traj_z[i]= pos[0], pos[1], pos[2] @@ -136,44 +150,33 @@ def no_match_warning(self, unmatched_values:list): def adjust_arrays(self): - ''' Method that trims the self.bag_* attributes to get rid of the datapoints where the drone is immobile on the ground and makes self.bag_times start at 0 [s]''' + ''' Method that adjusts the self.bag_* attributes to get rid of the datapoints whose timestamp don't make sense''' print(f"rosbag initial length {(self.bag_times[-1]-self.bag_times[0]) }s") - #find the takeoff time and landing times - ground_level = self.bag_z[0] - airborne = False - takeoff_index = None - landing_index = None - i=0 - for z_coord in self.bag_z: - if not(airborne) and z_coord > ground_level + 0.05: #when altitude of the drone is 5cm above ground level, it started takeoff - takeoff_index = i - airborne = True - print(f"takeoff time is {self.bag_times[takeoff_index]}s") - if airborne and z_coord < ground_level + 0.05: #find when it lands again - landing_index = i - print(f"landing time is {self.bag_times[landing_index]}s") - break - i+=1 - - - assert (takeoff_index != None) and (landing_index != None), "Plotter : couldn't find drone takeoff or landing" + #recurring problem : some messages recorded from /tf arrive way later (2-4 seconds) than when they were emitted, which makes a mess in the timestamps + #zB we have timestamps (in s) : 5.1, 5.2, 5.3, !!!3.7, 3.9, 4.5, 4.9!!!, 5.4, 5.5, 5.6, 5.7 etc. This bug almost always occurs in the first seconds of the recording & generally only happens once per recording + #and only concerns a very small percentage of datapoints (10-40 over about 1800 total), so it is not a big deal. We do not know if this bug stems from the Rosbag recording, from how /tf behaves or from the radio - ####get rid of datapoints before takeoff and after landing in bag_times, bag_x, bag_y, bag_y + #Since we use a lineplot, we need to get rid of these datapoints that are in an order that doesn't make sense so that the plot can be readable + self.nonsensical = [] #list of indexes of datapoints who arrived too late, meaning their timestamp doesn't follow the ones before + time = -1 + for index,t in enumerate(self.bag_times): + if t > time: + time = t + else: + self.nonsensical.append(index) - assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size before trimming" - index_arr = np.arange(len(self.bag_times)) - slicing_arr = np.delete(index_arr, index_arr[takeoff_index:landing_index+1]) #in our slicing array we take out all the indexes of when the drone is in flight so that it only contains the indexes of when the drone is on the ground - # #delete the datapoints where drone is on the ground - # self.bag_times = np.delete(self.bag_times, slicing_arr) - # self.bag_x = np.delete(self.bag_x, slicing_arr) - # self.bag_y = np.delete(self.bag_y, slicing_arr) - # self.bag_z = np.delete(self.bag_z, slicing_arr) + if self.nonsensical: #if self.nonsensical is not empty + self.bag_times = np.delete(self.bag_times, self.nonsensical) + self.bag_x = np.delete(self.bag_x, self.nonsensical) + self.bag_y = np.delete(self.bag_y, self.nonsensical) + self.bag_z = np.delete(self.bag_z, self.nonsensical) + print(f"{len(self.nonsensical)} datapoints were ignored because because their timestamp wasn't in the good order (delayed message problem). They go from index {self.nonsensical[0]} to {self.nonsensical[-1]}") - assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size after trimming" + assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size after adjusting arrays" print(f"trimmed bag_times starts: {self.bag_times[0]}s and ends: {self.bag_times[-1]}, size: {len(self.bag_times)}") @@ -200,15 +203,11 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, ove self.read_csv_and_set_arrays(ideal_csvfile,rosbag_csvfile) - offset_list = self.find_temporal_offset() - if len(offset_list) == 1: - offset_string = f"temporal offset : {offset_list[0]}s \n" - elif len(offset_list) ==2: - offset_string = f"averaged temporal offset : {(offset_list[0]+offset_list[1])/2}s \n" - passed="failed" - if self.test_passed(): - passed = "passed" + test_result="failed" + passed, percentage = self.test_passed() + if passed: + test_result = "passed" #create PDF to save figures if(pdfname[-4:] != ".pdf"): @@ -228,14 +227,19 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, ove name = "Unnamed test" print("Plotter : test name not defined") - text = f'{name} trajectory test' + title = f'{name} trajectory test' + if self.SIM: + title += "(SIMULATION)" title_text_settings = f'Settings:\n' title_text_parameters = f'Parameters:\n' for key, value in self.params.items(): title_text_parameters += f" {key}: {value}\n" - title_text_results = f'Results: test {passed}\n' + offset_string + f'max error : ' + title_text_results = f'Results: test {test_result}\n' + f"acceptable deviation EPSILON: {self.EPSILON}[m]\n" + title_text_results += f"percentage of points > EPSILON : {percentage:.4f}%\n" + f"average error : {np.mean(self.euclidian_dist):.6f} [m]\n" + title_text_results += f"median error : {np.median(self.euclidian_dist):.6f} [m]\n" + f"max error : {np.max(self.euclidian_dist):.6f} [m]\n" + - title_text = text + "\n" + title_text_settings + "\n" + title_text_parameters + "\n" + title_text_results + title_text = title + "\n" + title_text_settings + "\n" + title_text_parameters + "\n" + title_text_results fig = plt.figure(figsize=(5,8)) fig.text(0.1, 0.1, title_text, size=11) @@ -249,14 +253,12 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, ove ax.plot(self.bag_times, self.bag_x, label='Recorded trajectory') ax.set_xlabel('time [s]') ax.set_ylabel('x position [m]') - ax.set_title("Trajectory x") - + ax.set_title("Trajectory x") ax.grid(which='major', color='#DDDDDD', linewidth=0.8) ax.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) ax.minorticks_on() fig.tight_layout(pad = 4) - fig.legend() - + fig.legend() pdf_pages.savefig(fig) fig2, ax2 = plt.subplots() @@ -264,7 +266,7 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, ove ax2.plot(self.bag_times, self.bag_y, label='Recorded trajectory') ax2.set_xlabel('time [s]') ax2.set_ylabel('y position [m]') - ax2.set_title("Trajectory y") + ax2.set_title("trajectory y") ax2.grid(which='major', color='#DDDDDD', linewidth=0.8) ax2.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) ax2.minorticks_on() @@ -324,52 +326,31 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, ove plt.tight_layout(pad=4) pdf_pages.savefig(fig6) - - pdf_pages.close() print("Results saved in " + pdfname) - def test_passed(self) -> bool : - '''Returns True if the deviation between recorded and ideal trajectories of the drone didn't exceed EPSILON for more than ALLOWED_DEV_POINTS % of datapoints. - Returns False otherwise ''' + def test_passed(self) -> tuple : + '''Returns a tuple containing (bool:passed, float:percentage). If the deviation between recorded and ideal trajectories of the drone didn't exceed + EPSILON for more than ALLOWED_DEV_POINTS % of datapoints, the boolean passed is True. Otherwise it is false. float:percentage is the percentage + of points which whose deviation is less than EPSILON''' nb_dev_points = len(self.deviation) threshold = self.ALLOWED_DEV_POINTS * len(self.bag_times) percentage = (len(self.deviation) / len(self.bag_times)) * 100 if nb_dev_points < threshold: - print(f"Test {self.test_name} passed : {percentage:8.4f}% of datapoints had deviation larger than {self.EPSILON}m ({self.ALLOWED_DEV_POINTS * 100}% max for pass)") - return True + print(f"Test {self.test_name} passed : {percentage:.4f}% of datapoints had deviation larger than {self.EPSILON}m ({self.ALLOWED_DEV_POINTS * 100}% max for pass)") + return (True, percentage) else: print(f"Test {self.test_name} failed : The deviation between ideal and recorded trajectories is greater than {self.EPSILON}m for {percentage:8.4f}% of datapoints") - return False + return (False, percentage) - def find_temporal_offset(self) -> list : - ''' Returns a list containing the on-graph temporal offset between real and ideal trajectory. If offset is different for x and y axis, returns both in the same list''' - peak_x = self.bag_x.argmax() #find index of extremum value of real trajectory along x axis - peak_time_x = self.bag_times[peak_x] #find corresponding time - peak_x_ideal = self.ideal_traj_x.argmax() #find index of extremum value of ideal traj along x axis - peak_time_x_ideal = self.bag_times[peak_x_ideal] #find corresponding time - offset_x = peak_time_x_ideal - peak_time_x - - peak_y = self.bag_y.argmax() #find index of extremum value of real trajectory along y ayis - peak_time_y = self.bag_times[peak_y] #find corresponding time - peak_y_ideal = self.ideal_traj_y.argmax() #find index of extremum value of ideal traj along y ayis - peak_time_y_ideal = self.bag_times[peak_y_ideal] #find corresponding time - offset_y = peak_time_y_ideal - peak_time_y - - if offset_x == offset_y: - print(f"On-graph temporal offset is {offset_x}s, delay const is {self.DELAY_CONST_FIG8} so uncorrected/absolute offset is {offset_x-self.DELAY_CONST_FIG8}") - return [offset_x] - else : - print(f"On-graph temporal offsets are {offset_x} & {offset_y} secs, delay const is {self.DELAY_CONST_FIG8}") - return [offset_x, offset_y] if __name__=="__main__": - #command line utility + # command line utility from argparse import ArgumentParser, Namespace parser = ArgumentParser(description="Creates a pdf plotting the recorded trajectory of a drone against its desired trajectory") @@ -384,6 +365,4 @@ def find_temporal_offset(self) -> list : plotter.create_figures(args.desired_trajectory, args.recorded_trajectory, args.pdf, overwrite=args.overwrite) if args.open: import subprocess - subprocess.call(["xdg-open", args.pdf]) - - + subprocess.call(["xdg-open", args.pdf]) \ No newline at end of file diff --git a/systemtests/test_flights.py b/systemtests/test_flights.py index eb307cb49..619153df7 100644 --- a/systemtests/test_flights.py +++ b/systemtests/test_flights.py @@ -141,7 +141,7 @@ def record_start_and_clean(self, testname:str, max_wait:int): # src = f"source {str(self.ros2_ws)}/install/setup.bash" try: - command = f"{self.src} && ros2 bag record -s mcap -o test_{testname} /tf" + command = f"{self.src} && ros2 bag record -s mcap -o test_{testname} /tf /rosout" record_bag = Popen(command, shell=True, stderr=PIPE, stdout=True, text=True, cwd= self.ros2_ws / "results/", start_new_session=True, executable="/bin/bash") atexit.register(clean_process, record_bag) @@ -202,12 +202,11 @@ def test_figure8(self): test_passed = self.translate_plot_and_check("figure8") assert test_passed, "figure8 test failed : deviation larger than epsilon" - - def test_multi_trajectory(self): - self.test_file = "multi_trajectory_ideal_traj0.csv" - self.record_start_and_clean("multi_trajectory", 80) - test_passed = self.translate_plot_and_check("multi_trajectory") - assert test_passed, "multitrajectory test failed : deviation larger than epsilon" + # def test_multi_trajectory(self): + # self.test_file = "multi_trajectory_traj0_ideal.csv" + # self.record_start_and_clean("multi_trajectory", 80) + # test_passed = self.translate_plot_and_check("multi_trajectory") + # assert test_passed, "multitrajectory test failed : deviation larger than epsilon"