Commit 72481988 authored by Barciś, Agata's avatar Barciś, Agata
Browse files

modified display

parent 6dd217a8
...@@ -55,10 +55,14 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -55,10 +55,14 @@ class CameraSubscriber(Node, MissionExecutor):
for key in self.hosts.keys() for key in self.hosts.keys()
} }
self.fps = { self.fps = {
key: 0 key: 0.
for key in self.hosts.keys() for key in self.hosts.keys()
} }
self.red = { self.red = {
key: 0.
for key in self.hosts.keys()
}
self.frames = {
key: 0 key: 0
for key in self.hosts.keys() for key in self.hosts.keys()
} }
...@@ -67,14 +71,17 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -67,14 +71,17 @@ class CameraSubscriber(Node, MissionExecutor):
key: datetime.now() key: datetime.now()
for key in self.hosts.keys() 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()) hosts_num = len(self.hosts.keys())
self.shape = ( self.shape = (
int(np.floor(np.sqrt(hosts_num))), int(np.floor(np.sqrt(hosts_num))),
int(np.ceil(hosts_num / int(np.sqrt(hosts_num)))) int(np.ceil(hosts_num / int(np.sqrt(hosts_num))))
) )
self.images = np.zeros(self.shape + self.resolution) self.images = np.zeros(self.shape + self.resolution)
self.display_streams()
self.camera_subscriptions = [ self.camera_subscriptions = [
self.create_subscription( self.create_subscription(
...@@ -92,10 +99,27 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -92,10 +99,27 @@ class CameraSubscriber(Node, MissionExecutor):
def start_mission_at_timestamp(self, timestamp): def start_mission_at_timestamp(self, timestamp):
super().start_mission_at_timestamp(timestamp) super().start_mission_at_timestamp(timestamp)
self.t_start = 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 = { self.frames = {
key: 0 key: 0
for key in self.hosts.keys() 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): def end_mission_at_timestamp(self, timestamp):
super().end_mission_at_timestamp(timestamp) super().end_mission_at_timestamp(timestamp)
...@@ -116,6 +140,7 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -116,6 +140,7 @@ class CameraSubscriber(Node, MissionExecutor):
def start_mission(self, timestamp): def start_mission(self, timestamp):
self.get_logger().info('Mission started') self.get_logger().info('Mission started')
self.last_saved = datetime.now().timestamp()
def end_mission(self, timestamp): def end_mission(self, timestamp):
self.get_logger().info('Mission ended') self.get_logger().info('Mission ended')
...@@ -154,7 +179,7 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -154,7 +179,7 @@ class CameraSubscriber(Node, MissionExecutor):
) )
self.titles = np.array([ self.titles = np.array([
f'{host}, ' + f'{host[-3:]}, ' +
(f'{self.fps[host]:.02f} FPS, ' (f'{self.fps[host]:.02f} FPS, '
if self.fps[host] else 'OFFLINE, ') + if self.fps[host] else 'OFFLINE, ') +
f'latency: {self.delays[host]:.02f}, ' f'latency: {self.delays[host]:.02f}, '
...@@ -167,8 +192,12 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -167,8 +192,12 @@ class CameraSubscriber(Node, MissionExecutor):
self.red[host] self.red[host]
for host in self.hosts.keys() for host in self.hosts.keys()
]).reshape(self.shape) ]).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 pad_top = pad + title_pad
frame = np.zeros( frame = np.zeros(
...@@ -190,10 +219,34 @@ class CameraSubscriber(Node, MissionExecutor): ...@@ -190,10 +219,34 @@ class CameraSubscriber(Node, MissionExecutor):
frame, frame,
self.titles[r, c], self.titles[r, c],
(x0, y0-title_pad//4), (x0, y0-title_pad//4),
cv2.FONT_HERSHEY_COMPLEX, cv2.FONT_HERSHEY_DUPLEX,
.35, (255, 255, 255) .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.imshow(win_name, frame)
cv2.waitKey(1) cv2.waitKey(1)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment