Commit 81a23166 authored by Barcis, Michal's avatar Barcis, Michal
Browse files

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

parents 58af91a2 7ba0d5e0
...@@ -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,6 +32,9 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage ...@@ -33,6 +32,9 @@ 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,
) )
import serial
from sniffing import sniffer
from mission import generate_mission_context from mission import generate_mission_context
from detect import detect_red from detect import detect_red
...@@ -66,7 +68,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -66,7 +68,7 @@ class CameraStream(Node, MissionExecutor):
qos_profile=CUSTOM_QOS, qos_profile=CUSTOM_QOS,
# qos_profile=QoSPresetProfiles.get_from_short_key('SYSTEM_DEFAULT') # qos_profile=QoSPresetProfiles.get_from_short_key('SYSTEM_DEFAULT')
) )
self.timer_period = 0.25 self.timer_period = 0.33
self.timer = None self.timer = None
self.cam_fps = 90 self.cam_fps = 90
self.cam = cv2.VideoCapture(0) self.cam = cv2.VideoCapture(0)
...@@ -78,6 +80,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -78,6 +80,7 @@ class CameraStream(Node, MissionExecutor):
self.offset = self.timer_period/8 * int(self.hosts[self.hostname]) self.offset = self.timer_period/8 * int(self.hosts[self.hostname])
self.t_start = datetime.now().timestamp() self.t_start = datetime.now().timestamp()
self.t_end = 30 self.t_end = 30
# self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
self.sniffer = threading.Thread( self.sniffer = threading.Thread(
target=sniffer.sniff, target=sniffer.sniff,
args=(self.camera_received_callback,) args=(self.camera_received_callback,)
...@@ -111,7 +114,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -111,7 +114,7 @@ class CameraStream(Node, MissionExecutor):
# ) # )
'msg_num': create_msgnum_constraint_violations( 'msg_num': create_msgnum_constraint_violations(
msgnum=3, msgnum=3,
timeslot=0.5, timeslot=0.4,
) )
}, },
...@@ -137,6 +140,10 @@ class CameraStream(Node, MissionExecutor): ...@@ -137,6 +140,10 @@ class CameraStream(Node, MissionExecutor):
self.get_logger().info('Mission started') self.get_logger().info('Mission started')
time.sleep(self.offset) time.sleep(self.offset)
self.timer = self.create_timer(self.timer_period, self.timer_callback) 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): def end_mission(self, timestamp):
self.get_logger().info('Mission ended') self.get_logger().info('Mission ended')
...@@ -148,9 +155,10 @@ class CameraStream(Node, MissionExecutor): ...@@ -148,9 +155,10 @@ class CameraStream(Node, MissionExecutor):
self.timer_period = 1 / float(params.get('fps', 2)) self.timer_period = 1 / float(params.get('fps', 2))
def timer_callback(self): 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") print("skipping")
return return
all_start = datetime.now().timestamp()
for i in range(16): for i in range(16):
start = datetime.now() start = datetime.now()
self.cam.grab() self.cam.grab()
...@@ -159,7 +167,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -159,7 +167,7 @@ class CameraStream(Node, MissionExecutor):
break break
frame = self.cam.retrieve()[1] frame = self.cam.retrieve()[1]
now = self.get_clock().now() now = self.get_clock().now()
frame = cv2.resize(frame, (320, 240)) # frame = cv2.resize(frame, (320, 240))
mask = detect_red(frame) mask = detect_red(frame)
red = np.round(np.sum(mask) / mask.size / 255, 5) red = np.round(np.sum(mask) / mask.size / 255, 5)
jpg = bytearray(cv2.imencode('.jpg', frame)[1]) jpg = bytearray(cv2.imencode('.jpg', frame)[1])
...@@ -195,10 +203,48 @@ class CameraStream(Node, MissionExecutor): ...@@ -195,10 +203,48 @@ class CameraStream(Node, MissionExecutor):
msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
) - self.t_start ) - self.t_start
) )
msg_generated = datetime.now().timestamp()
try: try:
self.agent.generated(native_message) self.agent.generated(native_message)
except Exception as e: except Exception as e:
print(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(): def main():
......
...@@ -22,7 +22,7 @@ def generate_mission_context(agents, data_types, t_end, T): ...@@ -22,7 +22,7 @@ def generate_mission_context(agents, data_types, t_end, T):
'receivers': ['base_station'], 'receivers': ['base_station'],
'T': T, 'T': T,
'sender': f'{agent}', 'sender': f'{agent}',
'initial_t_start': T/8 * i, 'initial_t_start': T/8 * i + 0.01,
}, },
weight=1 weight=1
) )
......
...@@ -92,7 +92,7 @@ class UtilityVideo(Utility): ...@@ -92,7 +92,7 @@ class UtilityVideo(Utility):
t_rcv = m.t_rcv if m.t_rcv else m.t_gen t_rcv = m.t_rcv if m.t_rcv else m.t_gen
t_s = max(t_rcv, t_start) t_s = max(t_rcv, t_start)
t_l = t_rcv + 0.25 t_l = t_rcv + 0.33
red = m.data.red red = m.data.red
log_part = 0 log_part = 0
...@@ -104,20 +104,22 @@ class UtilityVideo(Utility): ...@@ -104,20 +104,22 @@ class UtilityVideo(Utility):
# logarithmic part # logarithmic part
t = t_l t = t_l
t0 = t_l - t_rcv t0 = t_l - t_rcv
t1 = t_end - t_rcv fmax = 3
fmax = 4 f1 = 1.5
f1 = 2 S = 0.2
S = 2
base = 2 base = 2
ln2 = math.log(base) ln2 = math.log(base)
k = S / (1 - f1 / (2 * fmax * ln2)) k = S / (1 - f1 / (2 * fmax * ln2))
m = 1 + math.log(1 / fmax, base) * k 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, ( log_part = max(0, (
(t1 * (1 - math.log(t1)) - t0 * (1 - math.log(t0))) / ln2 * k + (t1 * (1 - math.log(t1)) - t0 * (1 - math.log(t0))) / ln2 * k +
(t1 - t0) * m (t1 - t0) * m
)) ))
# print("######log#### ", log_part)
else: else:
t = t_end 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) # print('!!!!', t_start, t_end, t_s, t_l, result)
return result return result
#! /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