diff --git a/camera_streaming/camera_capture.py b/camera_streaming/camera_capture.py index 43408293a95cf1139fb55a16493b43fc15f67094..05a03b4822f6ab7540744771a0bc82386d31deb7 100644 --- a/camera_streaming/camera_capture.py +++ b/camera_streaming/camera_capture.py @@ -7,10 +7,12 @@ from datetime import datetime def camera_image_capture(callback): CAM_FPS = 10 + CAPTURED_IMG_SIZE = (640, 480) + # CAPTURED_IMG_SIZE = (800, 600) 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_FRAME_WIDTH, CAPTURED_IMG_SIZE[0]) + cam.set(cv2.CAP_PROP_FRAME_HEIGHT, CAPTURED_IMG_SIZE[1]) cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')) cam.read() diff --git a/camera_streaming/camera_stream.py b/camera_streaming/camera_stream.py index 5b5c6c13092a673f4874d9ef02b24fe7d3ff4056..3374f9183cf069fd9f90d2959746ef09644afd3c 100644 --- a/camera_streaming/camera_stream.py +++ b/camera_streaming/camera_stream.py @@ -1,6 +1,8 @@ #! /usr/bin/env python +import pickle import time +import traceback import json import threading @@ -14,7 +16,7 @@ import numpy as np from builtin_interfaces.msg import Time import socket -from datetime import datetime +from datetime import datetime, timedelta from rclpy.qos import ( QoSProfile, @@ -27,9 +29,7 @@ from rclpy.qos import ( from mission_manager.client import MissionExecutor, MissionClient from infdist.optimization.agent import EstimatingAgent from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage -from infdist.optimization.simplesim import ( - create_msgnum_constraint_violations, -) +from infdist.optimization import simplesim from sniffing import sniffer from mission import generate_mission_context @@ -100,10 +100,12 @@ class CameraStream(Node, MissionExecutor): int, (now.nanoseconds % 10**9, now.nanoseconds/10**9) ) + size = len(jpg) metadata = { 'stamp': (sec + nanosec * 1e-9) - self.t_start, 'red': red, - 'sender': self.hostname + 'sender': self.hostname[-1], + 'size': size, } metadata_str = json.dumps(metadata) frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str @@ -125,17 +127,33 @@ class CameraStream(Node, MissionExecutor): publisher=self.publisher, stamp=( msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 - ) - self.t_start + ) - self.t_start, + size=size, ) return native_message def start_mission_at_timestamp(self, timestamp): - super().start_mission_at_timestamp(timestamp) + super().start_mission_at_timestamp( + timestamp+timedelta( + seconds=20*self.timer_period * int(self.hosts[self.hostname]) + ) + ) self.t_start = timestamp.timestamp() def end_mission_at_timestamp(self, timestamp): super().end_mission_at_timestamp(timestamp) self.t_end = timestamp.timestamp() - self.t_start + self.constraints = { + 'RATE': simplesim.create_rate_constraint_violations( + timeslot_length=1., + delta=0.6, + ), + 'TPUT': simplesim.create_throughput_constraint_violations( + throughput=10.05, + timeslot_length=1., + ), + } + self.agent = EstimatingAgent( ident=self.hostname[-1], net=ROS2Network(), @@ -150,18 +168,7 @@ class CameraStream(Node, MissionExecutor): future_messages_num=10, window_size=0.1, simulations_num=200, - constraints={ - # create_throughput_constraint_violations( - # throughput, - # timeslot, - # message_size - # ) - 'msg_num': create_msgnum_constraint_violations( - msgnum=3, - timeslot=0.4, - ) - - }, + constraints=self.constraints, limit_history=15, ) @@ -176,7 +183,8 @@ class CameraStream(Node, MissionExecutor): data={ 'red': msg_metadata['red'], }, - stamp=msg_metadata['stamp'] + stamp=msg_metadata['stamp'], + size=msg_metadata['size'], ) self.agent.received(native_message) @@ -194,11 +202,26 @@ class CameraStream(Node, MissionExecutor): if self.timer: self.timer.cancel() + self.save_results('/home/pi/log/test_results.pickle') + + def save_results(self, filename): + rate_constraint = self.constraints['RATE'] + pickle.dump( + { + 'received_msgs': self.agent.received_messages, + 'sent_msgs': self.agent.received_messages, + 'train_data': rate_constraint.train_data, + 'model_params_history': rate_constraint.model_params_history, + }, + open(filename, 'wb') + ) + def change_params(self, params, timestamp): self.get_logger().info('Params change received') self.timer_period = 1 / float(params.get('fps', 2)) def timer_callback(self): + print("TIMER CALLBACK", datetime.now().timestamp()) if abs( (datetime.now().timestamp() - self.t_start - 0.01) % self.timer_period - self.offset @@ -212,6 +235,7 @@ class CameraStream(Node, MissionExecutor): end_time = datetime.now() print(f"Duration: {end_time-start_time}") except Exception as e: + traceback.print_exc() print(e) def move(self): diff --git a/camera_streaming/utility.py b/camera_streaming/utility.py index 070c8fe10662130f52fd975759e0325c00eb3064..8c3ffba3a12dfeeaf35289a6495c58c3da36f445 100644 --- a/camera_streaming/utility.py +++ b/camera_streaming/utility.py @@ -1,5 +1,4 @@ import math -from datetime import timedelta import numpy as np from infdist.optimization.utilities import Utility @@ -46,8 +45,10 @@ class PeriodicTypeForecast(BaseTypeForecast): set(self.receivers) - set([self.sender]), t_gen, self.data_type_name, + self.previous_message.size if self.previous_message else 200, { - 'red': self.previous_message.data.red if self.previous_message else 0, + 'red': self.previous_message.data.red + if self.previous_message else 0, } )