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