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

WIP adjusting to new experiments

parent 08a08628
...@@ -7,10 +7,12 @@ from datetime import datetime ...@@ -7,10 +7,12 @@ from datetime import datetime
def camera_image_capture(callback): def camera_image_capture(callback):
CAM_FPS = 10 CAM_FPS = 10
CAPTURED_IMG_SIZE = (640, 480)
# CAPTURED_IMG_SIZE = (800, 600)
cam = cv2.VideoCapture(0) cam = cv2.VideoCapture(0)
cam.set(cv2.CAP_PROP_FPS, CAM_FPS) cam.set(cv2.CAP_PROP_FPS, CAM_FPS)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, 640) cam.set(cv2.CAP_PROP_FRAME_WIDTH, CAPTURED_IMG_SIZE[0])
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) cam.set(cv2.CAP_PROP_FRAME_HEIGHT, CAPTURED_IMG_SIZE[1])
cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG')) cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
cam.read() cam.read()
......
#! /usr/bin/env python #! /usr/bin/env python
import pickle
import time import time
import traceback
import json import json
import threading import threading
...@@ -14,7 +16,7 @@ import numpy as np ...@@ -14,7 +16,7 @@ import numpy as np
from builtin_interfaces.msg import Time from builtin_interfaces.msg import Time
import socket import socket
from datetime import datetime from datetime import datetime, timedelta
from rclpy.qos import ( from rclpy.qos import (
QoSProfile, QoSProfile,
...@@ -27,9 +29,7 @@ from rclpy.qos import ( ...@@ -27,9 +29,7 @@ from rclpy.qos import (
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
from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
from infdist.optimization.simplesim import ( from infdist.optimization import simplesim
create_msgnum_constraint_violations,
)
from sniffing import sniffer from sniffing import sniffer
from mission import generate_mission_context from mission import generate_mission_context
...@@ -100,10 +100,12 @@ class CameraStream(Node, MissionExecutor): ...@@ -100,10 +100,12 @@ class CameraStream(Node, MissionExecutor):
int, int,
(now.nanoseconds % 10**9, now.nanoseconds/10**9) (now.nanoseconds % 10**9, now.nanoseconds/10**9)
) )
size = len(jpg)
metadata = { metadata = {
'stamp': (sec + nanosec * 1e-9) - self.t_start, 'stamp': (sec + nanosec * 1e-9) - self.t_start,
'red': red, 'red': red,
'sender': self.hostname 'sender': self.hostname[-1],
'size': size,
} }
metadata_str = json.dumps(metadata) metadata_str = json.dumps(metadata)
frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str frame_id = "MEEETADATA" + chr(len(metadata_str)) + metadata_str
...@@ -125,17 +127,33 @@ class CameraStream(Node, MissionExecutor): ...@@ -125,17 +127,33 @@ class CameraStream(Node, MissionExecutor):
publisher=self.publisher, publisher=self.publisher,
stamp=( stamp=(
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,
size=size,
) )
return native_message return native_message
def start_mission_at_timestamp(self, timestamp): 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() self.t_start = timestamp.timestamp()
def end_mission_at_timestamp(self, timestamp): def end_mission_at_timestamp(self, timestamp):
super().end_mission_at_timestamp(timestamp) super().end_mission_at_timestamp(timestamp)
self.t_end = timestamp.timestamp() - self.t_start 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( self.agent = EstimatingAgent(
ident=self.hostname[-1], ident=self.hostname[-1],
net=ROS2Network(), net=ROS2Network(),
...@@ -150,18 +168,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -150,18 +168,7 @@ class CameraStream(Node, MissionExecutor):
future_messages_num=10, future_messages_num=10,
window_size=0.1, window_size=0.1,
simulations_num=200, simulations_num=200,
constraints={ constraints=self.constraints,
# create_throughput_constraint_violations(
# throughput,
# timeslot,
# message_size
# )
'msg_num': create_msgnum_constraint_violations(
msgnum=3,
timeslot=0.4,
)
},
limit_history=15, limit_history=15,
) )
...@@ -176,7 +183,8 @@ class CameraStream(Node, MissionExecutor): ...@@ -176,7 +183,8 @@ class CameraStream(Node, MissionExecutor):
data={ data={
'red': msg_metadata['red'], 'red': msg_metadata['red'],
}, },
stamp=msg_metadata['stamp'] stamp=msg_metadata['stamp'],
size=msg_metadata['size'],
) )
self.agent.received(native_message) self.agent.received(native_message)
...@@ -194,11 +202,26 @@ class CameraStream(Node, MissionExecutor): ...@@ -194,11 +202,26 @@ class CameraStream(Node, MissionExecutor):
if self.timer: if self.timer:
self.timer.cancel() 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): def change_params(self, params, timestamp):
self.get_logger().info('Params change received') self.get_logger().info('Params change received')
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):
print("TIMER CALLBACK", datetime.now().timestamp())
if abs( if abs(
(datetime.now().timestamp() - self.t_start - 0.01) (datetime.now().timestamp() - self.t_start - 0.01)
% self.timer_period - self.offset % self.timer_period - self.offset
...@@ -212,6 +235,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -212,6 +235,7 @@ class CameraStream(Node, MissionExecutor):
end_time = datetime.now() end_time = datetime.now()
print(f"Duration: {end_time-start_time}") print(f"Duration: {end_time-start_time}")
except Exception as e: except Exception as e:
traceback.print_exc()
print(e) print(e)
def move(self): def move(self):
......
import math import math
from datetime import timedelta
import numpy as np import numpy as np
from infdist.optimization.utilities import Utility from infdist.optimization.utilities import Utility
...@@ -46,8 +45,10 @@ class PeriodicTypeForecast(BaseTypeForecast): ...@@ -46,8 +45,10 @@ class PeriodicTypeForecast(BaseTypeForecast):
set(self.receivers) - set([self.sender]), set(self.receivers) - set([self.sender]),
t_gen, t_gen,
self.data_type_name, 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