Feature systemtests sim fixed timestamp

.github/workflows/systemtests_sim.yml

@@ -0,0 +1,74 @@
+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
+          python3 src/crazyswarm2/systemtests/test_flights.py --sim
+
+      - name: Upload files
+        id: step7
+        if: '!cancelled()'
+        uses: actions/upload-artifact@v3
+        with:
+          name: pdf_rosbags_and_logs
+          path: |
+            ros2_ws/results
+
+
+
+
+
+

crazyflie_sim/crazyflie_sim/visualization/rviz.py

@@ -29,7 +29,7 @@ def step(self, t, states: list[State], states_desired: list[State], actions: lis
             msg.transform.translation.x = state.pos[0]
             msg.transform.translation.y = state.pos[1]
             msg.transform.translation.z = state.pos[2]
-            msg.transform.rotation.x = state.quat[1]
+            msg.transform.rotation.x = state.quat[1]    
             msg.transform.rotation.y = state.quat[2]
             msg.transform.rotation.z = state.quat[3]
             msg.transform.rotation.w = state.quat[0]

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

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

systemtests/plotter_class.py

@@ -9,7 +9,7 @@
 
 class Plotter:
 
-    def __init__(self):
+    def __init__(self, sim_backend = False):
         self.params = {'experiment':'1','trajectory':'figure8.csv','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'}
         self.bag_times = np.empty([0])
         self.bag_x = np.empty([0])
@@ -21,8 +21,13 @@ def __init__(self):
         self.euclidian_dist = np.empty([0])
         self.deviation = [] #list of all indexes where euclidian_distance(ideal - recorded) > EPSILON
 
+        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. This could be implemented better later ?
+        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_MT = 0
+        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_MT = 5.8
         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
@@ -91,9 +96,15 @@ def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile):
 
         no_match_in_idealcsv=[]
 
+        delay = 0
+        if fig8:
+            delay = self.DELAY_CONST_FIG8
+        elif 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)
@@ -103,7 +114,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)
@@ -147,7 +159,6 @@ 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")
-
         #find the takeoff time and landing times
         ground_level = self.bag_z[0]
         airborne = False
@@ -160,13 +171,15 @@ def adjust_arrays(self):
                 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
+            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}")
                 break
             i+=1
 
+
+
         assert (takeoff_index != None) and (landing_index != None), "Plotter : couldn't find drone takeoff or landing"
 
 
@@ -184,10 +197,6 @@ def adjust_arrays(self):
 
         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)}")
 
 
@@ -196,6 +205,11 @@ def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str):
         '''Method that creates the pdf with the plots'''
 
         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():
@@ -223,7 +237,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))
@@ -333,6 +347,28 @@ def test_passed(self) -> bool :
             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")
             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__":
 
@@ -351,5 +387,5 @@ def test_passed(self) -> bool :
     if args.open:
         import subprocess
         subprocess.call(["xdg-open", args.pdf])
-        
-        
+
+