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

camera_streaming/camera_stream.py

@@ -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,

camera_streaming/camera_subscriber.py

@@ -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

camera_streaming/mission.py

@@ -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
 
 

launch/robot.launch.py

+#! /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
+        ]
+    )

setup.py

+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',
         ],
     },