From 0b259d392497dc099ec8b0789916b35a49c80ee1 Mon Sep 17 00:00:00 2001 From: JulienThevenoz Date: Fri, 26 Jan 2024 15:23:03 +0100 Subject: [PATCH] created the full figure8 trajectory with takeoff and landing --- systemtests/figure8_ideal_traj.csv | 22 ++++++++++++++++++++++ systemtests/plotter_class.py | 7 ++++--- systemtests/test_flights.py | 12 ++++++------ 3 files changed, 32 insertions(+), 9 deletions(-) create mode 100644 systemtests/figure8_ideal_traj.csv diff --git a/systemtests/figure8_ideal_traj.csv b/systemtests/figure8_ideal_traj.csv new file mode 100644 index 000000000..5e98e3198 --- /dev/null +++ b/systemtests/figure8_ideal_traj.csv @@ -0,0 +1,22 @@ +duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, + + +####takeoff +2.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000001,0.000000,0.000000,0.232052,0.184839,0.030911,-0.176192,0.050572,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +####hover +3.65,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +####figure8 +1.050000,0.000000,-0.000000,0.000000,-0.000000,0.830443,-0.276140,-0.384219,0.180493,-0.000000,0.000000,-0.000000,0.000000,-1.356107,0.688430,0.587426,-0.329106,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.710000,0.396058,0.918033,0.128965,-0.773546,0.339704,0.034310,-0.026417,-0.030049,-0.445604,-0.684403,0.888433,1.493630,-1.361618,-0.139316,0.158875,0.095799,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.620000,0.922409,0.405715,-0.582968,-0.092188,-0.114670,0.101046,0.075834,-0.037926,-0.291165,0.967514,0.421451,-1.086348,0.545211,0.030109,-0.050046,-0.068177,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.700000,0.923174,-0.431533,-0.682975,0.177173,0.319468,-0.043852,-0.111269,0.023166,0.289869,0.724722,-0.512011,-0.209623,-0.218710,0.108797,0.128756,-0.055461,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.560000,0.405364,-0.834716,0.158939,0.288175,-0.373738,-0.054995,0.036090,0.078627,0.450742,-0.385534,-0.954089,0.128288,0.442620,0.055630,-0.060142,-0.076163,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.560000,0.001062,-0.646270,-0.012560,-0.324065,0.125327,0.119738,0.034567,-0.063130,0.001593,-1.031457,0.015159,0.820816,-0.152665,-0.130729,-0.045679,0.080444,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.700000,-0.402804,-0.820508,-0.132914,0.236278,0.235164,-0.053551,-0.088687,0.031253,-0.449354,-0.411507,0.902946,0.185335,-0.239125,-0.041696,0.016857,0.016709,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.620000,-0.921641,-0.464596,0.661875,0.286582,-0.228921,-0.051987,0.004669,0.038463,-0.292459,0.777682,0.565788,-0.432472,-0.060568,-0.082048,-0.009439,0.041158,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +0.710000,-0.923935,0.447832,0.627381,-0.259808,-0.042325,-0.032258,0.001420,0.005294,0.288570,0.873350,-0.515586,-0.730207,-0.026023,0.288755,0.215678,-0.148061,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +1.053185,-0.398611,0.850510,-0.144007,-0.485368,-0.079781,0.176330,0.234482,-0.153567,0.447039,-0.532729,-0.855023,0.878509,0.775168,-0.391051,-0.713519,0.391628,1.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, +####hover +2.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.0,0.000000,0.000000,0.0,0.0,0.0,0.0,0.0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +####landing +2.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000001,0.000000,0.000000,-0.232049,-0.184841,-0.030916,0.176196,-0.050573,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py index 4da400f47..20c291dce 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -23,9 +23,9 @@ 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.05 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test - 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 = 0 #4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. 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 = 5.45 #for an unknown reason, the delay constant with the sim_backend is slightly different + self.DELAY_CONST_FIG8 = 1.35 #5.45 #for an unknown reason, the delay constant with the sim_backend is slightly different 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 ? self.ALTITUDE_CONST_MULTITRAJ = 1 #takeoff altitude for traj0 in multi_trajectory.py self.X_OFFSET_CONST_MULTITRAJ = -0.3 #offest on the x axis between ideal and real trajectory. Reason: ideal trajectory (traj0.csv) starts with offset of 0.3m and CrazyflieServer.startTrajectory() is relative to start position @@ -105,7 +105,8 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): #special cases if fig8: - self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8 #special case: in fig8 no altitude is given in the trajectory polynomials (idealcsv) but is fixed as the takeoff altitude in figure8.py + # self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8 #special case: in fig8 no altitude is given in the trajectory polynomials (idealcsv) but is fixed as the takeoff altitude in figure8.py + pass elif m_t: self.ideal_traj_z[i] = pos[2] + self.ALTITUDE_CONST_MULTITRAJ #for multi_trajectory the altitude given in the trajectory polynomials is added to the fixed takeoff altitude specified in multi_trajectory.py self.ideal_traj_x[i] = pos[0] + self.X_OFFSET_CONST_MULTITRAJ #the x-axis is offset by 0.3 m because ideal start position not being (0,0,0) diff --git a/systemtests/test_flights.py b/systemtests/test_flights.py index c49345d8b..9bd5fdd71 100644 --- a/systemtests/test_flights.py +++ b/systemtests/test_flights.py @@ -163,18 +163,18 @@ def translate_plot_and_check(self, testname:str) -> bool : def test_figure8(self): - self.test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv" + self.test_file = "figure8_ideal_traj.csv" # run test self.record_start_and_clean("figure8", 20) #create the plot etc test_passed = self.translate_plot_and_check("figure8") test_passed, "figure8 test failed : deviation larger than epsilon" - def test_multi_trajectory(self): - self.test_file = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/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 = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/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"