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/CMakeLists.txt b/crazyflie/CMakeLists.txt
index c8899cfbe..8a2e4f378 100644
--- a/crazyflie/CMakeLists.txt
+++ b/crazyflie/CMakeLists.txt
@@ -109,6 +109,8 @@ install(PROGRAMS
   scripts/vel_mux.py
   scripts/cfmult.py
   scripts/simple_mapper_multiranger.py
+  scripts/aideck_streamer.py
+  scripts/gui.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/crazyflie/config/aideck_streamer.yaml b/crazyflie/config/aideck_streamer.yaml
new file mode 100644
index 000000000..280ba9163
--- /dev/null
+++ b/crazyflie/config/aideck_streamer.yaml
@@ -0,0 +1,30 @@
+image_topic: /camera/image
+camera_info_topic: /camera/camera_info
+deck_ip: "192.168.4.1"
+deck_port: 5000
+image_width: 324
+image_height: 324
+camera_name: crazyflie
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [181.87464,   0.     , 162.52301,
+           0.     , 182.58129, 160.79418,
+           0.     ,   0.     ,   1.     ]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.070366, -0.006434, -0.002691, -0.001983, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [169.24555,   0.     , 161.54541,   0.     ,
+           0.     , 169.97813, 159.07974,   0.     ,
+           0.     ,   0.     ,   1.     ,   0.     ]
\ No newline at end of file
diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml
index adefcbfd1..5a49a0e0e 100644
--- a/crazyflie/config/crazyflies.yaml
+++ b/crazyflie/config/crazyflies.yaml
@@ -72,7 +72,7 @@ all:
   # firmware logging for all drones (use robot_types/type_name to set per type, or
   # robots/drone_name to set per drone)
   firmware_logging:
-    enabled: false
+    enabled: true
     default_topics:
      # remove to disable default topic
       pose:
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/scripts/aideck_streamer.py b/crazyflie/scripts/aideck_streamer.py
new file mode 100755
index 000000000..c97df2960
--- /dev/null
+++ b/crazyflie/scripts/aideck_streamer.py
@@ -0,0 +1,170 @@
+#!/usr/bin/env python3
+import socket,os,struct, time
+import numpy as np
+import cv2
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image, CameraInfo
+
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType
+
+
+class ImageStreamerNode(Node):
+    def __init__(self):
+        super().__init__("image_node")
+
+        # declare config path parameter
+        self.declare_parameter(
+            name="config_path",
+            value=os.path.join(
+                    get_package_share_directory('crazyflie'),
+                    'config',
+                    'aideck_streamer.yaml'
+                )
+        )
+
+        config_path = self.get_parameter("config_path").value
+        with open(config_path) as f:
+            config = yaml.load(f, Loader=yaml.FullLoader)
+
+        # declare topic names
+        self.declare_parameter(
+            name="image_topic",
+            value=config["image_topic"],
+            descriptor=ParameterDescriptor(
+                type=ParameterType.PARAMETER_STRING,
+                description="Image topic to publish to.",
+            ),
+        )
+
+        self.declare_parameter(
+            name="camera_info_topic",
+            value=config["camera_info_topic"],
+            descriptor=ParameterDescriptor(
+                type=ParameterType.PARAMETER_STRING,
+                description="Camera info topic to subscribe to.",
+            ),
+        )
+
+        # declare aideck ip and port
+        self.declare_parameter(
+            name='deck_ip',
+            value=config['deck_ip'],        
+            )
+        
+        self.declare_parameter(
+            name='deck_port',
+            value=config['deck_port'],        
+        )
+
+        # define variables from ros2 parameters
+        image_topic = (
+            self.get_parameter("image_topic").value
+        )
+        self.get_logger().info(f"Image topic: {image_topic}")
+
+        info_topic = (
+            self.get_parameter("camera_info_topic").value
+        )
+        self.get_logger().info(f"Image info topic: {info_topic}")
+
+
+        # create messages and publishers
+        self.image_msg = Image()
+        self.camera_info_msg = self._construct_from_yaml(config)
+        self.image_publisher = self.create_publisher(Image, image_topic, 10)
+        self.info_publisher = self.create_publisher(CameraInfo, info_topic, 10)
+
+        # set up connection to AI Deck
+        deck_ip = self.get_parameter("deck_ip").value
+        deck_port = int(self.get_parameter("deck_port").value)
+        self.get_logger().info("Connecting to socket on {}:{}...".format(deck_ip, deck_port))
+        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.client_socket.connect((deck_ip, deck_port))
+        self.get_logger().info("Socket connected")
+        self.image = None
+        self.rx_buffer = bytearray()
+
+        # set up timers for callbacks
+        timer_period = 0.01
+        self.rx_timer = self.create_timer(timer_period, self.receive_callback)
+        self.tx_timer = self.create_timer(timer_period, self.publish_callback)
+
+
+    def _construct_from_yaml(self, config):
+        camera_info = CameraInfo()
+
+        camera_info.header.frame_id = config['camera_name']
+        camera_info.header.stamp = self.get_clock().now().to_msg()
+        camera_info.width = int(config['image_width'])
+        camera_info.height = int(config['image_height'])
+        camera_info.distortion_model = config['distortion_model']
+        camera_info.d = config['distortion_coefficients']['data']
+        camera_info.k = config['camera_matrix']['data']
+        camera_info.r = config['rectification_matrix']['data']
+        camera_info.p = config['projection_matrix']['data']
+        return camera_info
+
+    def _rx_bytes(self, size):
+        data = bytearray()
+        while len(data) < size:
+            data.extend(self.client_socket.recv(size-len(data)))
+        return data
+
+    def receive_callback(self):
+        # first get the info
+        packetInfoRaw = self._rx_bytes(4)
+        [length, routing, function] = struct.unpack('<HBB', packetInfoRaw)
+
+        # receive the header
+        imgHeader = self._rx_bytes(length - 2)
+        [magic, width, height, depth, format, size] = struct.unpack('<BHHBBI', imgHeader)
+
+        # if magic is correct, get new image
+        if magic == 0xBC:
+            imgStream = bytearray()
+
+            while len(imgStream) < size:
+                packetInfoRaw = self._rx_bytes(4)
+                [length, dst, src] = struct.unpack('<HBB', packetInfoRaw)
+                #print("Chunk size is {} ({:02X}->{:02X})".format(length, src, dst))
+                chunk = self._rx_bytes(length - 2)
+                imgStream.extend(chunk)
+
+            raw_img = np.frombuffer(imgStream, dtype=np.uint8)
+            raw_img.shape = (width, height)
+            self.image = cv2.cvtColor(raw_img, cv2.COLOR_BayerBG2RGBA)
+
+    def publish_callback(self):
+        if self.image is not None:
+            self.image_msg.header.frame_id = self.camera_info_msg.header.frame_id
+            self.image_msg.header.stamp = self.get_clock().now().to_msg()
+            self.camera_info_msg.header.stamp = self.image_msg.header.stamp
+            width, height, channels = self.image.shape
+            self.image_msg.height = height
+            self.image_msg.width = width
+            self.image_msg.encoding = 'rgba8'
+            self.image_msg.step = width * channels   # number of bytes each row in the array will occupy
+            self.image_msg.is_bigendian = 0 # TODO: implement automatic check depending on system
+            self.image_msg.data = self.image.flatten().data
+
+            self.image_publisher.publish(self.image_msg)
+            self.info_publisher.publish(self.camera_info_msg)
+            self.image = None
+            
+            
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImageStreamerNode()
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file
diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py
index fbbe8fe72..a38dd6808 100755
--- a/crazyflie/scripts/crazyflie_server.py
+++ b/crazyflie/scripts/crazyflie_server.py
@@ -26,7 +26,7 @@
 from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging
 from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop
 from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType
-from crazyflie_interfaces.msg import Hover, LogDataGeneric, FullState
+from crazyflie_interfaces.msg import Status, Hover, LogDataGeneric, FullState
 from motion_capture_tracking_interfaces.msg import NamedPoseArray
 
 from std_srvs.srv import Empty
@@ -67,24 +67,31 @@ def __init__(self):
         self._ros_parameters = self._param_to_dict(self._parameters)
 
         self.uris = []
-        self.cf_dict = {}
+        # for logging, assign a all -> all mapping
+        self.cf_dict = {
+            'all': 'all'
+        }
         self.uri_dict = {}
         self.type_dict = {}
 
         # Assign default topic types, variables and callbacks
         self.default_log_type = {"pose": PoseStamped,
                                  "scan": LaserScan,
-                                 "odom": Odometry}
+                                 "odom": Odometry,
+                                 "status": Status}
         self.default_log_vars = {"pose": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z',
                                           'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'],
                                  "scan": ['range.front', 'range.left', 'range.back', 'range.right'],
                                  "odom": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z',
                                           'stabilizer.yaw', 'stabilizer.roll', 'stabilizer.pitch',
                                           'kalman.statePX', 'kalman.statePY', 'kalman.statePZ',
-                                          'gyro.z', 'gyro.x', 'gyro.y']}
+                                          'gyro.z', 'gyro.x', 'gyro.y'],
+                                 "status": ['supervisor.info', 'pm.vbatMV', 'pm.state',
+                                          'radio.rssi']}
         self.default_log_fnc = {"pose": self._log_pose_data_callback,
                                 "scan": self._log_scan_data_callback,
-                                "odom": self._log_odom_data_callback}
+                                "odom": self._log_odom_data_callback,
+                                "status": self._log_status_data_callback}
 
         self.world_tf_name = "world"
         try:
@@ -220,6 +227,9 @@ def __init__(self):
             StartTrajectory, "all/start_trajectory", self._start_trajectory_callback)
 
         for uri in self.cf_dict:
+            if uri == "all":
+                continue
+
             name = self.cf_dict[uri]
 
             pub = self.create_publisher(String, name + '/robot_description',
@@ -350,11 +360,12 @@ def _fully_connected(self, link_uri):
         Called when all parameters have been updated
           and the full log toc has been received of the Crazyflie
         """
-        self.get_logger().info(f" {link_uri} is fully connected!")
+        self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!")
 
         self.swarm.fully_connected_crazyflie_cnt += 1
 
-        if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict):
+        # use len(self.cf_dict) - 1, since cf_dict contains "all" as well
+        if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict) - 1:
             self.get_logger().info("All Crazyflies are fully connected!")
             self._init_parameters()
             self._init_logging()
@@ -363,10 +374,10 @@ def _fully_connected(self, link_uri):
             return
 
     def _disconnected(self, link_uri):
-        self.get_logger().info(f" {link_uri} is disconnected!")
+        self.get_logger().info(f"[{self.cf_dict[link_uri]}] is disconnected!")
 
     def _connection_failed(self, link_uri, msg):
-        self.get_logger().info(f"{link_uri} connection Failed")
+        self.get_logger().info(f"[{self.cf_dict[link_uri]}] connection Failed")
         self.swarm.close_links()
 
     def _init_logging(self):
@@ -402,20 +413,20 @@ def _init_logging(self):
                             self._log_error_callback)
                         lg_custom.start()
                     except KeyError as e:
-                        self.get_logger().info(f'{link_uri}: Could not start log configuration,'
+                        self.get_logger().info(f'[{self.cf_dict[link_uri]}] Could not start log configuration,'
                                                '{} not found in TOC'.format(str(e)))
                     except AttributeError:
                         self.get_logger().info(
-                            f'{link_uri}: Could not add log config, bad configuration.')
+                            f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.')
 
-                self.get_logger().info(f"{link_uri} setup custom logging")
+                self.get_logger().info(f"[{self.cf_dict[link_uri]}] setup custom logging")
 
             self.create_service(
                 RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri))
             self.create_service(
                 AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri))
 
-        self.get_logger().info("All Crazyflies loggging are initialized")
+        self.get_logger().info("All Crazyflies logging are initialized.")
 
     def _init_default_logging(self, prefix, link_uri, callback_fnc):
         """
@@ -434,13 +445,13 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc):
             self.declare_parameter(
                 self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency)
             self.get_logger().info(
-                f"{link_uri} setup logging for {prefix} at freq {frequency}")
+                f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}")
         except KeyError as e:
-            self.get_logger().info(f'{link_uri}: Could not start log configuration,'
+            self.get_logger().error(f'[{self.cf_dict[link_uri]}] Could not start log configuration,'
                                    '{} not found in TOC'.format(str(e)))
         except AttributeError:
-            self.get_logger().info(
-                f'{link_uri}: Could not add log config, bad configuration.')
+            self.get_logger().error(
+                f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.')
 
     def _log_scan_data_callback(self, timestamp, data, logconf, uri):
         """
@@ -569,6 +580,21 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri):
         t_base.transform.rotation.w = q[3]
         self.tfbr.sendTransform(t_base)
 
+    def _log_status_data_callback(self, timestamp, data, logconf, uri):
+        """
+        Send out the ROS 2 status topic
+        """
+
+        msg = Status()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self.world_tf_name
+        msg.supervisor_info = data.get('supervisor.info')
+        msg.battery_voltage = data.get('pm.vbatMV') / 1000.0
+        msg.pm_state = data.get('pm.state')
+        msg.rssi = data.get('radio.rssi')
+
+        self.swarm._cfs[uri].logging["status_publisher"].publish(msg)
+
     def _log_custom_data_callback(self, timestamp, data, logconf, uri):
         """
         Once custom log block is retrieved from the Crazyflie, 
@@ -633,7 +659,7 @@ def _init_parameters(self):
                         #       crazyflie with get_value due to threading.
                         cf.param.set_value(name, set_param_value)
                         self.get_logger().info(
-                            f" {link_uri}: {name} is set to {set_param_value}"
+                            f"[{self.cf_dict[link_uri]}] {name} is set to {set_param_value}"
                         )
                         self.declare_parameter(
                             self.cf_dict[link_uri] +
@@ -672,7 +698,7 @@ def _init_parameters(self):
             # Now all parameters are set
             set_param_all = True
 
-        self.get_logger().info("All Crazyflies parameters are initialized")
+        self.get_logger().info("All Crazyflies parameters are initialized.")
 
     def _parameters_callback(self, params):
         """
@@ -682,38 +708,39 @@ def _parameters_callback(self, params):
         for param in params:
             param_split = param.name.split(".")
 
-            if param_split[0] in self.cf_dict.values():
-                cf_name = param_split[0]
+            if param_split[0] == "all":
                 if param_split[1] == "params":
                     name_param = param_split[2] + "." + param_split[3]
                     try:
-                        self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value(
-                            name_param, param.value
-                        )
+                        for link_uri in self.uris:
+                            cf = self.swarm._cfs[link_uri].cf.param.set_value(
+                                name_param, param.value
+                            )
                         self.get_logger().info(
-                            f" {self.uri_dict[cf_name]}: {name_param} is set to {param.value}"
+                            f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}"
                         )
                         return SetParametersResult(successful=True)
                     except Exception as e:
                         self.get_logger().info(str(e))
                         return SetParametersResult(successful=False)
-                if param_split[1] == "logs":
-                    return SetParametersResult(successful=True)
-            elif param_split[0] == "all":
+            elif param_split[0] in self.cf_dict.values():
+                cf_name = param_split[0]
                 if param_split[1] == "params":
                     name_param = param_split[2] + "." + param_split[3]
                     try:
-                        for link_uri in self.uris:
-                            cf = self.swarm._cfs[link_uri].cf.param.set_value(
-                                name_param, param.value
-                            )
+                        self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value(
+                            name_param, param.value
+                        )
                         self.get_logger().info(
-                            f" {link_uri}: {name_param} is set to {param.value}"
+                            f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}"
                         )
                         return SetParametersResult(successful=True)
                     except Exception as e:
                         self.get_logger().info(str(e))
                         return SetParametersResult(successful=False)
+                if param_split[1] == "logs":
+                    return SetParametersResult(successful=True)
+
 
         return SetParametersResult(successful=False)
 
@@ -732,12 +759,14 @@ def _takeoff_callback(self, request, response, uri="all"):
             a certain height in high level commander
         """
 
+        print("call1 ", uri)
+
         duration = float(request.duration.sec) + \
             float(request.duration.nanosec / 1e9)
         self.get_logger().info(
-            f"takeoff(height={request.height} m,"
+            f"[{self.cf_dict[uri]}] takeoff(height={request.height} m,"
             + f"duration={duration} s,"
-            + f"group_mask={request.group_mask}) {uri}"
+            + f"group_mask={request.group_mask})"
         )
         if uri == "all":
             for link_uri in self.uris:
@@ -759,7 +788,7 @@ def _land_callback(self, request, response, uri="all"):
         duration = float(request.duration.sec) + \
             float(request.duration.nanosec / 1e9)
         self.get_logger().info(
-            f"land(height={request.height} m,"
+            f"[{self.cf_dict[uri]}] land(height={request.height} m,"
             + f"duration={duration} s,"
             + f"group_mask={request.group_mask})"
         )
@@ -784,8 +813,9 @@ def _go_to_callback(self, request, response, uri="all"):
             float(request.duration.nanosec / 1e9)
 
         self.get_logger().info(
-            "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)"
+            "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)"
             % (
+                self.cf_dict[uri],
                 request.goal.x,
                 request.goal.y,
                 request.goal.z,
@@ -820,7 +850,7 @@ def _go_to_callback(self, request, response, uri="all"):
 
     def _notify_setpoints_stop_callback(self, request, response, uri="all"):
 
-        self.get_logger().info(f"{uri}: Received notify setpoint stop")
+        self.get_logger().info(f"[{self.cf_dict[uri]}] Received notify setpoint stop")
 
         if uri == "all":
             for link_uri in self.uris:
@@ -836,7 +866,8 @@ def _upload_trajectory_callback(self, request, response, uri="all"):
         offset = request.piece_offset
         lenght = len(request.pieces)
         total_duration = 0
-        self.get_logger().info("upload_trajectory(id=%d,offset=%d, lenght=%d)" % (
+        self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" % (
+            self.cf_dict[uri],
             id,
             offset,
             lenght,
@@ -862,7 +893,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"):
                 trajectory_mem.trajectory = trajectory
                 upload_result = trajectory_mem.write_data_sync()
                 if not upload_result:
-                    self.get_logger().info(f"{link_uri}: Upload failed")
+                    self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed")
                     upload_success_all = False
                 else:
                     self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory(
@@ -876,7 +907,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"):
             trajectory_mem.trajectory = trajectory
             upload_result = trajectory_mem.write_data_sync()
             if not upload_result:
-                self.get_logger().info(f"{uri}: Upload failed")
+                self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed")
                 response.success = False
                 return response
             self.swarm._cfs[uri].cf.high_level_commander.define_trajectory(
@@ -892,7 +923,8 @@ def _start_trajectory_callback(self, request, response, uri="all"):
         rev = request.reversed
         gm = request.group_mask
 
-        self.get_logger().info("start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % (
+        self.get_logger().info("[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % (
+            self.cf_dict[uri],
             id,
             ts,
             rel,
@@ -983,10 +1015,10 @@ def _remove_logging(self, request, response, uri="all"):
                 self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop()
                 self.destroy_publisher(
                     self.swarm._cfs[uri].logging[topic_name + "_publisher"])
-                self.get_logger().info(f"{uri}: Remove {topic_name} logging")
+                self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging")
             except rclpy.exceptions.ParameterNotDeclaredException:
                 self.get_logger().info(
-                    f"{uri}: No logblock of {topic_name} has been found ")
+                    f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ")
                 response.success = False
                 return response
         else:
@@ -996,10 +1028,10 @@ def _remove_logging(self, request, response, uri="all"):
                 for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]:
                     self.destroy_publisher(
                         self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name])
-                self.get_logger().info(f"{uri}: Remove {topic_name} logging")
+                self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging")
             except rclpy.exceptions.ParameterNotDeclaredException:
                 self.get_logger().info(
-                    f"{uri}: No logblock of {topic_name} has been found ")
+                    f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ")
                 response.success = False
                 return response
 
@@ -1023,10 +1055,10 @@ def _add_logging(self, request, response, uri="all"):
                                              "_log_config"].period_in_ms = 1000 / frequency
                 self.swarm._cfs[uri].logging[topic_name +
                                              "_log_config"].start()
-                self.get_logger().info(f"{uri}: Add {topic_name} logging")
+                self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging")
             except rclpy.exceptions.ParameterAlreadyDeclaredException:
                 self.get_logger().info(
-                    f"{uri}: The content the logging of {topic_name} has already started ")
+                    f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started ")
                 response.success = False
                 return response
         else:
@@ -1054,11 +1086,11 @@ def _add_logging(self, request, response, uri="all"):
                 self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables
                 self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency
 
-                self.get_logger().info(f"{uri}: Add {topic_name} logging")
+                self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging")
             except KeyError as e:
-                self.get_logger().info(
-                    f"{uri}: Failed to add {topic_name} logging")
-                self.get_logger().info(str(e) + "is not in TOC")
+                self.get_logger().error(
+                    f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging")
+                self.get_logger().error(str(e) + "is not in TOC")
                 self.undeclare_parameter(
                     self.cf_dict[uri] + ".logs." + topic_name + ".frequency.")
                 self.undeclare_parameter(
@@ -1066,8 +1098,8 @@ def _add_logging(self, request, response, uri="all"):
                 response.success = False
                 return response
             except rclpy.exceptions.ParameterAlreadyDeclaredException:
-                self.get_logger().info(
-                    f"{uri}: The content or part of the logging of {topic_name} has already started ")
+                self.get_logger().error(
+                    f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started ")
                 response.success = False
                 return response
 
diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py
new file mode 100755
index 000000000..9a7d4b9e5
--- /dev/null
+++ b/crazyflie/scripts/gui.py
@@ -0,0 +1,230 @@
+#!/usr/bin/env python3
+
+import threading
+import time
+from pathlib import Path
+from functools import partial
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rcl_interfaces.msg import Log
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+
+# from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+from crazyflie_interfaces.msg import Status
+import rowan
+
+from nicegui import Client, app, events, ui, ui_run
+
+
+class NiceGuiNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__('nicegui')
+
+        # find all crazyflies
+        self.cfnames = []
+        for srv_name, srv_types in self.get_service_names_and_types():
+            if 'crazyflie_interfaces/srv/StartTrajectory' in srv_types:
+                # remove '/' and '/start_trajectory'
+                cfname = srv_name[1:-17]
+                if cfname != 'all':
+                    self.cfnames.append(cfname)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 1)
+        self.sub_log = self.create_subscription(Log, 'rosout', self.on_rosout, rclpy.qos.QoSProfile(
+                        depth=1000,
+                        durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL))
+
+        self.logs = dict()
+        self.supervisor_labels = dict()
+        self.battery_labels = dict()
+        self.radio_labels = dict()
+        self.robotmodels = dict()
+
+        with Client.auto_index_client:
+
+            with ui.row().classes('items-stretch'):
+                with ui.card().classes('w-full h-full'):
+                    ui.label('Visualization').classes('text-2xl')
+                    with ui.scene(800, 400, on_click=self.on_vis_click) as scene:
+                        for name in self.cfnames:
+                            robot = scene.stl('/urdf/cf2_assembly.stl').scale(1.0).material('#ff0000').with_name(name)
+                            self.robotmodels[name] = robot
+                            # augment with some additional fields
+                            robot.status_ok = False
+                            robot.status_watchdog = time.time()
+                    scene.camera.x = 0
+                    scene.camera.y = -1
+                    scene.camera.z = 2
+                    scene.camera.look_at_x = 0
+                    scene.camera.look_at_y = 0
+                    scene.camera.look_at_z = 0
+
+            with ui.row().classes('w-full h-lvh'):
+                with ui.tabs().classes('w-full') as tabs:
+                    self.tabs = []
+                    for name in ["all"] + self.cfnames:
+                        self.tabs.append(ui.tab(name))
+                with ui.tab_panels(tabs, value=self.tabs[0], on_change=self.on_tab_change).classes('w-full') as self.tabpanels:
+                    for name, tab in zip(["all"] + self.cfnames, self.tabs):
+                        with ui.tab_panel(tab):
+                            self.logs[name] = ui.log().classes('w-full h-96 no-wrap')
+                            self.supervisor_labels[name] = ui.label("")
+                            self.battery_labels[name] = ui.label("")
+                            self.radio_labels[name] = ui.label("")
+
+            for name in self.cfnames:
+                self.create_subscription(Status, name + '/status', partial(self.on_status, name=name), 1)
+
+            # Call on_timer function
+            update_rate = 30 # Hz
+            self.timer = self.create_timer(
+                1.0/update_rate, 
+                self.on_timer,
+                clock=rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME))
+
+    def on_rosout(self, msg: Log) -> None:
+        # filter by crazyflie and add to the correct log
+        if msg.name == "crazyflie_server":
+            if msg.msg.startswith("["):
+                idx = msg.msg.find("]")
+                name = msg.msg[1:idx]
+                # if it was an "all" category, add only to CFs
+                if name == 'all':
+                    for logname ,log in self.logs.items():
+                        if logname != "all":
+                            log.push(msg.msg)
+                elif name in self.logs:
+                    self.logs[name].push(msg.msg[idx+2:])
+
+        # add all possible messages to the 'all' tab
+        self.logs['all'].push(msg.msg)
+
+    def on_timer(self) -> None:
+        for name, robotmodel in self.robotmodels.items():
+            ros_time = rclpy.time.Time() # get the latest
+            robot_status_ok = robotmodel.status_ok
+            if self.tf_buffer.can_transform("world", name, ros_time):
+                t = self.tf_buffer.lookup_transform(
+                                "world",
+                                name,
+                                ros_time)
+                transform_time = rclpy.time.Time.from_msg(t.header.stamp)
+                transform_age = self.get_clock().now() - transform_time
+                # latest transform is older than a second indicates a problem
+                if transform_age.nanoseconds * 1e-9 > 1:
+                    robot_status_ok = False
+                else:
+                    pos = t.transform.translation
+                    robotmodel.move(pos.x, pos.y, pos.z)
+                    robotmodel.rotate(*rowan.to_euler([
+                        t.transform.rotation.w,
+                        t.transform.rotation.x,
+                        t.transform.rotation.y,
+                        t.transform.rotation.z], "xyz"))
+            else:
+                # no available transform indicates a problem
+                robot_status_ok = False
+
+            # no status update for a while, indicate a problem
+            if time.time() - robotmodel.status_watchdog > 2.0:
+                robot_status_ok = False
+
+            # any issues detected -> mark red, otherwise green
+            if robot_status_ok:
+                robotmodel.material('#00ff00')
+            else:
+                robotmodel.material('#ff0000')
+
+    def on_vis_click(self, e: events.SceneClickEventArguments):
+        hit = e.hits[0]
+        name = hit.object_name or hit.object_id
+        ui.notify(f'You clicked on the {name}')
+        if name == 'ground':
+            self.tabpanels.value = 'all'
+        else:
+            self.tabpanels.value = name
+
+    def on_status(self, msg, name) -> None:
+        status_ok = True
+        supervisor_text = ""
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_BE_ARMED:
+            supervisor_text += "can be armed; "
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_ARMED:
+            supervisor_text += "is armed; "
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_AUTO_ARM:
+            supervisor_text += "auto-arm; "
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_FLY:
+            supervisor_text += "can fly; "
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_FLYING:
+            supervisor_text += "is flying; "
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_TUMBLED:
+            supervisor_text += "is tumpled; "
+            status_ok = False
+        if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_LOCKED:
+            supervisor_text += "is locked; "
+            status_ok = False
+        self.supervisor_labels[name].set_text(supervisor_text)
+
+        battery_text = f'{msg.battery_voltage:.2f} V'
+        if msg.battery_voltage < 3.8:
+            status_ok = False
+        if msg.pm_state == Status.PM_STATE_BATTERY:
+            battery_text += " (on battery)"
+        elif msg.pm_state == Status.PM_STATE_CHARGING:
+            battery_text += " (charging)"
+        elif msg.pm_state == Status.PM_STATE_CHARGED:
+            battery_text += " (charged)"
+        elif msg.pm_state == Status.PM_STATE_LOW_POWER:
+            battery_text += " (low power)"
+            status_ok = False
+        elif msg.pm_state == Status.PM_STATE_SHUTDOWN:
+            battery_text += " (shutdown)"
+            status_ok = False
+        self.battery_labels[name].set_text(battery_text)
+
+        radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}'
+        self.radio_labels[name].set_text(radio_text)
+
+        # save status here
+        self.robotmodels[name].status_ok = status_ok
+
+        # store the time when we last received any status
+        self.robotmodels[name].status_watchdog = time.time()
+
+    def on_tab_change(self, arg):
+        for name, robotmodel in self.robotmodels.items():
+            if name != arg.value:
+                robotmodel.scale(1)
+        if arg.value in self.robotmodels:
+            self.robotmodels[arg.value].scale(2)
+
+
+def main() -> None:
+    # NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading
+    pass
+
+
+def ros_main() -> None:
+    rclpy.init()
+    node = NiceGuiNode()
+    try:
+        rclpy.spin(node)
+    except ExternalShutdownException:
+        pass
+
+
+app.add_static_files("/urdf",
+                     str((Path(__file__).parent.parent.parent / "share" / "crazyflie" / "urdf").resolve()),
+                     follow_symlink=True)
+app.on_startup(lambda: threading.Thread(target=ros_main).start())
+ui_run.APP_IMPORT_STRING = f'{__name__}:app'  # ROS2 uses a non-standard module name, so we need to specify it here
+ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()), favicon='🤖')
diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp
index 1dfe1de98..6f65df3d5 100644
--- a/crazyflie/src/crazyflie_server.cpp
+++ b/crazyflie/src/crazyflie_server.cpp
@@ -39,13 +39,19 @@ using std_srvs::srv::Empty;
 using motion_capture_tracking_interfaces::msg::NamedPoseArray;
 using crazyflie_interfaces::msg::FullState;
 
+// Note on logging: we use a single logger with string prefixes
+// A better way would be to use named child loggers, but these do not
+// report to /rosout in humble, see https://github.com/ros2/rclpy/issues/1131
+// Once we do not support humble anymore, consider switching to child loggers
+
 // Helper class to convert crazyflie_cpp logging messages to ROS logging messages
 class CrazyflieLogger : public Logger
 {
 public:
-  CrazyflieLogger(rclcpp::Logger logger)
+  CrazyflieLogger(rclcpp::Logger logger, const std::string& prefix)
       : Logger()
       , logger_(logger)
+      , prefix_(prefix)
   {
   }
 
@@ -53,20 +59,21 @@ class CrazyflieLogger : public Logger
 
   virtual void info(const std::string &msg)
   {
-    RCLCPP_INFO(logger_, "%s", msg.c_str());
+    RCLCPP_INFO(logger_, "%s %s", prefix_.c_str(), msg.c_str());
   }
 
   virtual void warning(const std::string &msg)
   {
-    RCLCPP_WARN(logger_, "%s", msg.c_str());
+    RCLCPP_WARN(logger_, "%s %s", prefix_.c_str(),  msg.c_str());
   }
 
   virtual void error(const std::string &msg)
   {
-    RCLCPP_ERROR(logger_, "%s", msg.c_str());
+    RCLCPP_ERROR(logger_, "%s %s", prefix_.c_str(), msg.c_str());
   }
 private:
   rclcpp::Logger logger_;
+  std::string prefix_;
 };
 
 std::set<std::string> extract_names(
@@ -129,8 +136,8 @@ class CrazyflieROS
     rclcpp::CallbackGroup::SharedPtr callback_group_cf_srv,
     const CrazyflieBroadcaster* cfbc,
     bool enable_parameters = true)
-    : logger_(rclcpp::get_logger(name))
-    , cf_logger_(logger_)
+    : logger_(node->get_logger())
+    , cf_logger_(logger_, "[" + name + "]")
     , cf_(
       link_uri,
       cf_logger_,
@@ -230,7 +237,7 @@ class CrazyflieROS
       bool query_all_values_on_connect = node->get_parameter("firmware_params.query_all_values_on_connect").get_parameter_value().get<bool>();
 
       int numParams = 0;
-      RCLCPP_INFO(logger_, "Requesting parameters...");
+      RCLCPP_INFO(logger_, "[%s] Requesting parameters...", name_.c_str());
       cf_.requestParamToc(/*forceNoCache*/false, /*requestValues*/query_all_values_on_connect);
       for (auto iter = cf_.paramsBegin(); iter != cf_.paramsEnd(); ++iter) {
         auto entry = *iter;
@@ -287,7 +294,7 @@ class CrazyflieROS
           }
           break;
         default:
-          RCLCPP_WARN(logger_, "Unknown param type for %s/%s", entry.group.c_str(), entry.name.c_str());
+          RCLCPP_WARN(logger_, "[%s] Unknown param type for %s/%s", name_.c_str(), entry.group.c_str(), entry.name.c_str());
           break;
         }
         // If there is no such parameter in all, add it
@@ -303,7 +310,7 @@ class CrazyflieROS
       }
       auto end1 = std::chrono::system_clock::now();
       std::chrono::duration<double> elapsedSeconds1 = end1 - start;
-      RCLCPP_INFO(logger_, "reqParamTOC: %f s (%d params)", elapsedSeconds1.count(), numParams);
+      RCLCPP_INFO(logger_, "[%s] reqParamTOC: %f s (%d params)", name_.c_str(), elapsedSeconds1.count(), numParams);
       
       // Set parameters as specified in the configuration files, as in the following order
       // 1.) check all/firmware_params
@@ -353,7 +360,7 @@ class CrazyflieROS
           // check if any of the default topics are enabled
           if (i.first.find("default_topics.pose") == 0) {
             int freq = log_config_map["default_topics.pose.frequency"].get<int>();
-            RCLCPP_INFO(logger_, "Logging to /pose at %d Hz", freq);
+            RCLCPP_INFO(logger_, "[%s] Logging to /pose at %d Hz", name_.c_str(), freq);
 
             publisher_pose_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(name + "/pose", 10);
 
@@ -370,7 +377,7 @@ class CrazyflieROS
           }
           else if (i.first.find("default_topics.scan") == 0) {
             int freq = log_config_map["default_topics.scan.frequency"].get<int>();
-            RCLCPP_INFO(logger_, "Logging to /scan at %d Hz", freq);
+            RCLCPP_INFO(logger_, "[%s] Logging to /scan at %d Hz", name_.c_str(), freq);
 
             publisher_scan_ = node->create_publisher<sensor_msgs::msg::LaserScan>(name + "/scan", 10);
 
@@ -387,7 +394,7 @@ class CrazyflieROS
           }
           else if (i.first.find("default_topics.status") == 0) {
             int freq = log_config_map["default_topics.status.frequency"].get<int>();
-            RCLCPP_INFO(logger_, "Logging to /status at %d Hz", freq);
+            RCLCPP_INFO(logger_, "[%s] Logging to /status at %d Hz", name_.c_str(), freq);
 
             publisher_status_ = node->create_publisher<crazyflie_interfaces::msg::Status>(name + "/status", 10);
 
@@ -417,7 +424,7 @@ class CrazyflieROS
 
             // older firmware -> use other 16-bit variables
             if (!status_has_radio_stats_) {
-                RCLCPP_WARN(logger_, "Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero.");
+                RCLCPP_WARN(logger_, "[%s] Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero.", name_.c_str());
                 logvars.push_back({"pm", "vbatMV"});
                 logvars.push_back({"pm", "vbatMV"});
             }
@@ -433,7 +440,7 @@ class CrazyflieROS
             int freq = log_config_map["custom_topics." + topic_name + ".frequency"].get<int>();
             auto vars = log_config_map["custom_topics." + topic_name + ".vars"].get<std::vector<std::string>>();
             
-            RCLCPP_INFO(logger_, "Logging to %s at %d Hz", topic_name.c_str(), freq);
+            RCLCPP_INFO(logger_, "[%s] Logging to %s at %d Hz", name_.c_str(), topic_name.c_str(), freq);
 
             publishers_generic_.emplace_back(node->create_publisher<crazyflie_interfaces::msg::LogDataGeneric>(name + "/" + topic_name, 10));
 
@@ -455,7 +462,7 @@ class CrazyflieROS
       }
     }
 
-    RCLCPP_INFO(logger_, "Requesting memories...");
+    RCLCPP_INFO(logger_, "[%s] Requesting memories...", name_.c_str());
     cf_.requestMemoryToc();
   }
 
@@ -491,7 +498,7 @@ class CrazyflieROS
     if (p.get_name().find(prefix) != 0) {
       RCLCPP_ERROR(
               logger_,
-              "Incorrect parameter update request for param \"%s\"", p.get_name().c_str());
+              "[%s] Incorrect parameter update request for param \"%s\"", name_.c_str(), p.get_name().c_str());
       return;
     }
     size_t pos = p.get_name().find(".", prefix.size());
@@ -500,7 +507,8 @@ class CrazyflieROS
 
     RCLCPP_INFO(
         logger_,
-        "Update parameter \"%s.%s\" to %s",
+        "[%s] Update parameter \"%s.%s\" to %s",
+        name_.c_str(),
         group.c_str(),
         name.c_str(),
         p.value_to_string().c_str());
@@ -537,7 +545,7 @@ class CrazyflieROS
         break;
       }
     } else {
-      RCLCPP_ERROR(logger_, "Could not find param %s/%s", group.c_str(), name.c_str());
+      RCLCPP_ERROR(logger_, "[%s] Could not find param %s/%s", name_.c_str(), group.c_str(), name.c_str());
     }
   }
 
@@ -596,7 +604,7 @@ class CrazyflieROS
     if (pos != std::string::npos)
     {
       message_buffer_[pos] = 0;
-      RCLCPP_INFO(logger_, "%s", message_buffer_.c_str());
+      RCLCPP_INFO(logger_, "[%s] %s", name_.c_str(), message_buffer_.c_str());
       message_buffer_.erase(0, pos + 1);
     }
   }
@@ -604,14 +612,15 @@ class CrazyflieROS
   void emergency(const std::shared_ptr<Empty::Request> request,
             std::shared_ptr<Empty::Response> response)
   {
-    RCLCPP_INFO(logger_, "emergency()");
+    RCLCPP_INFO(logger_, "[%s] emergency()", name_.c_str());
     cf_.emergencyStop();
   }
 
   void start_trajectory(const std::shared_ptr<StartTrajectory::Request> request,
                         std::shared_ptr<StartTrajectory::Response> response)
   {
-    RCLCPP_INFO(logger_, "start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[%s] start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)",
+      name_.c_str(),
       request->trajectory_id,
       request->timescale,
       request->reversed,
@@ -627,7 +636,8 @@ class CrazyflieROS
   void takeoff(const std::shared_ptr<Takeoff::Request> request,
                std::shared_ptr<Takeoff::Response> response)
   {
-    RCLCPP_INFO(logger_, "takeoff(height=%f m, duration=%f s, group_mask=%d)", 
+    RCLCPP_INFO(logger_, "[%s] takeoff(height=%f m, duration=%f s, group_mask=%d)", 
+                name_.c_str(),
                 request->height,
                 rclcpp::Duration(request->duration).seconds(),
                 request->group_mask);
@@ -637,7 +647,8 @@ class CrazyflieROS
   void land(const std::shared_ptr<Land::Request> request,
             std::shared_ptr<Land::Response> response)
   {
-    RCLCPP_INFO(logger_, "land(height=%f m, duration=%f s, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[%s] land(height=%f m, duration=%f s, group_mask=%d)",
+                name_.c_str(),
                 request->height,
                 rclcpp::Duration(request->duration).seconds(),
                 request->group_mask);
@@ -647,7 +658,8 @@ class CrazyflieROS
   void go_to(const std::shared_ptr<GoTo::Request> request,
              std::shared_ptr<GoTo::Response> response)
   {
-    RCLCPP_INFO(logger_, "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)",
+                name_.c_str(),
                 request->goal.x, request->goal.y, request->goal.z, request->yaw,
                 rclcpp::Duration(request->duration).seconds(),
                 request->relative,
@@ -660,7 +672,8 @@ class CrazyflieROS
   void upload_trajectory(const std::shared_ptr<UploadTrajectory::Request> request,
                         std::shared_ptr<UploadTrajectory::Response> response)
   {
-    RCLCPP_INFO(logger_, "upload_trajectory(id=%d, offset=%d)",
+    RCLCPP_INFO(logger_, "[%s] upload_trajectory(id=%d, offset=%d)",
+                name_.c_str(),
                 request->trajectory_id,
                 request->piece_offset);
 
@@ -672,7 +685,7 @@ class CrazyflieROS
           || request->pieces[i].poly_z.size() != 8
           || request->pieces[i].poly_yaw.size() != 8)
       {
-        RCLCPP_FATAL(logger_, "Wrong number of pieces!");
+        RCLCPP_FATAL(logger_, "[%s] Wrong number of pieces!", name_.c_str());
         return;
       }
       pieces[i].duration = rclcpp::Duration(request->pieces[i].duration).seconds();
@@ -690,7 +703,8 @@ class CrazyflieROS
   void notify_setpoints_stop(const std::shared_ptr<NotifySetpointsStop::Request> request,
                          std::shared_ptr<NotifySetpointsStop::Response> response)
   {
-    RCLCPP_INFO(logger_, "notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[%s] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)",
+                name_.c_str(),
                 request->remain_valid_millisecs,
                 request->group_mask);
 
@@ -847,13 +861,13 @@ class CrazyflieROS
     auto now = std::chrono::steady_clock::now();
     std::chrono::duration<double> elapsed = now - last_on_latency_;
     if (elapsed.count() > 1.0 / warning_freq_) {
-      RCLCPP_WARN(logger_, "last latency update: %f s", elapsed.count());
+      RCLCPP_WARN(logger_, "[%s] last latency update: %f s", name_.c_str(), elapsed.count());
     }
 
     auto stats = cf_.connectionStatsDelta();
     float ack_rate = stats.sent_count / stats.ack_count;
     if (ack_rate < min_ack_rate_) {
-      RCLCPP_WARN(logger_, "Ack rate: %.1f %%", ack_rate * 100);
+      RCLCPP_WARN(logger_, "[%s] Ack rate: %.1f %%", name_.c_str(), ack_rate * 100);
     }
 
     if (publish_stats_) {
@@ -876,7 +890,7 @@ class CrazyflieROS
   void on_latency(uint64_t latency_in_us)
   {
     if (latency_in_us / 1000.0 > max_latency_) {
-      RCLCPP_WARN(logger_, "Latency: %.1f ms", latency_in_us / 1000.0);
+      RCLCPP_WARN(logger_, "[%s] Latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0);
     }
     last_on_latency_ = std::chrono::steady_clock::now();
   }
@@ -946,7 +960,7 @@ class CrazyflieServer : public rclcpp::Node
 public:
   CrazyflieServer()
       : Node("crazyflie_server")
-      , logger_(rclcpp::get_logger("all"))
+      , logger_(get_logger())
   {
     // Create callback groups (each group can run in a separate thread)
     callback_group_mocap_ = this->create_callback_group(
@@ -1063,7 +1077,7 @@ class CrazyflieServer : public rclcpp::Node
           uint8_t id = parameter_overrides.at("robots." + name + ".id").get<uint8_t>();
           update_name_to_id_map(name, id);
         } else {
-          RCLCPP_INFO(logger_, "Unknown connection type %s", constr.c_str());
+          RCLCPP_INFO(logger_, "[all] Unknown connection type %s", constr.c_str());
         }
       }
     }
@@ -1090,7 +1104,7 @@ class CrazyflieServer : public rclcpp::Node
   void emergency(const std::shared_ptr<Empty::Request> request,
             std::shared_ptr<Empty::Response> response)
   {
-    RCLCPP_INFO(logger_, "emergency()");
+    RCLCPP_INFO(logger_, "[all] emergency()");
     for (int i = 0; i < broadcasts_num_repeats_; ++i)
     {
       for (auto &bc : broadcaster_) {
@@ -1104,7 +1118,7 @@ class CrazyflieServer : public rclcpp::Node
   void start_trajectory(const std::shared_ptr<StartTrajectory::Request> request,
             std::shared_ptr<StartTrajectory::Response> response)
   {
-    RCLCPP_INFO(logger_, "start_trajectory(id=%d, timescale=%f, reversed=%d, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[all] start_trajectory(id=%d, timescale=%f, reversed=%d, group_mask=%d)",
                 request->trajectory_id,
                 request->timescale,
                 request->reversed,
@@ -1124,7 +1138,7 @@ class CrazyflieServer : public rclcpp::Node
   void takeoff(const std::shared_ptr<Takeoff::Request> request,
                         std::shared_ptr<Takeoff::Response> response)
   {
-    RCLCPP_INFO(logger_, "takeoff(height=%f m, duration=%f s, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[all] takeoff(height=%f m, duration=%f s, group_mask=%d)",
                 request->height,
                 rclcpp::Duration(request->duration).seconds(),
                 request->group_mask);
@@ -1140,7 +1154,7 @@ class CrazyflieServer : public rclcpp::Node
   void land(const std::shared_ptr<Land::Request> request,
            std::shared_ptr<Land::Response> response)
   {
-    RCLCPP_INFO(logger_, "land(height=%f m, duration=%f s, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[all] land(height=%f m, duration=%f s, group_mask=%d)",
                 request->height,
                 rclcpp::Duration(request->duration).seconds(),
                 request->group_mask);
@@ -1156,7 +1170,7 @@ class CrazyflieServer : public rclcpp::Node
   void go_to(const std::shared_ptr<GoTo::Request> request,
             std::shared_ptr<GoTo::Response> response)
   {
-    RCLCPP_INFO(logger_, "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[all] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, group_mask=%d)",
                 request->goal.x, request->goal.y, request->goal.z, request->yaw,
                 rclcpp::Duration(request->duration).seconds(),
                 request->group_mask);
@@ -1174,7 +1188,7 @@ class CrazyflieServer : public rclcpp::Node
   void notify_setpoints_stop(const std::shared_ptr<NotifySetpointsStop::Request> request,
                          std::shared_ptr<NotifySetpointsStop::Response> response)
   {
-    RCLCPP_INFO(logger_, "notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)",
+    RCLCPP_INFO(logger_, "[all] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)",
                 request->remain_valid_millisecs,
                 request->group_mask);
 
@@ -1281,7 +1295,7 @@ class CrazyflieServer : public rclcpp::Node
 
           RCLCPP_INFO(
               logger_,
-              "Update parameter \"%s.%s\" to %s",
+              "[all] Update parameter \"%s.%s\" to %s",
               group.c_str(),
               name.c_str(),
               p.value_to_string().c_str());
@@ -1351,11 +1365,11 @@ class CrazyflieServer : public rclcpp::Node
       mean_rate /= (mocap_data_received_timepoints_.size() - 1);
 
       if (num_rates_wrong > 0) {
-        RCLCPP_WARN(logger_, "Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate);
+        RCLCPP_WARN(logger_, "[all] Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate);
       }
     } else if (mocap_enabled_) {
       // b) warn if no data was received
-      RCLCPP_WARN(logger_, "Motion capture did not receive data!");
+      RCLCPP_WARN(logger_, "[all] Motion capture did not receive data!");
     }
 
     mocap_data_received_timepoints_.clear();
@@ -1404,7 +1418,7 @@ class CrazyflieServer : public rclcpp::Node
   {
     const auto iter = name_to_id_.find(name);
     if (iter != name_to_id_.end()) {
-      RCLCPP_WARN(logger_, "At least two objects with the same id (%d, %s, %s)", id, name.c_str(), iter->first.c_str());
+      RCLCPP_WARN(logger_, "[all] At least two objects with the same id (%d, %s, %s)", id, name.c_str(), iter->first.c_str());
     } else {
       name_to_id_.insert(std::make_pair(name, id));
     }
@@ -1427,6 +1441,8 @@ class CrazyflieServer : public rclcpp::Node
 
     std::map<std::string, std::unique_ptr<CrazyflieROS>> crazyflies_;
 
+
+
     // broadcastUri -> broadcast object
     std::map<std::string, std::unique_ptr<CrazyflieBroadcaster>> broadcaster_;
 
diff --git a/crazyflie/urdf/cf2_assembly.stl b/crazyflie/urdf/cf2_assembly.stl
new file mode 100644
index 000000000..02772d31e
Binary files /dev/null and b/crazyflie/urdf/cf2_assembly.stl differ
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/crazyflie_interfaces/msg/Status.msg b/crazyflie_interfaces/msg/Status.msg
index bebd42a71..d7c2f2352 100644
--- a/crazyflie_interfaces/msg/Status.msg
+++ b/crazyflie_interfaces/msg/Status.msg
@@ -1,6 +1,6 @@
 
 # Constants
-uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 0
+uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 1
 uint16 SUPERVISOR_INFO_IS_ARMED     = 2
 uint16 SUPERVISOR_INFO_AUTO_ARM     = 4
 uint16 SUPERVISOR_INFO_CAN_FLY      = 8
diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_server.py b/crazyflie_sim/crazyflie_sim/crazyflie_server.py
index 13bf4ae2e..93dd698da 100755
--- a/crazyflie_sim/crazyflie_sim/crazyflie_server.py
+++ b/crazyflie_sim/crazyflie_sim/crazyflie_server.py
@@ -210,7 +210,7 @@ def _param_to_dict(self, param_ros):
         return tree
 
     def _emergency_callback(self, request, response, name='all'):
-        self.get_logger().info('emergency not yet implemented')
+        self.get_logger().info(f'[{name}] emergency not yet implemented')
 
         return response
 
@@ -219,9 +219,9 @@ def _takeoff_callback(self, request, response, name='all'):
         duration = float(request.duration.sec) + \
             float(request.duration.nanosec / 1e9)
         self.get_logger().info(
-            f'takeoff(height={request.height} m,'
+            f'[{name}] takeoff(height={request.height} m,'
             + f'duration={duration} s,'
-            + f'group_mask={request.group_mask}) {name}'
+            + f'group_mask={request.group_mask})'
         )
         cfs = self.cfs if name == 'all' else {name: self.cfs[name]}
         for _, cf in cfs.items():
@@ -234,7 +234,7 @@ def _land_callback(self, request, response, name='all'):
         duration = float(request.duration.sec) + \
             float(request.duration.nanosec / 1e9)
         self.get_logger().info(
-            f'land(height={request.height} m,'
+            f'[{name}] land(height={request.height} m,'
             + f'duration={duration} s,'
             + f'group_mask={request.group_mask})'
         )
@@ -250,8 +250,13 @@ def _go_to_callback(self, request, response, name='all'):
             float(request.duration.nanosec / 1e9)
 
         self.get_logger().info(
-            'go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)'
+            """[%s] go_to(position=%f,%f,%f m,
+             yaw=%f rad,
+             duration=%f s,
+             relative=%d,
+             group_mask=%d)"""
             % (
+                name,
                 request.goal.x,
                 request.goal.y,
                 request.goal.z,
@@ -269,11 +274,11 @@ def _go_to_callback(self, request, response, name='all'):
         return response
 
     def _notify_setpoints_stop_callback(self, request, response, name='all'):
-        self.get_logger().info('Notify setpoint stop not yet implemented')
+        self.get_logger().info(f'[{name}] Notify setpoint stop not yet implemented')
         return response
 
     def _upload_trajectory_callback(self, request, response, name='all'):
-        self.get_logger().info('Upload trajectory(id=%d)' % (request.trajectory_id))
+        self.get_logger().info('[%s] Upload trajectory(id=%d)' % (name, request.trajectory_id))
 
         cfs = self.cfs if name == 'all' else {name: self.cfs[name]}
         for _, cf in cfs.items():
@@ -297,8 +302,9 @@ def _upload_trajectory_callback(self, request, response, name='all'):
 
     def _start_trajectory_callback(self, request, response, name='all'):
         self.get_logger().info(
-            'start_trajectory(id=%d, timescale=%f, reverse=%d, relative=%d, group_mask=%d)'
+            '[%s] start_trajectory(id=%d, timescale=%f, reverse=%d, relative=%d, group_mask=%d)'
             % (
+                name,
                 request.trajectory_id,
                 request.timescale,
                 request.reversed,
diff --git a/docs2/faq.rst b/docs2/faq.rst
index 8270b9343..7422be127 100644
--- a/docs2/faq.rst
+++ b/docs2/faq.rst
@@ -45,9 +45,9 @@ Crazyswarm2 was forked from Crazyswarm. However, there is also heavy re-design o
   In Crazyswarm1, a simple visualization of the setpoints for high-level Python scripts is supported. There is no support for simulation of ROS code that does not use the high-level Python scripts and no support for physics-based simulation.
   In contrast, Crazyswarm2 implements the simulation as an alternative backend. This will support multiple physics/visualization backends (optionally with physics and aerodynamic interaction).
 
-- **Support of Distributed Swarm Monitoring (Planned).**
+- **Support of Distributed Swarm Monitoring.**
   In Crazyswarm1, a common swarm monitoring tool is the chooser.py (to enable/disable CFs, check the battery voltage etc.). However, this tool was not functioning while the swarm is operational.
-  In contrast, Crazyswarm2 will allow common swarm monitoring tasks without restarting ROS nodes or launching additional tools. 
+  In contrast, Crazyswarm2 allows common swarm monitoring tasks without restarting ROS nodes or launching additional tools.
 
 
 How is Crazyswarm2 different from Bitcraze's cflib?
diff --git a/docs2/overview.rst b/docs2/overview.rst
index 014cfc93d..cc413fe1f 100644
--- a/docs2/overview.rst
+++ b/docs2/overview.rst
@@ -67,6 +67,8 @@ Support functionality with backends
 +---------------------+---------+-----------+---------+
 | - default: odom     | No      | Yes       | No      |
 +---------------------+---------+-----------+---------+
+| - default: status   | Yes     | Yes       | No      |
++---------------------+---------+-----------+---------+
 | - custom            | Yes     | Yes       | No      |
 +---------------------+---------+-----------+---------+
 | - Add/Remove Srv    | No      | Yes       | No      |
diff --git a/docs2/usage.rst b/docs2/usage.rst
index d8278958b..52117f0ae 100644
--- a/docs2/usage.rst
+++ b/docs2/usage.rst
@@ -155,3 +155,11 @@ You may run the script multiple times or different scripts while leaving the ser
 
     [terminal1]$ ros2 launch crazyflie launch.py
     [terminal2]$ ros2 run crazyflie_examples hello_world
+
+Swarm Management
+----------------
+
+The launch file will also start a swarm management tool that is a ROS node and web-based GUI.
+In the upper pane is the location of the drone visualized in a 3D window, similar to rviz.
+In the lower pane, the status as well as log messages are visible (tabbed per drone).
+In the future, we are planning to add support for rebooting and other actions.
\ No newline at end of file
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)