Commit b9e97373 authored by Barciś, Agata's avatar Barciś, Agata

agent added to subscriber

parent 629f1fc4
......@@ -9,6 +9,7 @@ from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np
from datetime import datetime
import time
from rclpy.qos import (
QoSProfile,
......@@ -16,9 +17,12 @@ from rclpy.qos import (
QoSHistoryPolicy,
QoSReliabilityPolicy,
)
from rclpy.qos import QoSPresetProfiles
from mission_manager.client import MissionExecutor, MissionClient
from infdist.optimization.agent import EstimatingAgent
from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
from mission import generate_mission_context
CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
......@@ -71,10 +75,23 @@ class CameraSubscriber(Node, MissionExecutor):
'/{}/camera'.format(host),
self.gen_camera_callback(host),
qos_profile=CUSTOM_QOS,
# qos_profile=QoSPresetProfiles.get_from_short_key('SYSTEM_DEFAULT')
) for host in self.hosts.keys()
]
# self.figure, self.display = plt.subplots(2, 4)
# plt.show(block=False)
# print(self.camera_subscriptions[0].get_actual_qos())
self.t_end = 30
self.agent = EstimatingAgent(
ident='base_station',
net=ROS2Network(),
messages_context=generate_mission_context(
agents=self.hosts.keys(),
data_types=['camera'],
t_end=self.t_end,
),
now_func=time.time,
agents={'base_station': lambda t: set()},
constraints={},
)
def start_mission(self, timestamp):
self.get_logger().info('Mission started')
......@@ -88,22 +105,28 @@ class CameraSubscriber(Node, MissionExecutor):
def check_for_old_images(self):
for host in self.hosts.keys():
now = datetime.now()
if self.fps[host] and (now - self.last_frame[host]).total_seconds() > 10:
if (
self.fps[host] and
(now - self.last_frame[host]).total_seconds() > 10
):
host_id = self.hosts[host]
place = (int(host_id / self.shape[1]), int(host_id % self.shape[1]))
place = (
int(host_id / self.shape[1]), int(host_id % self.shape[1])
)
self.images[place[0], place[1]] *= 0.25
self.fps[host] = 0
def display_streams(self, # 2d np array of imgs (each img an np array)
pad=10, # number of pixels to use for padding, must be even
pad=4, # even number of pixels to use for padding
win_name='camera streams' # name of cv2 window
):
self.check_for_old_images()
rows, cols = self.shape
subplot_shapes = np.array([list(map(np.shape, x)) for x in self.images])
subplot_shapes = np.array(
[list(map(np.shape, x)) for x in self.images]
)
sp_height, sp_width, depth = np.max(
np.max(subplot_shapes, axis=0),
axis=0
......@@ -133,7 +156,13 @@ class CameraSubscriber(Node, MissionExecutor):
x0 = c * (sp_width+pad) + pad//2
frame[y0:y0+h, x0:x0+w] = img
frame = cv2.putText(frame, self.titles[r, c], (x0, y0-title_pad//4), cv2.FONT_HERSHEY_COMPLEX, .35, (255,255,255))
frame = cv2.putText(
frame,
self.titles[r, c],
(x0, y0-title_pad//4),
cv2.FONT_HERSHEY_COMPLEX,
.35, (255, 255, 255)
)
# print('images', self.images)
# print('frame', frame)
......@@ -148,7 +177,9 @@ class CameraSubscriber(Node, MissionExecutor):
img = cv2.imdecode(npimg, cv2.IMREAD_COLOR)/256
# cv2.imwrite('/home/agniewek/longTemp/test.jpg', img)
host_id = self.hosts[host]
place = (int(host_id / self.shape[1]), int(host_id % self.shape[1]))
place = (
int(host_id / self.shape[1]), int(host_id % self.shape[1])
)
self.images[place[0], place[1]] = img
self.display_streams()
# self.display[int(host_id / 4), host_id % 4].imshow(img)
......@@ -157,15 +188,24 @@ class CameraSubscriber(Node, MissionExecutor):
t_gen = datetime.fromtimestamp(
msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
)
print("HEADER", msg.header.frame_id)
self.fps[host] = round(
1/(t_gen - self.last_frame[host]).total_seconds(),
2
)
self.last_frame[host] = t_gen
now2 = datetime.now()
self.delays[host] = (now - t_gen).total_seconds()
# print('delay: ', (now - t_gen).total_seconds())
# print('delay2: ', (now2 - t_gen).total_seconds())
native_message = ROS2NativeMessage(
msg=msg,
sender=host,
receivers=['base_station'],
data_type_name=f'camera_{host}',
data={
'battery_level': 1,
'max_depl_rate': 0.1,
},
)
self.agent.received(native_message)
return _camera_callback
......
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