Commit fb310bf0 authored by Barcis, Michal's avatar Barcis, Michal

WIP adjusting to new experiments

parent 08a08628
......@@ -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()
......
#! /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):
......
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,
}
)
......
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