Add simulation system tests

IMRCLab · Feb 2, 2024 · 4260ba1 · 4260ba1
1 parent f861618
commit 4260ba1
Show file tree

Hide file tree

Showing 8 changed files with 294 additions and 78 deletions.
diff --git a/.github/workflows/systemtests_sim.yml 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
@@ -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
@@ -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
@@ -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
@@ -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)
+