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

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

parents 72481988 81a23166
......@@ -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():
......
......@@ -18,14 +18,11 @@ from rclpy.qos import (
QoSHistoryPolicy,
QoSReliabilityPolicy,
)
from rclpy.qos import QoSPresetProfiles
from mission_manager.client import MissionExecutor, MissionClient
from infdist.optimization.agent import FullCommAgent
from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
from mission import generate_mission_context
from detect import detect_red
from .mission import generate_mission_context
CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
......@@ -256,7 +253,6 @@ class CameraSubscriber(Node, MissionExecutor):
img = msg.data
npimg = np.frombuffer(img, dtype=np.uint8)
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]
......
......@@ -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
......@@ -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
)
......
......@@ -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
#! /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