add flight test script

.github/workflows/systemtests.yml

@@ -30,6 +30,10 @@ jobs:
           cd ros2_ws
           colcon build --symlink-install
 
+      - name: Flight test
+        run: |
+          python3 src/crazyswarm2/systemtests/newsub.py
+
       # - name: build and test ROS 2
       #   uses: ros-tooling/action-ros-ci@v0.3
       #   with:

systemtests/mcap_read_write.py

@@ -0,0 +1,53 @@
+from pathlib import Path
+from rclpy.serialization import deserialize_message
+from rosidl_runtime_py.utilities import get_message
+from std_msgs.msg import String
+import rosbag2_py
+import csv
+
+class McapHandler:
+    def __init__(self):
+        pass
+
+    def read_messages(self, input_bag: str):
+        reader = rosbag2_py.SequentialReader()
+        reader.open(
+            rosbag2_py.StorageOptions(uri=input_bag, storage_id="mcap"),
+            rosbag2_py.ConverterOptions(
+                input_serialization_format="cdr", output_serialization_format="cdr"
+            ),
+        )
+        topic_types = reader.get_all_topics_and_types()
+
+        def typename(topic_name):
+            for topic_type in topic_types:
+                if topic_type.name == topic_name:
+                    return topic_type.type
+            raise ValueError(f"topic {topic_name} not in bag")
+
+        while reader.has_next():
+            topic, data, timestamp = reader.read_next()
+            msg_type = get_message(typename(topic))
+            msg = deserialize_message(data, msg_type)
+            yield topic, msg, timestamp
+        del reader
+
+    def write_mcap_to_csv(self, inputbag:str, outputfile:str):
+        try:
+            print("Translating .mcap to .csv")
+            f = open(outputfile, 'w+')
+            writer = csv.writer(f)
+            for topic, msg, timestamp in self.read_messages(inputbag):
+                writer.writerow([timestamp, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z])
+            f.close()
+        except FileNotFoundError:
+            print(f"McapHandler : file {outputfile} not found")
+            exit(1)
+
+
+
+
+if __name__ == "__main__":
+    yo =  McapHandler()
+    # # yo.write_mcap_to_csv('Home/julien/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.mcap','Home/julien/ros2_ws/bagfiles/bag_06_11_2023-17:11:23/bag_06_11_2023-17:11:23_0.csv')
+    yo.write_mcap_to_csv('/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.mcap','/home/julien/Desktop/IMRC/IMRC-shared/bags/multibag/multibag_0.csv')

systemtests/newsub.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3   
+from subprocess import Popen
+import time
+import os
+import signal
+from mcap_read_write import McapHandler
+from datetime import datetime
+from pathlib import Path
+from plotter_class import Plotter
+
+
+if __name__ == "__main__":
+
+    command = "source install/setup.bash && ros2 launch crazyflie launch.py"
+    launch_crazyswarm = Popen(command, shell=True, stderr=True, 
+                            stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)   
+    time.sleep(3)
+    bagname = 'bag_' + datetime.now().strftime('%d_%m_%Y-%H:%M:%S')
+    command = f"mkdir bagfiles && cd bagfiles && ros2 bag record -s mcap -o {bagname} /tf"
+    start_rosbag =  Popen(command, shell=True, stderr=True, 
+                            stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid) 
+    command = "ros2 run crazyflie_examples figure8"
+    start_test = Popen(command, shell=True, stderr=True, 
+                       stdout=True, text=True, executable="/bin/bash", preexec_fn=os.setsid)    
+    time.sleep(30)
+    os.killpg(os.getpgid(start_test.pid), signal.SIGTERM)  #kill start_test process and all of its child processes
+    os.killpg(os.getpgid(start_rosbag.pid), signal.SIGTERM) #kill rosbag
+    os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM)   #kill crazyswarm
+
+    print("Flight finished")
+
+    #test done, now we compare the ideal and real trajectories
+
+    #create a file that translates the .mcap format of the rosbag to .csv
+    writer = McapHandler()
+    #path = str(Path(__file__).parent /"bags/tfbag.mcap")
+
+    ### attention le bagname est different du bagfile par un _0 
+    inputbag = "bagfiles/" + bagname + '/' + bagname + '_0' + '.mcap'
+    output_csv = "bagfiles/" + bagname + '/' + bagname + '_0' + '.csv'
+
+    writer.write_mcap_to_csv(inputbag, output_csv)
+
+    output_pdf = "bagfiles/" + bagname + '/' + 'Results_'+ datetime.now().strftime('%d_%m_%Y-%H:%M:%S')
+
+    rosbag_csv = output_csv
+
+    test_file = "src/crazyswarm2/crazyflie_examples/crazyflie_examples/data/figure8.csv"
+    paul = Plotter()
+    paul.create_figures(test_file, rosbag_csv, output_pdf)
+
+    exit()