agent added to subscriber

camera_streaming/camera_subscriber.py

@@ -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