Merge branch 'main' into issue419

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,11 +72,13 @@ 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:
         frequency: 10 # Hz
+      status:
+        frequency: 1 # Hz
     #custom_topics:
     #  topic_name1:
     #    frequency: 10 # Hz

crazyflie/config/server.yaml

@@ -7,6 +7,8 @@
       communication:
         max_unicast_latency: 10.0 # ms
         min_unicast_ack_rate: 0.9
+        min_unicast_receive_rate: 0.9 # requires status topic to be enabled
+        min_broadcast_receive_rate: 0.9 # requires status topic to be enabled
         publish_stats: false
     firmware_params:
       query_all_values_on_connect: False

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