modified display

camera_streaming/camera_subscriber.py

@@ -55,10 +55,14 @@ class CameraSubscriber(Node, MissionExecutor):
             for key in self.hosts.keys()
         }
         self.fps = {
-            key: 0
+            key: 0.
             for key in self.hosts.keys()
         }
         self.red = {
+            key: 0.
+            for key in self.hosts.keys()
+        }
+        self.frames = {
             key: 0
             for key in self.hosts.keys()
         }
@@ -67,14 +71,17 @@ class CameraSubscriber(Node, MissionExecutor):
             key: datetime.now()
             for key in self.hosts.keys()
         }
+        self.saved = 0
+        self.last_saved = datetime.now().timestamp()
 
-        self.resolution = (240, 320, 3)
+        self.resolution = (480, 640, 3)
         hosts_num = len(self.hosts.keys())
         self.shape = (
             int(np.floor(np.sqrt(hosts_num))),
             int(np.ceil(hosts_num / int(np.sqrt(hosts_num))))
         )
         self.images = np.zeros(self.shape + self.resolution)
+        self.display_streams()
 
         self.camera_subscriptions = [
             self.create_subscription(
@@ -92,10 +99,27 @@ class CameraSubscriber(Node, MissionExecutor):
     def start_mission_at_timestamp(self, timestamp):
         super().start_mission_at_timestamp(timestamp)
         self.t_start = timestamp.timestamp()
+        self.delays = {
+            key: 0
+            for key in self.hosts.keys()
+        }
+        self.fps = {
+            key: 0.
+            for key in self.hosts.keys()
+        }
+        self.red = {
+            key: 0.
+            for key in self.hosts.keys()
+        }
         self.frames = {
             key: 0
             for key in self.hosts.keys()
         }
+        self.last_frame = {
+            key: datetime.now()
+            for key in self.hosts.keys()
+        }
+        self.saved = 0
 
     def end_mission_at_timestamp(self, timestamp):
         super().end_mission_at_timestamp(timestamp)
@@ -116,6 +140,7 @@ class CameraSubscriber(Node, MissionExecutor):
 
     def start_mission(self, timestamp):
         self.get_logger().info('Mission started')
+        self.last_saved = datetime.now().timestamp()
 
     def end_mission(self, timestamp):
         self.get_logger().info('Mission ended')
@@ -154,7 +179,7 @@ class CameraSubscriber(Node, MissionExecutor):
         )
 
         self.titles = np.array([
-            f'{host}, ' +
+            f'{host[-3:]}, ' +
             (f'{self.fps[host]:.02f} FPS, '
              if self.fps[host] else 'OFFLINE, ') +
             f'latency: {self.delays[host]:.02f}, '
@@ -167,8 +192,12 @@ class CameraSubscriber(Node, MissionExecutor):
             self.red[host]
             for host in self.hosts.keys()
         ]).reshape(self.shape)
+        fps = np.array([
+            self.fps[host]
+            for host in self.hosts.keys()
+        ]).reshape(self.shape)
 
-        title_pad = 15
+        title_pad = 20
         pad_top = pad + title_pad
 
         frame = np.zeros(
@@ -190,10 +219,34 @@ class CameraSubscriber(Node, MissionExecutor):
                     frame,
                     self.titles[r, c],
                     (x0, y0-title_pad//4),
-                    cv2.FONT_HERSHEY_COMPLEX,
-                    .35, (255, 255, 255)
+                    cv2.FONT_HERSHEY_DUPLEX,
+                    .5, (1, 1, 1)
+                )
+                frame = cv2.putText(
+                    frame,
+                    f'{fps[r,c]} FPS',
+                    (x0, y0+sp_height - 10),
+                    cv2.FONT_HERSHEY_DUPLEX,
+                    3, (0, 0, 0),
+                    thickness=7,
+                )
+                frame = cv2.putText(
+                    frame,
+                    f'{fps[r,c]} FPS',
+                    (x0, y0+sp_height - 10),
+                    cv2.FONT_HERSHEY_DUPLEX,
+                    3, ((1 - 2 * red[r,c]), (1 - 3 * red[r,c]), 1),
+                    thickness=3,
                 )
 
+        now = datetime.now().timestamp()
+        if now - self.last_saved > 5:
+            print('saving')
+            frame2 = frame.copy() * 255
+            cv2.imwrite(f'/home/agniewek/longTemp/infdist/img{datetime.now()}.jpg', frame2)
+            self.saved += 1
+            self.last_saved = now
+        frame = cv2.resize(frame, (frame.shape[1]//2, frame.shape[0]//2))
         cv2.imshow(win_name, frame)
         cv2.waitKey(1)