add system tests CI draft

.github/workflows/systemtests.yml

@@ -0,0 +1,57 @@
+name: System Tests
+
+on:
+  push:
+    branches: [ "feature_systemtests" ]
+  # manual trigger
+  workflow_dispatch:
+
+jobs:
+  build:
+    runs-on: self-hosted
+    steps:
+      - name: Create workspace
+        run: |
+          pwd
+          rm -rf ros2_ws
+          mkdir -p ros2_ws/src
+      - name: Checkout motion capture package
+        run: |
+          cd ros2_ws/src
+          ls motion_capture_tracking || git clone --branch ros2 --recursive https://github.com/IMRCLab/motion_capture_tracking.git
+      - name: Checkout Crazyswarm2
+        uses: actions/checkout@v4
+        with:
+          path: ros2_ws/src/crazyswarm2
+          submodules: 'recursive'
+      - name: Build workspace
+        run: |
+          source /opt/ros/humble/setup.bash
+          cd ros2_ws
+          colcon build --symlink-install
+
+      - name: Flight test
+        run: |
+          cd ros2_ws
+          source /opt/ros/humble/setup.bash
+          . install/local_setup.bash
+          export ROS_LOCALHOST_ONLY=1
+          python3 src/crazyswarm2/systemtests/newsub.py
+
+      - name: Upload files
+        uses: actions/upload-artifact@v3
+        with:
+          name: bagfiles
+          path: ros2_ws/bagfiles/
+
+      # - name: build and test ROS 2
+      #   uses: ros-tooling/action-ros-ci@v0.3
+      #   with:
+      #     package-name: |
+      #       crazyflie
+      #       crazyflie_examples
+      #       crazyflie_interfaces
+      #       crazyflie_py
+      #       crazyflie_sim
+      #     target-ros2-distro: humble
+      #     vcs-repo-file-url: rosinstall

crazyflie/config/crazyflies.yaml

@@ -2,7 +2,7 @@
 robots:
   cf231:
     enabled: true
-    uri: radio://0/80/2M/E7E7E7E7E7
+    uri: radio://0/60/2M/E7E7E7E7E7
     initial_position: [0, 0, 0]
     type: cf21  # see robot_types
     # firmware_params:

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 = "../crazyflie_examples/crazyflie_examples/data/figure8.csv"
+    paul = Plotter()
+    paul.create_figures(test_file, rosbag_csv, output_pdf)
+
+    exit()