Commit 3c8cd9dc authored by Barcis, Michal's avatar Barcis, Michal

added separate camera thread

parent bcba197a
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import cv2
from datetime import datetime
def camera_image_capture(callback):
CAM_FPS = 10
cam = cv2.VideoCapture(0)
cam.set(cv2.CAP_PROP_FPS, CAM_FPS)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
cam.read()
while True:
for i in range(16):
start = datetime.now()
cam.grab()
end = datetime.now()
if (end - start).total_seconds() >= 1 / CAM_FPS:
# buffer is empty
break
frame = cam.retrieve()[1]
if frame is None:
raise Exception('Image cannot be retrieved. Disconnected camera?')
# frame = cv2.resize(frame, (320, 240))
jpg = bytearray(cv2.imencode('.jpg', frame)[1])
callback(frame, jpg)
......@@ -10,7 +10,6 @@ from rclpy.executors import SingleThreadedExecutor
from std_msgs.msg import Header
from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np
from builtin_interfaces.msg import Time
......@@ -23,7 +22,6 @@ from rclpy.qos import (
QoSHistoryPolicy,
QoSReliabilityPolicy,
)
from rclpy.qos import QoSPresetProfiles
from mission_manager.client import MissionExecutor, MissionClient
......@@ -32,11 +30,11 @@ 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
from camera_capture import camera_image_capture
CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
......@@ -68,15 +66,8 @@ class CameraStream(Node, MissionExecutor):
qos_profile=CUSTOM_QOS,
# qos_profile=QoSPresetProfiles.get_from_short_key('SYSTEM_DEFAULT')
)
self.timer_period = 0.33
self.timer_period = 0.25
self.timer = None
self.cam_fps = 90
self.cam = cv2.VideoCapture(0)
self.cam.set(cv2.CAP_PROP_FPS, self.cam_fps)
self.cam.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
self.cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
self.cam.read()
self.offset = self.timer_period/8 * int(self.hosts[self.hostname])
self.t_start = datetime.now().timestamp()
self.t_end = 30
......@@ -86,6 +77,57 @@ class CameraStream(Node, MissionExecutor):
args=(self.camera_received_callback,)
)
self.sniffer.start()
self.camera_thread = threading.Thread(
target=camera_image_capture,
args=(self.new_camera_image,)
)
self.camera_thread.daemon = True
self.camera_thread.start()
def new_camera_image(self, frame, jpg):
self.prepared_message_timestamp = self.get_clock().now()
mask = detect_red(frame)
self.prepared_message_red = np.round(np.sum(mask) / mask.size / 255, 5)
self.prepared_message_jpg = jpg
def get_prepared_message(self):
now = self.prepared_message_timestamp
red = self.prepared_message_red
jpg = self.prepared_message_jpg
msg = CompressedImage()
nanosec, sec = map(
int,
(now.nanoseconds % 10**9, now.nanoseconds/10**9)
)
metadata = {
'stamp': (sec + nanosec * 1e-9) - self.t_start,
'red': red,
'sender': self.hostname
}
metadata_str = json.dumps(metadata)
frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str
header = Header(
frame_id=frame_id,
stamp=Time(sec=sec, nanosec=nanosec)
)
msg.header = header
msg.format = 'jpeg'
msg.data = jpg
native_message = ROS2NativeMessage(
msg=msg,
sender=self.hostname[-1],
receivers=self.hosts,
data_type_name=f'v{self.hostname[-1]}',
data={
'red': red,
},
publisher=self.publisher,
stamp=(
msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
) - self.t_start
)
return native_message
def start_mission_at_timestamp(self, timestamp):
super().start_mission_at_timestamp(timestamp)
......@@ -105,7 +147,9 @@ class CameraStream(Node, MissionExecutor):
),
now_func=lambda: time.time() - self.t_start,
agents={'base_station': lambda t: set()},
future_messages_num=10,
window_size=0.1,
simulations_num=200,
constraints={
# create_throughput_constraint_violations(
# throughput,
......@@ -155,61 +199,20 @@ 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 - 0.01)%self.timer_period - self.offset) > 0.02:
print("skipping")
if abs(
(datetime.now().timestamp() - self.t_start - 0.01)
% self.timer_period - self.offset
) > 0.02:
print("WARN: timer callback called out of sync, skipping")
return
all_start = datetime.now().timestamp()
for i in range(16):
start = datetime.now()
self.cam.grab()
end = datetime.now()
if (end - start).total_seconds() >= 1 / self.cam_fps: # buffer is empty
break
frame = self.cam.retrieve()[1]
now = self.get_clock().now()
# 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])
msg = CompressedImage()
nanosec, sec = map(
int,
(now.nanoseconds % 10**9, now.nanoseconds/10**9)
)
metadata = {
'stamp': (sec + nanosec * 1e-9) - self.t_start,
'red': red,
'sender': self.hostname
}
metadata_str = json.dumps(metadata)
frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str
header = Header(
frame_id=frame_id,
stamp=Time(sec=sec, nanosec=nanosec)
)
msg.header = header
msg.format = 'jpeg'
msg.data = jpg
native_message = ROS2NativeMessage(
msg=msg,
sender=self.hostname[-1],
receivers=self.hosts,
data_type_name=f'v{self.hostname[-1]}',
data={
'red': red,
},
publisher=self.publisher,
stamp=(
msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
) - self.t_start
)
msg_generated = datetime.now().timestamp()
try:
self.agent.generated(native_message)
start_time = datetime.now()
self.agent.generated(self.get_prepared_message())
end_time = datetime.now()
print(f"Duration: {end_time-start_time}")
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 = {
......
......@@ -3,7 +3,7 @@
from infdist.optimization.models import MissionContext, InformationType
from infdist.optimization.aggregations import AggregationMostRecent
from .utility import PeriodicTypeForecast, UtilityVideo
from camera_streaming.utility import PeriodicTypeForecast, UtilityVideo
# from infdist.optimization.message_forecast import PeriodicTypeForecast
......
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