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

WIP

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