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,6 +32,9 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
 from infdist.optimization.simplesim import (
     create_msgnum_constraint_violations,
 )
+import serial
+
+from sniffing import sniffer
 from mission import generate_mission_context
 from detect import detect_red
 
@@ -66,7 +68,7 @@ class CameraStream(Node, MissionExecutor):
             qos_profile=CUSTOM_QOS,
             # qos_profile=QoSPresetProfiles.get_from_short_key('SYSTEM_DEFAULT')
         )
-        self.timer_period = 0.25
+        self.timer_period = 0.33
         self.timer = None
         self.cam_fps = 90
         self.cam = cv2.VideoCapture(0)
@@ -78,6 +80,7 @@ class CameraStream(Node, MissionExecutor):
         self.offset = self.timer_period/8 * int(self.hosts[self.hostname])
         self.t_start = datetime.now().timestamp()
         self.t_end = 30
+        # self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
         self.sniffer = threading.Thread(
             target=sniffer.sniff,
             args=(self.camera_received_callback,)
@@ -111,7 +114,7 @@ class CameraStream(Node, MissionExecutor):
                 # )
                 'msg_num': create_msgnum_constraint_violations(
                     msgnum=3,
-                    timeslot=0.5,
+                    timeslot=0.4,
                 )
 
             },
@@ -137,6 +140,10 @@ class CameraStream(Node, MissionExecutor):
         self.get_logger().info('Mission started')
         time.sleep(self.offset)
         self.timer = self.create_timer(self.timer_period, self.timer_callback)
+        # self.movement = threading.Thread(
+        #     target=self.move
+        # )
+        # self.movement.start()
 
     def end_mission(self, timestamp):
         self.get_logger().info('Mission ended')
@@ -148,9 +155,10 @@ class CameraStream(Node, MissionExecutor):
         self.timer_period = 1 / float(params.get('fps', 2))
 
     def timer_callback(self):
-        if abs((datetime.now().timestamp() - self.t_start)%self.timer_period - self.offset) > 0.02:
+        if abs((datetime.now().timestamp() - self.t_start - 0.01)%self.timer_period - self.offset) > 0.02:
             print("skipping")
             return
+        all_start = datetime.now().timestamp()
         for i in range(16):
             start = datetime.now()
             self.cam.grab()
@@ -159,7 +167,7 @@ class CameraStream(Node, MissionExecutor):
                 break
         frame = self.cam.retrieve()[1]
         now = self.get_clock().now()
-        frame = cv2.resize(frame, (320, 240))
+        # frame = cv2.resize(frame, (320, 240))
         mask = detect_red(frame)
         red = np.round(np.sum(mask) / mask.size / 255, 5)
         jpg = bytearray(cv2.imencode('.jpg', frame)[1])
@@ -195,10 +203,48 @@ class CameraStream(Node, MissionExecutor):
                 msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
             ) - self.t_start
         )
+        msg_generated = datetime.now().timestamp()
         try:
             self.agent.generated(native_message)
         except Exception as e:
             print(e)
+        end = datetime.now().timestamp()
+        print(f'generated: {msg_generated-all_start}, agent: {end-msg_generated}')
+
+    def move(self):
+        mission = {
+            'fdrones_b01': 'wwwlaaawwlssldddl',
+            'fdrones_b02': 'ssswwwwssswwwwwww',
+            'fdrones_b03': 'wllllaldlalllaaal',
+            'fdrones_b05': 'wwllllwwllllddlld',
+            'fdrones_b06': 'wwwwllllsssssllll',
+            'fdrones_b07': 'aalddlaalwwlaaldd',
+            'fdrones_b08': 'wwwwwsssswwwsssww',
+            'fdrones_b09': 'wwellallallallall',
+        }
+
+        KEYS = {
+            "w": (1., 0., 'front'),
+            "s": (-1., 0., 'back'),
+            "a": (0., 1., 'left'),
+            "d": (0., -1., 'right'),
+            "e": (1., -1., 'front right'),
+            "q": (1., 1., 'front left'),
+            "l": (0, 0, 'sleeping'),
+            "n": (0, 0, 'noop'),
+        }
+
+        repeats = 10
+
+        for c in mission[self.hostname]:
+            for i in range(repeats):
+                linear, angular, text = KEYS[c]
+
+                self.ser.write("V {} {}\n".format(
+                    int(angular * 0.5 * 100),
+                    int(linear * 0.2 * 100)
+                ).encode('ascii'))
+                time.sleep(0.2)
 
 
 def main():

camera_streaming/mission.py

@@ -22,7 +22,7 @@ def generate_mission_context(agents, data_types, t_end, T):
                     'receivers': ['base_station'],
                     'T': T,
                     'sender': f'{agent}',
-                    'initial_t_start': T/8 * i,
+                    'initial_t_start': T/8 * i + 0.01,
                 },
                 weight=1
             )

camera_streaming/utility.py

@@ -92,7 +92,7 @@ class UtilityVideo(Utility):
         t_rcv = m.t_rcv if m.t_rcv else m.t_gen
         t_s = max(t_rcv, t_start)
 
-        t_l = t_rcv + 0.25
+        t_l = t_rcv + 0.33
 
         red = m.data.red
         log_part = 0
@@ -104,20 +104,22 @@ class UtilityVideo(Utility):
             # logarithmic part
             t = t_l
             t0 = t_l - t_rcv
-            t1 = t_end - t_rcv
-            fmax = 4
-            f1 = 2
-            S = 2
+            fmax = 3
+            f1 = 1.5
+            S = 0.2
             base = 2
             ln2 = math.log(base)
             k = S / (1 - f1 / (2 * fmax * ln2))
             m = 1 + math.log(1 / fmax, base) * k
+            t1 = t0 + 1
+            t1 = min(t_end - t_rcv, base**(1/k)/fmax)
             log_part = max(0, (
                 (t1 * (1 - math.log(t1)) - t0 * (1 - math.log(t0))) / ln2 * k +
                 (t1 - t0) * m
             ))
+            # print("######log#### ", log_part)
         else:
             t = t_end
-        result = (t - t_s + log_part) * (1 + 1 * red)
+        result = (t - t_s + log_part) * (0.5 + 5 * red)
         # print('!!!!', t_start, t_end, t_s, t_l, result)
         return result

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