diff --git a/.github/workflows/systemtests.yml b/.github/workflows/systemtests.yml
index 8dbd8b71c..1b9358199 100644
--- a/.github/workflows/systemtests.yml
+++ b/.github/workflows/systemtests.yml
@@ -30,6 +30,10 @@ jobs:
           cd ros2_ws
           colcon build --symlink-install
 
+      - name: Flight test
+        run: |
+          python3 src/crazyswarm2/systemtests/newsub.py
+
       # - name: build and test ROS 2
       #   uses: ros-tooling/action-ros-ci@v0.3
       #   with:
diff --git a/systemtests/mcap_read_write.py b/systemtests/mcap_read_write.py
new file mode 100644
index 000000000..9b38caa00
--- /dev/null
+++ b/systemtests/mcap_read_write.py
@@ -0,0 +1,53 @@
+from pathlib import Path
+from rclpy.serialization import deserialize_message
+from rosidl_runtime_py.utilities import get_message
+from std_msgs.msg import String
+import rosbag2_py
+import csv
+
+class McapHandler:
+    def __init__(self):
+        pass
+
+    def read_messages(self, input_bag: str):
+        reader = rosbag2_py.SequentialReader()
+        reader.open(
+            rosbag2_py.StorageOptions(uri=input_bag, storage_id="mcap"),
+            rosbag2_py.ConverterOptions(
+                input_serialization_format="cdr", output_serialization_format="cdr"
+            ),
+        )
+        topic_types = reader.get_all_topics_and_types()
+
+        def typename(topic_name):
+            for topic_type in topic_types:
+                if topic_type.name == topic_name:
+                    return topic_type.type
+            raise ValueError(f"topic {topic_name} not in bag")
+
+        while reader.has_next():
+            topic, data, timestamp = reader.read_next()
+            msg_type = get_message(typename(topic))
+            msg = deserialize_message(data, msg_type)
+            yield topic, msg, timestamp
+        del reader
+    
+    def write_mcap_to_csv(self, inputbag:str, outputfile:str):
+        try:
+            print("Translating .mcap to .csv")
+            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])
+            f.close()
+        except FileNotFoundError:
+            print(f"McapHandler : file {outputfile} not found")
+            exit(1)
+
+
+
+
+if __name__ == "__main__":
+    yo =  McapHandler()
+    # # yo.write_mcap_to_csv('Home/julien/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.mcap','Home/julien/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.csv')
+    yo.write_mcap_to_csv('/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.mcap','/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.csv')
diff --git a/systemtests/newsub.py b/systemtests/newsub.py
new file mode 100755
index 000000000..c8103664f
--- /dev/null
+++ b/systemtests/newsub.py
@@ -0,0 +1,52 @@
+#!/usr/bin/env python3   
+from subprocess import Popen
+import time
+import os
+import signal
+from mcap_read_write import McapHandler
+from datetime import datetime
+from pathlib import Path
+from plotter_class import Plotter
+
+
+if __name__ == "__main__":
+
+    command = "source install/setup.bash && ros2 launch crazyflie launch.py"
+    launch_crazyswarm = Popen(command, shell=True, stderr=True, 
+                            stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)   
+    time.sleep(3)
+    bagname = 'bag_' + datetime.now().strftime('%d_%m_%Y-%H:%M:%S')
+    command = f"mkdir bagfiles && cd bagfiles && ros2 bag record -s mcap -o {bagname} /tf"
+    start_rosbag =  Popen(command, shell=True, stderr=True, 
+                            stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) 
+    command = "ros2 run crazyflie_examples figure8"
+    start_test = Popen(command, shell=True, stderr=True, 
+                       stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)    
+    time.sleep(30)
+    os.killpg(os.getpgid(start_test.pid), signal.SIGTERM)  #kill start_test process and all of its child processes
+    os.killpg(os.getpgid(start_rosbag.pid), signal.SIGTERM) #kill rosbag
+    os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM)   #kill crazyswarm
+
+    print("Flight finished")
+
+    #test done, now we compare the ideal and real trajectories
+
+    #create a file that translates the .mcap format of the rosbag to .csv
+    writer = McapHandler()
+    #path = str(Path(__file__).parent /"bags/tfbag.mcap")
+
+    ### attention le bagname est different du bagfile par un _0 
+    inputbag = "bagfiles/" + bagname + '/' + bagname + '_0' + '.mcap'
+    output_csv = "bagfiles/" + bagname + '/' + bagname + '_0' + '.csv'
+
+    writer.write_mcap_to_csv(inputbag, output_csv)
+
+    output_pdf = "bagfiles/" + bagname + '/' + 'Results_'+ datetime.now().strftime('%d_%m_%Y-%H:%M:%S')
+
+    rosbag_csv = output_csv
+
+    test_file = "src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
+    paul = Plotter()
+    paul.create_figures(test_file, rosbag_csv, output_pdf)
+
+    exit()
diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py
new file mode 100644
index 000000000..184bd621e
--- /dev/null
+++ b/systemtests/plotter_class.py
@@ -0,0 +1,358 @@
+
+# import cfusdlog
+import matplotlib.pyplot as plt
+import os
+import sys 
+import yaml
+from matplotlib.backends.backend_pdf import PdfPages
+import numpy as np
+
+# import model  #dennis module
+
+#my stuff
+from crazyflie_py.uav_trajectory import Trajectory
+from pathlib import Path
+import pandas as pd
+from mcap_read_write import McapHandler
+from mpl_toolkits.mplot3d import Axes3D
+
+class Plotter:
+
+    def __init__(self):
+        self.output_dir = ''
+        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])
+        self.bag_y = np.empty([0])
+        self.bag_z = np.empty([0])
+        self.ideal_traj_x = np.empty([0])
+        self.ideal_traj_y = np.empty([0])
+        self.ideal_traj_z = np.empty([0])
+        self.euclidian_dist = np.empty([0])
+
+        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 should be implemented better later
+        self.ALTITUDE_CONST_FIG8 = 0.5 #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
+
+    def file_guard(self, pdf_path):
+        msg = None
+        if os.path.exists(pdf_path):
+            msg = input("file already exists, overwrite? [y/n]: ")
+            if msg == "n":
+                print("exiting...")
+                sys.exit(0)
+            elif msg == "y":
+                print("overwriting...")
+                os.remove(pdf_path)
+            else:
+                print("invalid msg...")
+                self.file_guard(pdf_path)
+        return
+    
+
+
+    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
+
+        if("fig8" in rosbag_csvfile):
+            fig8 = True
+            print("Plotting fig8 test data")
+        else:
+            fig8 = False
+            print("Plotting multi_trajectory test data")
+
+        #get ideal trajectory data
+        self.ideal_traj_csv = Trajectory()
+        try:
+            self.ideal_traj_csv.loadcsv(Path(__file__).parent / ideal_csvfile,)
+        except FileNotFoundError:
+            print("Plotter : File not found " + str(Path(__file__).parent / ideal_csvfile,))
+            exit(1)
+
+
+        #get rosbag data
+        rosbag_data = pd.read_csv(rosbag_csvfile, names=['time','x','y','z'])
+        
+
+        #we have to split the pandas dataframe into separate numpy arrays to plot them later
+        self.bag_times = rosbag_data['time'].to_numpy()
+        self.bag_x = rosbag_data['x'].to_numpy()
+        self.bag_y = rosbag_data['y'].to_numpy()
+        self.bag_z = rosbag_data['z'].to_numpy()
+    
+        self.adjust_arrays()
+        bag_arrays_size = len(self.bag_times)
+        print("number of datapoints in self.bag_*: ",bag_arrays_size)
+
+        #####calculate ideal trajectory points corresponding to the times of recorded points 
+
+        self.ideal_traj_x = np.empty([bag_arrays_size])
+        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=[]
+
+        for i in range(bag_arrays_size):  
+            try:
+                pos = self.ideal_traj_csv.eval(self.bag_times[i] - self.DELAY_CONST_FIG8).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]= pos[0], pos[1]
+            if(fig8) :                                                    #special case : in fig8 test no altitude is given in the trajectory polynomials but is stays constant after takeoff in figure8.py
+                self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8
+            else :                                                        #for multi_trajectory test the altitude given in the trajectory polynomials is added to the takeoff altitude
+                self.ideal_traj_z[i] = self.ALTITUDE_CONST_MULTITRAJ + pos[2]
+
+            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]])
+            
+        self.no_match_warning(no_match_in_idealcsv)
+
+
+
+    def no_match_warning(self, unmatched_values:list):
+        ''' A method which prints a warning saying how many (if any) recorded datapoints could not be matched to an ideal datapoint'''
+
+        no_match_arr = np.array(unmatched_values)
+
+        if no_match_arr.size == 0:
+            return
+        
+        split_index_arr = []
+
+        for i in range(no_match_arr.size - 1):                    #find indexes which are not consecutive
+            if(no_match_arr[i+1] != no_match_arr[i]+1):
+                split_index_arr.append(i+1)
+
+        banana_split = np.split(no_match_arr, split_index_arr)     #split array into sub-array of consecutive indexes -> each sub-array is a timerange for which ideal positions weren't able to calculated
+        print(f"{len(no_match_arr)} recorded positions weren't able to be matched with a specified ideal position and were given the default (0,0,0) ideal position instead.")
+        print("Probable reason : their timestamp is before the start of the ideal trajectory or after its end.")
+        if len(banana_split)==2:
+            timerange1_start = self.bag_times[banana_split[0][0]]
+            timerange1_end= self.bag_times[banana_split[0][1]]
+            timerange2_start = self.bag_times[banana_split[1][0]]
+            timerange2_end = self.bag_times[banana_split[1][1]]
+            print(f"These datapoints correspond to the time ranges [{timerange1_start} , {timerange1_end}] and [{timerange2_start} , {timerange2_end}]")
+
+
+
+    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
+        takeoff_index = None
+        landing_index = None
+        i=0
+        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
+                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), "couldn't find drone takeoff or landing"
+
+
+        ####get rid of datapoints before takeoff and after landing in bag_times, bag_x, bag_y, bag_y   
+
+        assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "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)
+
+        assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "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_and_path:str):
+        '''Method that creates the pdf with the plots'''
+
+        self.read_csv_and_set_arrays(ideal_csvfile,rosbag_csvfile)
+        
+        #create PDF to save figures
+        pdf_path = self.output_dir + pdfname_and_path + '.pdf'
+
+        #check if user wants to overwrite the report file
+        # self.file_guard(pdf_path)
+        pdf_pages = PdfPages(pdf_path)
+
+        #create title page
+        text = 'figure8 trajectory test'
+        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:\n' + f'max error : '
+
+        title_text = text + "\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)
+        pdf_pages.savefig(fig)
+    
+       
+        #plt.style.use('ggplot')
+   
+         #create plots
+        fig, ax = plt.subplots()
+        ax.plot(self.bag_times, self.ideal_traj_x, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10)
+        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.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()
+        
+        pdf_pages.savefig(fig) 
+          
+        fig2, ax2 = plt.subplots()
+        ax2.plot(self.bag_times, self.ideal_traj_y, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10)
+        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.grid(which='major', color='#DDDDDD', linewidth=0.8)
+        ax2.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5)
+        ax2.minorticks_on()
+        fig2.tight_layout(pad = 4) 
+        fig2.legend()
+        pdf_pages.savefig(fig2) 
+          
+
+        fig3, ax3 = plt.subplots()
+        ax3.plot(self.bag_times, self.ideal_traj_z, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10)
+        ax3.plot(self.bag_times, self.bag_z, label='Recorded trajectory')
+        ax3.set_xlabel('time [s]')
+        ax3.set_ylabel('z position [m]')   
+        ax3.set_title("Trajectory z")
+        ax3.grid(which='major', color='#DDDDDD', linewidth=0.8)
+        ax3.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5)
+        ax3.minorticks_on()
+        fig3.tight_layout(pad = 4)
+        fig3.legend()
+        pdf_pages.savefig(fig3) 
+ 
+        fig4, ax4 = plt.subplots()
+        ax4.plot(self.bag_times, self.euclidian_dist)
+        ax4.set_xlabel('time [s]')
+        ax4.set_ylabel('Euclidean distance [m]')
+        ax4.set_title('Deviation between ideal and recorded trajectories')
+        fig4.tight_layout(pad=4)
+        ax4.grid(which='major', color='#DDDDDD', linewidth=0.8)
+        ax4.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5)
+        ax4.minorticks_on()
+        pdf_pages.savefig(fig4)
+
+
+        fig5,ax5 = plt.subplots()
+        ax5.plot(self.ideal_traj_x, self.ideal_traj_y, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10)
+        ax5.plot(self.bag_x, self.bag_y, label='Recorded trajectory')
+        ax5.set_xlabel('x [m]')
+        ax5.set_ylabel('y [m]')
+        ax5.set_title('2D visualization')
+        fig5.tight_layout(pad=4)
+        ax5.grid(which='major', color='#DDDDDD', linewidth=0.8)
+        ax5.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5)
+        ax5.minorticks_on()
+        pdf_pages.savefig(fig5)
+
+
+        fig6 = plt.figure()
+        ax6 = fig6.add_subplot(projection="3d")
+        ax6.plot3D(self.ideal_traj_x,self.ideal_traj_y,self.ALTITUDE_CONST_FIG8, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10)
+        ax6.plot3D(self.bag_x,self.bag_y,self.bag_z, label='Recorded trajectory', linewidth=1)
+        ax6.grid(True)
+        ax6.set_title('3D visualization')
+        ax6.set_xlabel('x [m]')
+        ax6.set_ylabel('y [m]')
+        ax6.set_zlabel('z [m]')
+        plt.close(fig6)
+        plt.tight_layout(pad=4)
+        pdf_pages.savefig(fig6)
+
+
+
+
+
+        pdf_pages.close()
+        print("Result saved in " + pdfname_and_path + '.pdf')
+
+    
+    # def create_pdf(self, ideal_csvfile:str, rosbag:str, pdfname_and_path:str):
+    #     pdf_pages = PdfPages(self.output_dir + pdfname_and_path + '.pdf')
+
+
+    #     fig8_ideal_csv = "/home/jthevenoz/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
+
+    #     index = rosbag.find("bag_")
+    #     bagname = rosbag[index:] 
+    #     print(bagname)
+    #     fig8_recorded_csv_filepath = rosbag + "fig8_" + bagname + "fig8_" + bagname[:bagname.index("/")] + "_0.mcap"
+    #     print(fig8_recorded_csv_filepath)
+
+
+    
+    #     create_figures(fig8_recorded_csv_filepath,fig8_ideal_csv)
+
+    #     multitraj_ideal_csv = "/home/julien/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv"
+    #     create_figure(multitraj_ideal_csv)
+
+    #     pdf_pages.close()
+    #     print("Result saved in " + pdfname_and_path + '.pdf')
+
+
+
+if __name__=="__main__":
+    #####writing rosbag to csv
+    # writer = McapHandler()
+    # path = str(Path(__file__).parent /"bags/tfbag.mcap")
+    # writer.write_bag_to_csv(path, "tf_rosbag.csv")
+
+
+
+    # ideal_csv  =  "/home/jthevenoz/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
+    # rosbagcsv = "/home/jthevenoz/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.csv"
+    # pdf = "/home/jthevenoz/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/testingtheclass"
+
+    ideal_csv_multi = "/home/julien/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv"
+    ideal_csv_fig8 = "/home/julien/ros2_ws/src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
+    rosbagcsv_multi = "/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.csv"
+    rosbagcsv_fig8 = "/"
+    pdf = "/home/julien/Desktop/IMRC/IMRC-shared5/bags/multibag/multitesting"
+
+    paul = Plotter()
+    paul.create_figures(ideal_csv_multi,rosbagcsv_multi, pdf)
+    import subprocess
+    ####directly open the pdf is nice
+    subprocess.call(["xdg-open", "/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multitesting.pdf"])
\ No newline at end of file