From 4260ba13458fe4bd77fcf27f2ba340ca43f5ccf6 Mon Sep 17 00:00:00 2001 From: julienthevenoz <125280576+julienthevenoz@users.noreply.github.com> Date: Fri, 2 Feb 2024 16:22:07 +0100 Subject: [PATCH] Add simulation system tests --- .github/workflows/systemtests_sim.yml | 75 +++++++++ crazyflie/config/motion_capture.yaml | 2 +- .../data/multi_trajectory/traj0.csv | 6 +- systemtests/figure8_ideal_traj.csv | 24 +++ systemtests/mcap_handler.py | 5 +- systemtests/multi_trajectory_traj0_ideal.csv | 52 +++++++ systemtests/plotter_class.py | 146 +++++++++++------- systemtests/test_flights.py | 62 ++++++-- 8 files changed, 294 insertions(+), 78 deletions(-) create mode 100644 .github/workflows/systemtests_sim.yml create mode 100644 systemtests/figure8_ideal_traj.csv create mode 100644 systemtests/multi_trajectory_traj0_ideal.csv diff --git a/.github/workflows/systemtests_sim.yml b/.github/workflows/systemtests_sim.yml new file mode 100644 index 000000000..e30cd8fc7 --- /dev/null +++ b/.github/workflows/systemtests_sim.yml @@ -0,0 +1,75 @@ +name: System Tests Simulation + +on: + push: + branches: [ "main", "feature-systemtests-sim-fixed-timestamp" ] + # manual trigger + workflow_dispatch: + +jobs: + build: + runs-on: self-hosted + steps: + - name: Build firmware + id: step1 + run: | + ls crazyflie-firmware || git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git + cd crazyflie-firmware + git pull + git submodule sync + git submodule update --init --recursive + make cf2_defconfig + make bindings_python + cd build + python3 setup.py install --user + - name: Create workspace + id: step2 + run: | + cd ros2_ws/src || mkdir -p ros2_ws/src + - name: Checkout motion capture package + id: step3 + run: | + cd ros2_ws/src + ls motion_capture_tracking || git clone --branch ros2 --recursive https://github.com/IMRCLab/motion_capture_tracking.git + cd motion_capture_tracking + git pull + git submodule sync + git submodule update --recursive --init + - name: Checkout Crazyswarm2 + id: step4 + uses: actions/checkout@v4 + with: + path: ros2_ws/src/crazyswarm2 + submodules: 'recursive' + - name: Build workspace + id: step5 + run: | + source /opt/ros/humble/setup.bash + cd ros2_ws + colcon build --symlink-install + + - name: Flight test + id: step6 + run: | + cd ros2_ws + source /opt/ros/humble/setup.bash + . install/local_setup.bash + export PYTHONPATH="${PYTHONPATH}:/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/crazyflie-firmware/build/" + export ROS_LOCALHOST_ONLY=1 + export ROS_DOMAIN_ID=99 + python3 src/crazyswarm2/systemtests/test_flights.py --sim -v #-v is verbose argument of unittest + + - name: Upload files + id: step7 + if: '!cancelled()' + uses: actions/upload-artifact@v3 + with: + name: pdf_rosbags_and_logs + path: | + ros2_ws/results + + + + + + diff --git a/crazyflie/config/motion_capture.yaml b/crazyflie/config/motion_capture.yaml index cf5e025c3..c9324ea41 100644 --- a/crazyflie/config/motion_capture.yaml +++ b/crazyflie/config/motion_capture.yaml @@ -1,7 +1,7 @@ /motion_capture_tracking: ros__parameters: type: "optitrack" - hostname: "130.149.82.37" + hostname: "141.23.110.143" mode: "libobjecttracker" # one of motionCapture,libRigidBodyTracker diff --git a/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv b/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv index 98c33752e..8a257c03e 100644 --- a/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv +++ b/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv @@ -1,5 +1,5 @@ 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 -0.960464,0.294342,0,0,0,-2.11233,4.6768,-3.73784,1.04989,0.172656,0,0,0,3.30457,-7.9007,6.65669,-1.94082,0.069979,0,0,0,-0.297773,0.623628,-0.493335,0.138654,0,0,0,0,0,0,0,0 +0.960464,0.0,0,0,0,-2.11233,4.6768,-3.73784,1.04989,0.0,0,0,0,3.30457,-7.9007,6.65669,-1.94082,0.0,0,0,0,-0.297773,0.623628,-0.493335,0.138654,0,0,0,0,0,0,0,0 1.57331,0.176622,-0.147985,0.0532986,0.0623229,0.343746,-0.500924,0.241338,-0.0403025,0.289553,0.0742267,-0.0522539,-0.0351683,-0.975361,1.50407,-0.788135,0.141456,0.043558,-0.0592408,-0.0401473,-0.00342225,-1.05145,1.64557,-0.878344,0.159958,0,0,0,0,0,0,0,0 1.68226,0.294342,0.17123,-0.0334701,-0.0192207,-0.0992382,0.118149,-0.0472082,0.00658999,0.084993,-0.0345305,0.0714366,0.00548031,0.800464,-1.18606,0.592827,-0.10086,-0.246232,0.00212717,0.0685892,0.0024846,1.23285,-1.76366,0.869779,-0.147011,0,0,0,0,0,0,0,0 1.92896,0.374489,-0.0338637,0.00252255,0.00888171,0.0111398,-0.0108929,0.00298726,-0.000251823,0.276975,-0.0784884,-0.0934159,0.000645144,-1.04135,1.33809,-0.584077,0.0868443,0.183524,0.0937667,-0.0541849,-0.00524925,-0.318377,0.393297,-0.166638,0.0242508,0,0,0,0,0,0,0,0 @@ -29,5 +29,5 @@ 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 1.27955,0.296893,-0.0207413,-0.0288722,0.000206756,-1.45292,2.73635,-1.78001,0.396694,0.363893,0.0205828,-0.0396241,-0.00426978,-2.18054,3.96089,-2.52441,0.555328,0.000432,0.116162,-0.0271769,-0.0121203,-0.0967398,0.0511672,0.0231312,-0.0137364,0,0,0,0,0,0,0,0 1.25924,0.129979,-0.0386626,0.0254433,0.00229348,0.314605,-0.568015,0.359101,-0.0786867,0.096373,-0.177655,-0.00188175,0.0162637,-1.43193,2.92354,-2.00733,0.465729,0.019741,-0.0837562,-0.0114419,0.0108961,-0.89746,1.80018,-1.22397,0.282268,0,0,0,0,0,0,0,0 1.35118,0.155486,0.0338914,-0.00270101,-0.0039661,0.202739,-0.374799,0.237823,-0.0512712,-0.106737,0.0769914,0.0704076,-0.0152089,1.54198,-2.90366,1.8396,-0.394692,-0.101778,0.0286494,0.0412961,-0.00329222,1.31264,-2.35215,1.45212,-0.306802,0,0,0,0,0,0,0,0 -1.14424,0.2,0.0015442,-0.00224162,0.00123466,-0.00661606,0.0134679,-0.00982511,0.00245085,0.1,-0.0953098,-0.0830381,0.0218392,-2.55729,5.66335,-4.2252,1.06822,0.1,0.0445589,-0.0317429,-0.000623191,-0.213517,0.427027,-0.293881,0.0698972,0,0,0,0,0,0,0,0 -1.06974,0.2,-7.36408e-05,0.000107209,-5.42089e-05,0.000468972,-0.00106323,0.000841569,-0.000226684,-0.1,0.0736875,0.104178,-0.0410587,3.38468,-7.9737,6.38949,-1.73737,0.1,-0.0104492,0.00871066,0.00133714,0.089598,-0.209747,0.164384,-0.043835,0,0,0,0,0,0,0,0 +1.14424,0.2,0.0015442,-0.00224162,0.00123466,-0.00661606,0.0134679,-0.00982511,0.00245085,0.0,-0.0953098,-0.0830381,0.0218392,-2.55729,5.66335,-4.2252,1.06822,0.1,0.0445589,-0.0317429,-0.000623191,-0.213517,0.427027,-0.293881,0.0698972,0,0,0,0,0,0,0,0 +1.06974,0.0,-7.36408e-05,0.000107209,-5.42089e-05,0.000468972,-0.00106323,0.000841569,-0.000226684,0.0,0.0736875,0.104178,-0.0410587,3.38468,-7.9737,6.38949,-1.73737,0.1,-0.0104492,0.00871066,0.00133714,0.089598,-0.209747,0.164384,-0.043835,0,0,0,0,0,0,0,0 diff --git a/systemtests/figure8_ideal_traj.csv b/systemtests/figure8_ideal_traj.csv new file mode 100644 index 000000000..5b91fc79f --- /dev/null +++ b/systemtests/figure8_ideal_traj.csv @@ -0,0 +1,24 @@ +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, + +#### wait on the ground +0.6,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.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 + +####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.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 +####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 +1.7,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/mcap_handler.py b/systemtests/mcap_handler.py index de15911c1..5e55a8784 100644 --- a/systemtests/mcap_handler.py +++ b/systemtests/mcap_handler.py @@ -41,7 +41,9 @@ def write_mcap_to_csv(self, inputbag:str, outputfile:str): f = open(outputfile, 'w+') writer = csv.writer(f) for topic, msg, timestamp in self.read_messages(inputbag): - writer.writerow([timestamp, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z]) + if topic =="/tf": + t = msg.transforms[0].header.stamp.sec + msg.transforms[0].header.stamp.nanosec *10**(-9) + 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: print(f"McapHandler : file {outputfile} not found") @@ -62,3 +64,4 @@ def write_mcap_to_csv(self, inputbag:str, outputfile:str): translator = McapHandler() translator.write_mcap_to_csv(args.inputbag,args.outputfile) + diff --git a/systemtests/multi_trajectory_traj0_ideal.csv b/systemtests/multi_trajectory_traj0_ideal.csv new file mode 100644 index 000000000..5cc458941 --- /dev/null +++ b/systemtests/multi_trajectory_traj0_ideal.csv @@ -0,0 +1,52 @@ +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 + +#wait before takeoff +0.5,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.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 + + +####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.5,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 + +####flying around +0.960464,0.0,0,0,0,-2.11233,4.6768,-3.73784,1.04989,0.0,0,0,0,3.30457,-7.9007,6.65669,-1.94082,1.0,0,0,0,-0.297773,0.623628,-0.493335,0.138654,0,0,0,0,0,0,0,0 +1.57331,0.176622,-0.147985,0.0532986,0.0623229,0.343746,-0.500924,0.241338,-0.0403025,0.289553,0.0742267,-0.0522539,-0.0351683,-0.975361,1.50407,-0.788135,0.141456,1.043558,-0.0592408,-0.0401473,-0.00342225,-1.05145,1.64557,-0.878344,0.159958,0,0,0,0,0,0,0,0 +1.68226,0.294342,0.17123,-0.0334701,-0.0192207,-0.0992382,0.118149,-0.0472082,0.00658999,0.084993,-0.0345305,0.0714366,0.00548031,0.800464,-1.18606,0.592827,-0.10086,0.753768,0.00212717,0.0685892,0.0024846,1.23285,-1.76366,0.869779,-0.147011,0,0,0,0,0,0,0,0 +1.92896,0.374489,-0.0338637,0.00252255,0.00888171,0.0111398,-0.0108929,0.00298726,-0.000251823,0.276975,-0.0784884,-0.0934159,0.000645144,-1.04135,1.33809,-0.584077,0.0868443,1.183524,0.0937667,-0.0541849,-0.00524925,-0.318377,0.393297,-0.166638,0.0242508,0,0,0,0,0,0,0,0 +2.03711,0.374489,0.0286391,-6.1852e-05,-0.00368623,0.00253097,-0.0050055,0.00272644,-0.000441277,-0.358361,0.0262046,0.110055,-0.00620508,0.544041,-0.672761,0.279349,-0.0393627,1.0461,-0.0452504,0.0303272,0.00292994,0.0964916,-0.116635,0.047237,-0.00652766,0,0,0,0,0,0,0,0 +1.08507,0.4,-0.00978883,-0.00352811,0.00421159,-0.147076,0.385891,-0.320935,0.0884478,0.1,-0.0261795,-0.0932209,0.0135473,-3.39392,7.64989,-5.90562,1.55714,1.1,0.0120733,-0.0160302,0.00109914,-0.170679,0.405338,-0.316775,0.0837347,0,0,0,0,0,0,0,0 +1.46341,0.4,0.0345966,0.0216228,0.00040938,0.113468,-0.193811,0.109932,-0.021215,-0.1,-0.00953811,0.0938259,0.00686414,1.78784,-3.00192,1.71647,-0.335021,1.1,0.0162005,0.0183118,0.001431,0.543281,-0.905734,0.51742,-0.101032,0,0,0,0,0,0,0,0 +2.19353,0.492638,0.0469231,-0.0199807,-0.000892996,-0.0338512,0.0362701,-0.0130911,0.00162474,0.203839,-0.0288206,-0.11848,-0.00554452,-0.630569,0.72139,-0.276928,0.0361384,1.209968,0.00986131,-0.0243065,-0.00119445,-0.0998581,0.115095,-0.0441976,0.00576199,0,0,0,0,0,0,0,0 +2.40126,0.48694,-0.00615544,0.0115191,0.000423929,0.00796213,-0.00925373,0.00321793,-0.000373305,-0.469909,0.0557579,0.149918,-0.00106415,0.538482,-0.564538,0.198219,-0.0236403,1.119622,0.0192151,0.028845,-0.000113746,0.115626,-0.12072,0.0423492,-0.00505007,0,0,0,0,0,0,0,0 +2.46094,0.515442,-0.000365976,-0.0122673,2.25527e-05,4.3861e-06,0.00181356,-0.000737567,8.47618e-05,0.463845,-0.0412104,-0.17329,0.000264977,-0.593703,0.60121,-0.20494,0.023781,1.330891,0.000348253,-0.0330759,0.000313802,-0.0979748,0.0999974,-0.0342066,0.00397451,0,0,0,0,0,0,0,0 +2.68427,0.48694,0.00486563,0.0132321,-0.00114426,0.0129524,-0.0144619,0.00482814,-0.000528691,-0.71593,-0.0408684,0.184692,0.00619732,0.442694,-0.405637,0.125531,-0.0132641,1.142929,-0.00451953,0.0270264,-0.00218942,0.0121858,-0.0179685,0.00647746,-0.000732907,0,0,0,0,0,0,0,0 +1.92558,0.505486,-0.0481952,-0.0204291,0.00284946,-0.0470553,0.0773259,-0.0377233,0.00598077,0.718761,0.267399,-0.141554,-0.0117975,-0.800345,0.963882,-0.403221,0.0582575,1.098904,-0.131338,-0.0396354,0.00782912,-0.4965,0.661823,-0.295209,0.0445271,0,0,0,0,0,0,0,0 +2.0699,0.421491,0.0186218,0.0156054,-0.00129697,0.07458,-0.0953844,0.0399616,-0.00562765,0.302142,-0.266958,0.0381168,0.0177516,-0.0552536,0.0911841,-0.0450511,0.00702141,0.771628,0.115857,0.0879918,-0.00554767,0.814447,-0.973674,0.396022,-0.0548837,0,0,0,0,0,0,0,0 +1.9596,0.48694,-0.0304046,-0.0213345,0.00170875,-0.231304,0.295149,-0.127553,0.0187408,0.120541,0.123032,0.0122544,-0.012462,0.149347,-0.218046,0.101473,-0.0155924,1.505486,0.00192474,-0.107286,0.000790515,-1.01863,1.26183,-0.536142,0.0779269,0,0,0,0,0,0,0,0 +1.70183,0.333009,0.0183142,0.028215,-0.00185434,0.43896,-0.63547,0.3139,-0.0528803,0.232223,-0.140993,-0.0449547,0.00936592,-1.12957,1.6455,-0.81818,0.138527,0.832615,-0.101997,0.0790866,0.00096767,0.497639,-0.719611,0.351991,-0.0587304,0,0,0,0,0,0,0,0 +1.34472,0.48694,-0.00625689,-0.0317257,0.000382684,-1.03393,1.83762,-1.13117,0.238966,-0.226478,0.0101228,0.0610516,-0.00764983,1.25229,-2.30188,1.44081,-0.307367,0.917626,-0.033286,-0.0550669,0.00313228,-1.52482,2.7969,-1.74891,0.372958,0,0,0,0,0,0,0,0 +2.1222,0.333009,-0.0545061,0.0199991,0.00480515,0.107972,-0.116334,0.0436145,-0.00566624,-0.072397,-0.0307885,-0.0689067,-0.00236443,-0.48434,0.57055,-0.226496,0.0305878,0.717497,0.028035,0.0816772,0.00612047,0.73381,-0.837942,0.327938,-0.0439369,0,0,0,0,0,0,0,0 +2.22722,0.421491,0.0660872,-0.00287208,-0.00340824,0.062709,-0.0713995,0.0277343,-0.00364623,-0.495874,0.0534612,0.0981472,0.00118253,0.675836,-0.73877,0.276221,-0.03532,1.457563,0.131649,-0.0799577,-0.00792167,-0.366161,0.385528,-0.140404,0.0176255,0,0,0,0,0,0,0,0 +1.85734,0.540865,-0.00634081,-0.00724899,0.00147892,-0.0808568,0.10675,-0.0483186,0.00746342,0.379252,0.132248,-0.0957692,-0.00873243,-1.07307,1.34465,-0.588112,0.0887744,1.038957,-0.200084,0.0246642,0.00970083,-0.25658,0.36452,-0.173423,0.0277121,0,0,0,0,0,0,0,0 +1.69138,0.496281,-0.00160133,0.00618968,0.000136705,0.124512,-0.174729,0.0851269,-0.0142547,-0.1853,-0.289684,0.024136,0.0175717,-0.384504,0.663858,-0.362455,0.0651215,0.81169,0.0702784,0.0295687,-0.00247486,1.21881,-1.70715,0.833757,-0.139995,0,0,0,0,0,0,0,0 +2.19933,0.540865,0.0144117,-0.00391837,-0.00119932,-0.0275664,0.0268387,-0.00940817,0.0011576,-0.385893,0.235345,0.0702187,-0.0148152,0.588274,-0.67496,0.261551,-0.0343838,1.323729,0.185085,-0.00841054,-0.00925475,0.039381,-0.0748132,0.0351087,-0.00510156,0,0,0,0,0,0,0,0 +2.68201,0.500322,-0.0412104,-0.00763103,0.000713901,-0.0520821,0.0470691,-0.0146416,0.00155911,0.387882,-0.0842107,-0.121138,0.00454433,-0.324702,0.302031,-0.0946549,0.0100916,1.366876,-0.264264,-0.0765883,0.0107036,-0.298247,0.281606,-0.0893991,0.00961875,0,0,0,0,0,0,0,0 +2.21932,0.292621,-0.0377479,0.0107761,0.00185615,0.0523879,-0.0498758,0.0171324,-0.002072,-0.664787,-0.0756608,0.114634,0.000204595,0.482644,-0.530616,0.198653,-0.0254146,0.289226,0.0179677,0.111474,-0.00732309,0.507412,-0.56779,0.21534,-0.0277882,0,0,0,0,0,0,0,0 +0.826943,0.365479,0.0796986,0.011073,-0.00345361,6.10559,-17.7498,17.8943,-6.18313,-0.128196,0.0924843,-0.0686475,0.000538459,-0.967377,2.3937,-2.12002,0.661123,0.899606,0.03983,-0.0650002,0.0145722,-1.19604,3.65303,-3.72378,1.2885,0,0,0,0,0,0,0,0 +1.70943,0.515442,0.0778242,-0.0130839,-0.00452718,-0.0331128,0.0135171,0.00336571,-0.00165812,-0.128196,-0.0503162,-0.00225906,0.0131073,0.046757,-0.0481522,0.016617,-0.00196654,0.899606,0.0424298,0.0647269,0.0125499,1.45743,-2.08038,1.01596,-0.169609,0,0,0,0,0,0,0,0 +2.0764,0.515442,-0.0859543,-0.0188315,0.00474201,-0.258721,0.311102,-0.127276,0.0176996,-0.128196,0.0471026,0.00501662,-0.00187713,0.203127,-0.228639,0.0905749,-0.0123697,1.418091,0.0269611,-0.0986299,-0.00460336,-0.709972,0.831305,-0.332664,0.0455369,0,0,0,0,0,0,0,0 +1.34033,0.241704,0.00169625,0.0267376,-0.0040746,0.496448,-0.922,0.581167,-0.124661,0.125962,0.103337,0.00817997,-0.00308402,1.45034,-2.64983,1.66366,-0.356691,0.813306,-0.0951879,0.0808931,0.00400333,1.56751,-2.73421,1.6614,-0.348002,0,0,0,0,0,0,0,0 +1.27955,0.296893,-0.0207413,-0.0288722,0.000206756,-1.45292,2.73635,-1.78001,0.396694,0.363893,0.0205828,-0.0396241,-0.00426978,-2.18054,3.96089,-2.52441,0.555328,1.000432,0.116162,-0.0271769,-0.0121203,-0.0967398,0.0511672,0.0231312,-0.0137364,0,0,0,0,0,0,0,0 +1.25924,0.129979,-0.0386626,0.0254433,0.00229348,0.314605,-0.568015,0.359101,-0.0786867,0.096373,-0.177655,-0.00188175,0.0162637,-1.43193,2.92354,-2.00733,0.465729,1.019741,-0.0837562,-0.0114419,0.0108961,-0.89746,1.80018,-1.22397,0.282268,0,0,0,0,0,0,0,0 +1.35118,0.155486,0.0338914,-0.00270101,-0.0039661,0.202739,-0.374799,0.237823,-0.0512712,-0.106737,0.0769914,0.0704076,-0.0152089,1.54198,-2.90366,1.8396,-0.394692,0.898222,0.0286494,0.0412961,-0.00329222,1.31264,-2.35215,1.45212,-0.306802,0,0,0,0,0,0,0,0 +1.14424,0.2,0.0015442,-0.00224162,0.00123466,-0.00661606,0.0134679,-0.00982511,0.00245085,0.0,-0.0953098,-0.0830381,0.0218392,-2.55729,5.66335,-4.2252,1.06822,1.1,0.0445589,-0.0317429,-0.000623191,-0.213517,0.427027,-0.293881,0.0698972,0,0,0,0,0,0,0,0 +1.06974,0.0,-7.36408e-05,0.000107209,-5.42089e-05,0.000468972,-0.00106323,0.000841569,-0.000226684,0.0,0.0736875,0.104178,-0.0410587,3.38468,-7.9737,6.38949,-1.73737,1.1,-0.0104492,0.00871066,0.00133714,0.089598,-0.209747,0.164384,-0.043835,0,0,0,0,0,0,0,0 + +####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 5655aedb6..4b34d461c 100644 --- a/systemtests/plotter_class.py +++ b/systemtests/plotter_class.py @@ -9,8 +9,8 @@ class Plotter: - def __init__(self): - self.params = {'experiment':'1','trajectory':'figure8.csv','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'} + def __init__(self, sim_backend = False): + self.params = {'experiment':'1','trajectory':'','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'} self.bag_times = np.empty([0]) self.bag_x = np.empty([0]) self.bag_y = np.empty([0]) @@ -20,13 +20,16 @@ def __init__(self): self.ideal_traj_z = np.empty([0]) self.euclidian_dist = np.empty([0]) self.deviation = [] #list of all indexes where euclidian_distance(ideal - recorded) > EPSILON - - 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. This could be implemented better later ? + 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.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. + 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.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 - + def file_guard(self, pdf_path): msg = None if os.path.exists(pdf_path): @@ -47,22 +50,11 @@ def file_guard(self, pdf_path): def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): '''Method that reads the csv data of the ideal test trajectory and of the actual recorded trajectory and initializes the attribute arrays''' - #check which test we are plotting : figure8 or multi_trajectory or another one - if("figure8" in rosbag_csvfile): - fig8, m_t = True, False - print("Plotting fig8 test data") - elif "multi_trajectory" in rosbag_csvfile: - fig8, m_t = False, True - print("Plotting multi_trajectory test data") - else: - fig8, m_t = False, False - print("Plotting unspecified test data") - + #get ideal trajectory data self.ideal_traj_csv = Trajectory() try: path_to_ideal_csv = os.path.join(os.path.dirname(os.path.abspath(__file__)),ideal_csvfile) - print(path_to_ideal_csv) self.ideal_traj_csv.loadcsv(path_to_ideal_csv) except FileNotFoundError: print("Plotter : File not found " + path_to_ideal_csv) @@ -91,9 +83,15 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): no_match_in_idealcsv=[] + delay = 0 + if self.test_name == "fig8": + delay = self.DELAY_CONST_FIG8 + elif self.test_name == "m_t": + delay = self.DELAY_CONST_MT + for i in range(bag_arrays_size): try: - pos = self.ideal_traj_csv.eval(self.bag_times[i] - self.DELAY_CONST_FIG8).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) @@ -101,14 +99,6 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): self.ideal_traj_x[i], self.ideal_traj_y[i], self.ideal_traj_z[i]= pos[0], pos[1], pos[2] - #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 - 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) - - self.euclidian_dist[i] = np.linalg.norm([self.ideal_traj_x[i]-self.bag_x[i], self.ideal_traj_y[i]-self.bag_y[i], self.ideal_traj_z[i]-self.bag_z[i]]) if self.euclidian_dist[i] > self.EPSILON: @@ -146,8 +136,7 @@ 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]''' - print(f"rosbag initial length {(self.bag_times[-1]-self.bag_times[0]) * 10**-9}s") - + 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 @@ -157,16 +146,16 @@ def adjust_arrays(self): for z_coord in self.bag_z: if not(airborne) and z_coord > ground_level + ground_level*(0.1): #when altitude of the drone is 10% higher than the ground level, it started takeoff takeoff_index = i - takeoff_time = self.bag_times[takeoff_index] airborne = True - print(f"takeof time is {(takeoff_time-self.bag_times[0]) * 10**-9}") - if airborne and z_coord < ground_level + ground_level*(0.1): #find when it lands again + print(f"takeoff time is {self.bag_times[takeoff_index]}s") + if airborne and z_coord <= ground_level + ground_level*(0.1): #find when it lands again landing_index = i - landing_time = self.bag_times[landing_index] - print(f"landing time is {(landing_time-self.bag_times[0]) * 10**-9}") + 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" @@ -176,26 +165,44 @@ def adjust_arrays(self): 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) + # #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) 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" - #rewrite bag_times to start at 0 and be written in [s] instead of [ns] - bag_start_time = self.bag_times[0] - self.bag_times = (self.bag_times-bag_start_time) * (10**-9) - assert self.bag_times[0] == 0 print(f"trimmed bag_times starts: {self.bag_times[0]}s and ends: {self.bag_times[-1]}, size: {len(self.bag_times)}") - def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): + def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str, overwrite=False): '''Method that creates the pdf with the plots''' + #check which test we are plotting : figure8 or multi_trajectory or another one + if("figure8" in rosbag_csvfile): + self.test_name = "fig8" + self.params["trajectory"] = "figure8" + print("Plotting fig8 test data") + elif "multi_trajectory" in rosbag_csvfile: + self.test_name = "mt" + self.params["trajectory"] = "multi_trajectory" + self.EPSILON *= 2 #multi_trajectory test has way more difficulties + self.ALLOWED_DEV_POINTS *= 2 + print("Plotting multi_trajectory test data") + else: + self.test_name = "undefined" + self.params["trajectory"] = "undefined" + print("Plotting unspecified test data") + + 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(): @@ -206,7 +213,8 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): pdfname= pdfname + '.pdf' #check if user wants to overwrite the report file - self.file_guard(pdfname) + if not overwrite : + self.file_guard(pdfname) pdf_pages = PdfPages(pdfname) #create title page @@ -223,7 +231,7 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): 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' + f'max error : ' + title_text_results = f'Results: test {passed}\n' + offset_string + f'max error : ' title_text = text + "\n" + title_text_settings + "\n" + title_text_parameters + "\n" + title_text_results fig = plt.figure(figsize=(5,8)) @@ -321,18 +329,41 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): print("Results saved in " + pdfname) def test_passed(self) -> bool : - '''Returns True if the deviation between recorded and ideal trajectories of the drone always stayed lower - than EPSILON. Returns False otherwise ''' + '''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 ''' 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 == 0: - print("Test passed") + 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 else: - print(f"The deviation between ideal and recorded trajectories is greater than {self.EPSILON}m for {nb_dev_points} " - f"datapoints, which corresponds to a duration of {nb_dev_points*0.01}s") + 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 + + 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__": @@ -344,12 +375,13 @@ def test_passed(self) -> bool : parser.add_argument("recorded_trajectory", type=str, help=".csv file containing (time,x,y,z) of the recorded drone trajectory") parser.add_argument("pdf", type=str, help="name of the pdf file you want to create/overwrite") parser.add_argument("--open", help="Open the pdf directly after it is created", action="store_true") + parser.add_argument("--overwrite", action="store_true", help="If the given pdf already exists, overwrites it without asking") args : Namespace = parser.parse_args() plotter = Plotter() - plotter.create_figures(args.desired_trajectory, args.recorded_trajectory, args.pdf) + 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]) - - + + diff --git a/systemtests/test_flights.py b/systemtests/test_flights.py index cfd84f1c1..8f7c20d10 100644 --- a/systemtests/test_flights.py +++ b/systemtests/test_flights.py @@ -9,6 +9,7 @@ import time import signal import atexit +from argparse import ArgumentParser, Namespace ############################# @@ -30,7 +31,7 @@ def clean_process(process:Popen) -> int : '''Kills process and its children on exit if they aren't already terminated (called with atexit). Returns 0 on termination, 1 if SIGKILL was needed''' if process.poll() == None: group_id = os.getpgid(process.pid) - print(f"cleaning process {group_id}") + #print(f"cleaning process {group_id}") os.killpg(group_id, signal.SIGTERM) time.sleep(0.01) #necessary delay before first poll i=0 @@ -44,9 +45,21 @@ def clean_process(process:Popen) -> int : return 0 else: return 0 #process already terminated + +def print_PIPE(process : Popen, process_name, always=False): + '''If process creates some error, prints the stderr and stdout PIPE of the process. NB : stderr and stdout must = PIPE in the Popen constructor''' + if process.returncode != 0 or always: + out, err = process.communicate() + print(f"{process_name} returncode = {process.returncode}") + print(f"{process_name} stderr : {err}") + print(f"{process_name} stdout : {out}") + else: + print(f"{process_name} completed sucessfully") + class TestFlights(unittest.TestCase): + SIM = False def __init__(self, methodName: str = "runTest") -> None: super().__init__(methodName) @@ -65,17 +78,21 @@ def setUp(self): self.test_file = None # launch server + current_env = None src = "source " + str(Path(__file__).parents[3] / "install/setup.bash") # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" command = f"{src} && ros2 launch crazyflie launch.py" - self.launch_crazyswarm = Popen(command, shell=True, stderr=True, stdout=PIPE, text=True, - start_new_session=True, executable="/bin/bash") + if TestFlights.SIM : + command += " backend:=sim" #launch crazyswarm from simulation backend + current_env = os.environ.copy() + self.launch_crazyswarm = Popen(command, shell=True, stderr=PIPE, stdout=PIPE, text=True, + start_new_session=True, executable="/bin/bash", env=current_env) atexit.register(clean_process, self.launch_crazyswarm) #atexit helps us to make sure processes are cleaned even if script exits unexpectedly time.sleep(1) - # runs once per test_ function def tearDown(self) -> None: clean_process(self.launch_crazyswarm) #kill crazyswarm_server and all of its child processes + print_PIPE(self.launch_crazyswarm, "launch_crazyswarm") # copy .ros/log files to results folder if Path(Path.home() / ".ros/log").exists(): @@ -92,19 +109,26 @@ def record_start_and_clean(self, testname:str, max_wait:int): src = f"source {str(self.ros2_ws)}/install/setup.bash" try: - command = f"{src} && ros2 bag record -s mcap -o test_{testname} /tf" + command = f"{src} && ros2 bag record -s mcap -o test_{testname} /tf" 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) command = f"{src} && ros2 run crazyflie_examples {testname}" + if TestFlights.SIM: + command += " --ros-args -p use_sim_time:=True" #necessary args to start the test in simulation start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, start_new_session=True, text=True, executable="/bin/bash") atexit.register(clean_process, start_flight_test) - start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself + if TestFlights.SIM : + start_flight_test.wait(timeout=max_wait*5) #simulation can be super slow + else : + start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself + clean_process(start_flight_test) clean_process(record_bag) + print("finished the test") except TimeoutExpired: #if max_wait is exceeded clean_process(start_flight_test) @@ -113,14 +137,12 @@ def record_start_and_clean(self, testname:str, max_wait:int): except KeyboardInterrupt: #if drone crashes, user can ^C to skip the waiting clean_process(start_flight_test) clean_process(record_bag) - + #if something went wrong with the bash command lines in Popen, print the error - if record_bag.stderr != None: - print(testname," record_bag stderr: ", record_bag.stderr.readlines()) - if start_flight_test.stderr != None: - print(testname," start_flight flight stderr: ", start_flight_test.stderr.readlines()) - + print_PIPE(record_bag, f"record_bag for {self.idFolderName()}") + print_PIPE(start_flight_test, f"start_flight_test for {self.idFolderName()}") + def translate_plot_and_check(self, testname:str) -> bool : '''Translates rosbag .mcap format to .csv, then uses that csv to plot a pdf. Checks the deviation between ideal and real trajectories, i.e if the drone successfully followed its given trajectory. Returns True if deviation < epsilon(defined in plotter_class.py) at every timestep, false if not. ''' @@ -134,14 +156,14 @@ def translate_plot_and_check(self, testname:str) -> bool : output_pdf = f"{str(self.ros2_ws)}/results/test_{testname}/results_{testname}.pdf" rosbag_csv = output_csv - plotter = Plotter() + plotter = Plotter(sim_backend=TestFlights.SIM) plotter.create_figures(self.test_file, rosbag_csv, output_pdf) #plot the data return plotter.test_passed() 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 @@ -149,7 +171,7 @@ def test_figure8(self): assert 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.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" @@ -158,5 +180,13 @@ def test_multi_trajectory(self): if __name__ == '__main__': - unittest.main() + from argparse import ArgumentParser + import sys + parser = ArgumentParser(description="Runs (real or simulated) flight tests with pytest framework") + parser.add_argument("--sim", action="store_true", help="Runs the test from the simulation backend") + args, other_args = parser.parse_known_args() + if args.sim : + TestFlights.SIM = True + + unittest.main(argv=[sys.argv[0]] + other_args)