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

WIP

parent 129b9fe6
......@@ -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
......
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