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

Merge branch 'master' of gitlab.aau.at:aau-nav/development/camera_streaming

parents 2113e608 904df5a0
......@@ -25,7 +25,6 @@ from rclpy.qos import (
)
from rclpy.qos import QoSPresetProfiles
from sniffing import sniffer
from mission_manager.client import MissionExecutor, MissionClient
from infdist.optimization.agent import EstimatingAgent
......@@ -33,8 +32,10 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
from infdist.optimization.simplesim import (
create_msgnum_constraint_violations,
)
from mission import generate_mission_context
from detect import detect_red
from .sniffing import sniffer
from .mission import generate_mission_context
from .detect import detect_red
CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
......
......@@ -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
......
......@@ -3,7 +3,7 @@
from infdist.optimization.models import MissionContext, InformationType
from infdist.optimization.aggregations import AggregationMostRecent
from utility import PeriodicTypeForecast, UtilityVideo
from .utility import PeriodicTypeForecast, UtilityVideo
# from infdist.optimization.message_forecast import PeriodicTypeForecast
......
#! /usr/bin/env python
# -*- coding: utf-8 -*-
"""Launch sync and swarm simulation"""
import launch
from launch_ros.substitutions import ExecutableInPackage
import socket
def generate_launch_description():
hostname = socket.gethostname()
interface = [
ExecutableInPackage(
package='balboa_lowlevel',
executable='balboa_interface'
),
'__ns:=/{}/balboa'.format(hostname),
]
camera_streaming = [
ExecutableInPackage(
package='camera_streaming',
executable='camera_streaming'),
]
executables_list = [interface, camera_streaming]
processes = [
launch.actions.ExecuteProcess(
cmd=executable, output='screen')
for executable in executables_list
]
return launch.LaunchDescription(
processes + [
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=process,
on_exit=[launch.actions.EmitEvent(
event=launch.events.Shutdown())],
))
for process in processes
]
)
import glob
import os
from setuptools import find_packages
from setuptools import setup
......@@ -11,8 +14,10 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch',
glob.glob(os.path.join('launch', '*.launch.py'))),
],
install_requires=['setuptools'],
install_requires=['launch', 'setuptools'],
zip_safe=True,
author='Michal Barcis',
author_email='michal.barcis@aau.at',
......@@ -30,7 +35,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
'camera_streaming = camera_streaming.camera_streaming:main',
'camera_streaming = camera_streaming.camera_stream:main',
'camera_subscriber = camera_streaming.camera_subscriber:main',
],
},
......
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