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