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

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

parents 2113e608 904df5a0
...@@ -25,7 +25,6 @@ from rclpy.qos import ( ...@@ -25,7 +25,6 @@ from rclpy.qos import (
) )
from rclpy.qos import QoSPresetProfiles from rclpy.qos import QoSPresetProfiles
from sniffing import sniffer
from mission_manager.client import MissionExecutor, MissionClient from mission_manager.client import MissionExecutor, MissionClient
from infdist.optimization.agent import EstimatingAgent from infdist.optimization.agent import EstimatingAgent
...@@ -33,8 +32,10 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage ...@@ -33,8 +32,10 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
from infdist.optimization.simplesim import ( from infdist.optimization.simplesim import (
create_msgnum_constraint_violations, 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( CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
......
...@@ -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
......
...@@ -3,7 +3,7 @@ ...@@ -3,7 +3,7 @@
from infdist.optimization.models import MissionContext, InformationType from infdist.optimization.models import MissionContext, InformationType
from infdist.optimization.aggregations import AggregationMostRecent from infdist.optimization.aggregations import AggregationMostRecent
from utility import PeriodicTypeForecast, UtilityVideo from .utility import PeriodicTypeForecast, UtilityVideo
# from infdist.optimization.message_forecast import PeriodicTypeForecast # 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 find_packages
from setuptools import setup from setuptools import setup
...@@ -11,8 +14,10 @@ setup( ...@@ -11,8 +14,10 @@ setup(
('share/ament_index/resource_index/packages', ('share/ament_index/resource_index/packages',
['resource/' + package_name]), ['resource/' + package_name]),
('share/' + package_name, ['package.xml']), ('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, zip_safe=True,
author='Michal Barcis', author='Michal Barcis',
author_email='michal.barcis@aau.at', author_email='michal.barcis@aau.at',
...@@ -30,7 +35,7 @@ setup( ...@@ -30,7 +35,7 @@ setup(
tests_require=['pytest'], tests_require=['pytest'],
entry_points={ entry_points={
'console_scripts': [ 'console_scripts': [
'camera_streaming = camera_streaming.camera_streaming:main', 'camera_streaming = camera_streaming.camera_stream:main',
'camera_subscriber = camera_streaming.camera_subscriber: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