Merge branch 'master' of gitlab.aau.at:aau-nav/development/camera_streaming

camera_streaming/camera_capture.py

+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+import cv2
+from datetime import datetime
+
+
+def camera_image_capture(callback):
+    CAM_FPS = 10
+
+    cam = cv2.VideoCapture(0)
+    cam.set(cv2.CAP_PROP_FPS, CAM_FPS)
+    cam.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+    cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+    cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
+    cam.read()
+
+    while True:
+        for i in range(16):
+            start = datetime.now()
+            cam.grab()
+            end = datetime.now()
+            if (end - start).total_seconds() >= 1 / CAM_FPS:
+                # buffer is empty
+                break
+        frame = cam.retrieve()[1]
+        if frame is None:
+            raise Exception('Image cannot be retrieved. Disconnected camera?')
+        # frame = cv2.resize(frame, (320, 240))
+        jpg = bytearray(cv2.imencode('.jpg', frame)[1])
+        callback(frame, jpg)

camera_streaming/camera_stream.py

@@ -10,7 +10,6 @@ from rclpy.executors import SingleThreadedExecutor
 from std_msgs.msg import Header
 from sensor_msgs.msg import CompressedImage
 
-import cv2
 import numpy as np
 from builtin_interfaces.msg import Time
 
@@ -23,7 +22,6 @@ from rclpy.qos import (
     QoSHistoryPolicy,
     QoSReliabilityPolicy,
 )
-from rclpy.qos import QoSPresetProfiles
 
 
 from mission_manager.client import MissionExecutor, MissionClient
@@ -32,11 +30,11 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
 from infdist.optimization.simplesim import (
     create_msgnum_constraint_violations,
 )
-import serial
 
 from sniffing import sniffer
 from mission import generate_mission_context
 from detect import detect_red
+from camera_capture import camera_image_capture
 
 CUSTOM_QOS = QoSProfile(
     history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
@@ -68,15 +66,8 @@ class CameraStream(Node, MissionExecutor):
             qos_profile=CUSTOM_QOS,
             # qos_profile=QoSPresetProfiles.get_from_short_key('SYSTEM_DEFAULT')
         )
-        self.timer_period = 0.33
+        self.timer_period = 0.25
         self.timer = None
-        self.cam_fps = 90
-        self.cam = cv2.VideoCapture(0)
-        self.cam.set(cv2.CAP_PROP_FPS, self.cam_fps)
-        self.cam.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
-        self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
-        self.cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
-        self.cam.read()
         self.offset = self.timer_period/8 * int(self.hosts[self.hostname])
         self.t_start = datetime.now().timestamp()
         self.t_end = 30
@@ -86,6 +77,57 @@ class CameraStream(Node, MissionExecutor):
             args=(self.camera_received_callback,)
         )
         self.sniffer.start()
+        self.camera_thread = threading.Thread(
+            target=camera_image_capture,
+            args=(self.new_camera_image,)
+        )
+        self.camera_thread.daemon = True
+        self.camera_thread.start()
+
+    def new_camera_image(self, frame, jpg):
+        self.prepared_message_timestamp = self.get_clock().now()
+        mask = detect_red(frame)
+        self.prepared_message_red = np.round(np.sum(mask) / mask.size / 255, 5)
+        self.prepared_message_jpg = jpg
+
+    def get_prepared_message(self):
+        now = self.prepared_message_timestamp
+        red = self.prepared_message_red
+        jpg = self.prepared_message_jpg
+
+        msg = CompressedImage()
+        nanosec, sec = map(
+            int,
+            (now.nanoseconds % 10**9, now.nanoseconds/10**9)
+        )
+        metadata = {
+            'stamp': (sec + nanosec * 1e-9) - self.t_start,
+            'red': red,
+            'sender': self.hostname
+        }
+        metadata_str = json.dumps(metadata)
+        frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str
+        header = Header(
+            frame_id=frame_id,
+            stamp=Time(sec=sec, nanosec=nanosec)
+        )
+        msg.header = header
+        msg.format = 'jpeg'
+        msg.data = jpg
+        native_message = ROS2NativeMessage(
+            msg=msg,
+            sender=self.hostname[-1],
+            receivers=self.hosts,
+            data_type_name=f'v{self.hostname[-1]}',
+            data={
+                'red': red,
+            },
+            publisher=self.publisher,
+            stamp=(
+                msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+            ) - self.t_start
+        )
+        return native_message
 
     def start_mission_at_timestamp(self, timestamp):
         super().start_mission_at_timestamp(timestamp)
@@ -105,7 +147,9 @@ class CameraStream(Node, MissionExecutor):
             ),
             now_func=lambda: time.time() - self.t_start,
             agents={'base_station': lambda t: set()},
+            future_messages_num=10,
             window_size=0.1,
+            simulations_num=200,
             constraints={
                 # create_throughput_constraint_violations(
                 #     throughput,
@@ -155,61 +199,20 @@ class CameraStream(Node, MissionExecutor):
         self.timer_period = 1 / float(params.get('fps', 2))
 
     def timer_callback(self):
-        if abs((datetime.now().timestamp() - self.t_start - 0.01)%self.timer_period - self.offset) > 0.02:
-            print("skipping")
+        if abs(
+            (datetime.now().timestamp() - self.t_start - 0.01)
+            % self.timer_period - self.offset
+        ) > 0.02:
+            print("WARN: timer callback called out of sync, skipping")
             return
-        all_start = datetime.now().timestamp()
-        for i in range(16):
-            start = datetime.now()
-            self.cam.grab()
-            end = datetime.now()
-            if (end - start).total_seconds() >= 1 / self.cam_fps:  # buffer is empty
-                break
-        frame = self.cam.retrieve()[1]
-        now = self.get_clock().now()
-        # frame = cv2.resize(frame, (320, 240))
-        mask = detect_red(frame)
-        red = np.round(np.sum(mask) / mask.size / 255, 5)
-        jpg = bytearray(cv2.imencode('.jpg', frame)[1])
-        msg = CompressedImage()
-        nanosec, sec = map(
-            int,
-            (now.nanoseconds % 10**9, now.nanoseconds/10**9)
-        )
-        metadata = {
-            'stamp': (sec + nanosec * 1e-9) - self.t_start,
-            'red': red,
-            'sender': self.hostname
-        }
-        metadata_str = json.dumps(metadata)
-        frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str
-        header = Header(
-            frame_id=frame_id,
-            stamp=Time(sec=sec, nanosec=nanosec)
-        )
-        msg.header = header
-        msg.format = 'jpeg'
-        msg.data = jpg
-        native_message = ROS2NativeMessage(
-            msg=msg,
-            sender=self.hostname[-1],
-            receivers=self.hosts,
-            data_type_name=f'v{self.hostname[-1]}',
-            data={
-                'red': red,
-            },
-            publisher=self.publisher,
-            stamp=(
-                msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
-            ) - self.t_start
-        )
-        msg_generated = datetime.now().timestamp()
+
         try:
-            self.agent.generated(native_message)
+            start_time = datetime.now()
+            self.agent.generated(self.get_prepared_message())
+            end_time = datetime.now()
+            print(f"Duration: {end_time-start_time}")
         except Exception as e:
             print(e)
-        end = datetime.now().timestamp()
-        print(f'generated: {msg_generated-all_start}, agent: {end-msg_generated}')
 
     def move(self):
         mission = {

camera_streaming/mission.py

@@ -3,7 +3,7 @@
 from infdist.optimization.models import MissionContext, InformationType
 from infdist.optimization.aggregations import AggregationMostRecent
 
-from .utility import PeriodicTypeForecast, UtilityVideo
+from camera_streaming.utility import PeriodicTypeForecast, UtilityVideo
 # from infdist.optimization.message_forecast import PeriodicTypeForecast