Merge branch 'main' into feature_com_warnings

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

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

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.     ]

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:

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
 

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