Initial commit

.gitignore

@@ -0,0 +1,2 @@
+__pycache__
+build

client/streamer/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ai_sail_stream_receiver)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  sensor_msgs
+)
+
+catkin_python_setup()

client/streamer/REAME.md

@@ -0,0 +1,3 @@
+# AI-Deck Streamer [ROS] client
+
+...

client/streamer/launch/ai_deck.launch

@@ -0,0 +1,11 @@
+<launch>
+
+  <arg name="pipe" default=""/>
+  <arg name="host" default="192.168.201.40"/>
+
+  <node required="true" pkg="ai_sail_stream_receiver" name="himax" type="ros_node.py" output="screen">
+    <param name="pipe" value="$(arg pipe)"/>
+    <param name="host" value="$(arg host)"/>
+  </node>
+
+</launch>

client/streamer/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package>
+  <name>ai_sail_stream_receiver</name>
+  <version>0.0.1</version>
+  <description>A minimal driver that republish to ROS image streamed via sockets or image pipes by the AI-deck</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <maintainer email="[email protected]">Jerome Guzzi</maintainer>
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/drone_arena</url> -->
+
+  <author email="[email protected]">Jerome Guzzi</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>rospy</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>

client/streamer/script/client.py

@@ -0,0 +1,187 @@
+# This is an extension of the example provided by bitcraze:
+# - added raw format
+# - added color
+# - added pipe as transport
+
+import fcntl
+import os
+import socket
+import struct
+import threading
+from datetime import datetime as dt
+from typing import Iterator, List, Tuple, Any
+import signal
+
+import cv2
+import numpy as np
+
+SOCK_REC_SIZE = 256
+F_SETPIPE_SZ = 1031
+
+
+class RepeatTimer(threading.Timer):
+    def run(self) -> None:
+        while not self.finished.wait(self.interval):  # type: ignore
+            self.function(*self.args, **self.kwargs)  # type: ignore
+
+
+class Client:
+    color: int
+    encoding: str = ""
+    shape: Tuple[int, int] = (0, 0)
+    fps: float = 0.0
+    frame: np.ndarray
+    ts: List[dt]
+    receive: Iterator[bytes]
+    ok: bool
+
+    @property
+    def size(self) -> int:
+        return self.shape[0] * self.shape[1]
+
+    def run(self) -> Iterator[np.ndarray]:
+        has_received_header = False
+        has_received_shape = False
+        self.encoding = ""
+        bs = b''
+        self.color = 0
+        for rx in self.receive:
+            bs += rx
+            if not has_received_header:
+                start_raw = bs.find(b"?IMG")
+                start_jpeg = bs.find(b"\xff\xd8")
+                if start_raw >= 0 and (start_jpeg < 0 or start_jpeg > start_raw):
+                    self.encoding = "raw"
+                    bs = bs[start_raw + 4:]
+                    buffer = b''
+                    has_received_header = True
+                    has_received_shape = False
+                if start_jpeg >= 0 and (start_raw < 0 or start_raw > start_jpeg):
+                    self.encoding = "jpeg"
+                    has_received_header = True
+                    bs = bs[start_jpeg:]
+                    buffer = b''
+                if not has_received_header:
+                    bs = b''
+                    continue
+            if self.encoding == 'raw':
+                if has_received_header and not has_received_shape:
+                    buffer += bs
+                    if len(buffer) >= 6:
+                        w, h, self.color = struct.unpack('HHH', buffer[:6])
+                        self.shape = (h, w)
+                        buffer = buffer[6:]
+                        has_received_shape = True
+                        bs = b''
+                elif has_received_shape:
+                    end = bs.find(b"!IMG")
+                    if end >= 0:
+                        buffer += bs[0: end]
+                        bs = bs[end:]
+                        if(len(buffer) == self.size):
+                            frame = np.frombuffer(buffer, dtype=np.uint8)
+                            frame = frame.reshape(self.shape)
+                            if self.color == 1:
+                                frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_BG2BGR)
+                            elif self.color == 2:
+                                frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_GB2BGR)
+                            elif self.color == 3:
+                                frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_GR2BGR)
+                            elif self.color == 4:
+                                frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_RG2BGR)
+                            self.ts.append(dt.now())
+                            yield frame
+                        else:
+                            print(f"Expected {self.size} pixels ... got {len(buffer)}")
+                            print(buffer[:8])
+                        has_received_header = False
+                        has_received_shape = False
+                    else:
+                        buffer += bs
+                        bs = b''
+            else:
+                if has_received_header:
+                    end = bs.find(b"\xff\xd9")
+                    if end >= 0:
+                        buffer += bs[0: end]
+                        # print('buffer len', len(buffer))
+                        data = np.frombuffer(buffer, dtype=np.uint8)
+                        frame = cv2.imdecode(data, cv2.IMREAD_GRAYSCALE)
+                        if frame is not None:
+                            self.shape = frame.shape
+                            self.ts.append(dt.now())
+                            yield frame
+                        bs = bs[end:]
+                        has_received_header = False
+                    else:
+                        buffer += bs
+                        bs = b''
+
+    def update_fps(self) -> None:
+        if(len(self.ts) == 0):
+            self.fps = 0.0
+        elif len(self.ts) == 1:
+            self.fps = 1.0
+        else:
+            self.fps = len(self.ts) / (self.ts[-1] - self.ts[0]).total_seconds()
+        self.ts = self.ts[-2:]
+        if self.fps or self.shape:
+            print(f'\r{self.shape[1]} x {self.shape[0]} {self.encoding} ({self.color}) @ {self.fps:.1f} fps', end='')
+
+    def read_socket(self, host: str, port: int = 5000) -> Iterator[bytes]:
+        while self.ok:
+            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as c:
+                print(f"Connecting to socket on {host}:{port}...")
+                # c.connect((host, port))
+                c.settimeout(5)
+                try:
+                    c.connect((host, port))
+                except (socket.timeout, OSError, ConnectionRefusedError):
+                    continue
+                print("Socket connected, ready to get images")
+                first = True
+                while self.ok:
+                    try:
+                        yield c.recv(SOCK_REC_SIZE)
+                        if first:
+                            print("Getting images")
+                            first = False
+                    except socket.timeout:
+                        break
+                print("Close socket")
+
+    def read_pipe(self, path: str) -> Iterator[bytes]:
+        print(f"Connecting to pipe at {path} ...")
+        if not os.path.exists(path):
+            os.mkfifo(path)
+        with open(path, 'rb') as c:
+            fcntl.fcntl(c, F_SETPIPE_SZ, 1000000)
+            print(f"Connected to pipe at {path}, ready to get images")
+            first = True
+            while 1:
+                # os.read(fd, n)
+                # Read at most n bytes from file descriptor fd.
+                yield c.read(SOCK_REC_SIZE)
+                if first:
+                    print("Getting images")
+                    first = False
+
+    def exit_gracefully(self, signum: Any, frame: Any) -> None:
+        self.ok = False
+
+    def __init__(self, pipe: str = '', host: str = '', port: int = 5000,
+                 keep_track_of_fps: bool = True) -> None:
+        self.fps = 0.0
+        self.ts = []
+        self.ok = True
+        signal.signal(signal.SIGINT, self.exit_gracefully)
+        signal.signal(signal.SIGTERM, self.exit_gracefully)
+        if keep_track_of_fps:
+            RepeatTimer(1.0, self.update_fps).start()
+        if pipe:
+            self.receive = self.read_pipe(pipe)
+        elif host:
+            self.receive = self.read_socket(host, port)
+        else:
+            self.receive = iter([])
+            print("No source")

client/streamer/script/opencv_node.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+# This is an extension of the example provided by bitcraze:
+# - added raw format
+# - added color
+# - added pipe as transport
+
+import argparse
+import cv2
+from client import Client
+import numpy as np
+
+
+def display(frame: np.ndarray) -> None:
+    cv2.imshow('camera stream', frame)
+    cv2.waitKey(10)
+
+
+def main() -> None:
+    # Args for setting IP/port of AI-deck. Default settings are for when
+    # AI-deck is in AP mode.
+    parser = argparse.ArgumentParser(description='Connect to AI-deck streamer')
+    parser.add_argument("-host", default="192.168.4.1", metavar="host", help="AI-deck host")
+    parser.add_argument("-port", type=int, default='5000', metavar="port", help="AI-deck port")
+    parser.add_argument("-pipe", type=str, default='', metavar="pipe", help="Pipe path")
+    args = parser.parse_args()
+    client = Client(host=args.host, port=args.port, pipe=args.pipe)
+    for frame in client.run():
+        display(frame)
+
+
+if __name__ == '__main__':
+    main()

client/streamer/script/ros_node.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# This is an extension of the example provided by bitcraze:
+# - added raw format
+# - added color
+# - added pipe as transport
+
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+from client import Client
+
+
+def main() -> None:
+    rospy.init_node('himax_driver', anonymous=True)
+    image_pub = rospy.Publisher("image_raw", Image, queue_size=1)
+    bridge = CvBridge()
+    pipe = rospy.get_param('~pipe', '')
+    host = rospy.get_param('~host', '')
+    port = rospy.get_param('~port', 5000)
+    client = Client(host=host, port=port, pipe=pipe, keep_track_of_fps=False)
+    for seq, frame in enumerate(client.run()):
+        msg = bridge.cv2_to_imgmsg(frame)
+        msg.header.stamp = rospy.Time.now()
+        msg.header.seq = seq
+        image_pub.publish(msg)
+        rospy.sleep(0)
+
+
+if __name__ == '__main__':
+    main()

crazyflie/README.md

@@ -0,0 +1,6 @@
+# Crazyflie AI-Deck driver
+
+- list of improvements
+(mostly just uart protocol)
+
+explain logging