WIP

camera_streaming/camera_subscriber.py

@@ -10,6 +10,7 @@ import cv2
 import numpy as np
 from datetime import datetime
 import time
+import json
 
 from rclpy.qos import (
     QoSProfile,
@@ -40,14 +41,14 @@ class CameraSubscriber(Node, MissionExecutor):
     def __init__(self):
         super().__init__('camera_subscriber')
         self.hosts = {
-            'fdrones_t90': 0,
-            'fdrones_t91': 1,
-            'fdrones_t92': 2,
-            'fdrones_t93': 3,
-            'fdrones_t94': 4,
-            'fdrones_t95': 5,
-            'fdrones_t96': 6,
-            'fdrones_t97': 7,
+            'fdrones_b01': 0,
+            'fdrones_b02': 1,
+            'fdrones_b03': 2,
+            'fdrones_b05': 3,
+            'fdrones_b06': 4,
+            'fdrones_b07': 5,
+            'fdrones_b08': 6,
+            'fdrones_b09': 7,
         }
         self.delays = {
             key: 0
@@ -57,6 +58,10 @@ class CameraSubscriber(Node, MissionExecutor):
             key: 0
             for key in self.hosts.keys()
         }
+        self.red = {
+            key: 0
+            for key in self.hosts.keys()
+        }
 
         self.last_frame = {
             key: datetime.now()
@@ -87,6 +92,10 @@ class CameraSubscriber(Node, MissionExecutor):
     def start_mission_at_timestamp(self, timestamp):
         super().start_mission_at_timestamp(timestamp)
         self.t_start = timestamp.timestamp()
+        self.frames = {
+            key: 0
+            for key in self.hosts.keys()
+        }
 
     def end_mission_at_timestamp(self, timestamp):
         super().end_mission_at_timestamp(timestamp)
@@ -95,8 +104,8 @@ class CameraSubscriber(Node, MissionExecutor):
             ident='base_station',
             net=ROS2Network(),
             messages_context=generate_mission_context(
-                agents=self.hosts.keys(),
-                data_types=['camera'],
+                agents=[k[-1] for k in self.hosts.keys()],
+                data_types=['v'],
                 t_end=self.t_end,
                 T=0.5,
             ),
@@ -149,11 +158,17 @@ class CameraSubscriber(Node, MissionExecutor):
             (f'{self.fps[host]:.02f} FPS, '
              if self.fps[host] else 'OFFLINE, ') +
             f'latency: {self.delays[host]:.02f}, '
-            f'red: {0}'
+            f'red: {self.red[host]:.03f}, '
+            f'{self.frames[host]}'
             for host in self.hosts.keys()
         ]).reshape(self.shape)
 
-        title_pad = 30
+        red = np.array([
+            self.red[host]
+            for host in self.hosts.keys()
+        ]).reshape(self.shape)
+
+        title_pad = 15
         pad_top = pad + title_pad
 
         frame = np.zeros(
@@ -163,9 +178,12 @@ class CameraSubscriber(Node, MissionExecutor):
         for r in range(rows):
             for c in range(cols):
                 img = self.images[r, c]
+                img = cv2.copyMakeBorder(img, pad//2, pad//2, pad//2, pad//2,
+                                         cv2.BORDER_CONSTANT,
+                                         value=(0, 0, 2 * red[r, c]))
                 h, w, _ = img.shape
-                y0 = r * (sp_height+pad_top) + pad_top//2
-                x0 = c * (sp_width+pad) + pad//2
+                y0 = r * (sp_height+pad_top) + title_pad
+                x0 = c * (sp_width+pad) # + pad//2
                 frame[y0:y0+h, x0:x0+w] = img
 
                 frame = cv2.putText(
@@ -184,34 +202,42 @@ class CameraSubscriber(Node, MissionExecutor):
             now = datetime.now()
             img = msg.data
             npimg = np.frombuffer(img, dtype=np.uint8)
-            img = cv2.imdecode(npimg, cv2.IMREAD_COLOR)
-            mask = detect_red(img)
-            mask = cv2.bitwise_not(mask)
-            img = cv2.bitwise_and(img, img, mask=mask)/256
+            img = cv2.imdecode(npimg, cv2.IMREAD_COLOR)/255
+            # mask = detect_red(img)
+            # mask = cv2.bitwise_not(mask)
+            # img = cv2.bitwise_and(img, img, mask=mask)/256
             host_id = self.hosts[host]
             place = (
                 int(host_id / self.shape[1]), int(host_id % self.shape[1])
             )
             self.images[place[0], place[1]] = img
-            self.display_streams()
             t_gen = datetime.fromtimestamp(
                 msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
             )
-            print("HEADER", msg.header.frame_id)
+            # print("HEADER", msg.header.frame_id)
+            msg_metadata = msg.header.frame_id
+            metadata_dict = json.loads(msg_metadata[msg_metadata.index('{'):])
             self.fps[host] = round(
-                1/(t_gen - self.last_frame[host]).total_seconds(),
+                (
+                    0.8 * self.fps[host] +
+                    0.2 * 1/(t_gen - self.last_frame[host]).total_seconds()
+                ),
                 2
             )
+            self.red[host] = metadata_dict['red']
+            self.frames[host] += 1
             self.last_frame[host] = t_gen
-            self.delays[host] = (now - t_gen).total_seconds()
+            self.display_streams()
+            self.delays[host] = (
+                self.delays[host] + (now - t_gen).total_seconds()
+            ) / 2
             native_message = ROS2NativeMessage(
                 msg=msg,
-                sender=host,
+                sender=host[-1],
                 receivers=['base_station'],
-                data_type_name=f'camera_{host}',
+                data_type_name=f'v{host[-1]}',
                 data={
-                    'battery_level': 1,
-                    'max_depl_rate': 0.1,
+                    'red': metadata_dict['red'],
                 },
                 stamp=(
                     msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9