red detection, red marked with black on base station

camera_streaming/camera_subscriber.py

@@ -20,10 +20,12 @@ from rclpy.qos import (
 from rclpy.qos import QoSPresetProfiles
 
 from mission_manager.client import MissionExecutor, MissionClient
-from infdist.optimization.agent import EstimatingAgent
+from infdist.optimization.agent import FullCommAgent
 from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
 from mission import generate_mission_context
 
+from detect import detect_red
+
 CUSTOM_QOS = QoSProfile(
     history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
     reliability=QoSReliabilityPolicy.
@@ -79,18 +81,28 @@ class CameraSubscriber(Node, MissionExecutor):
             ) for host in self.hosts.keys()
         ]
         # print(self.camera_subscriptions[0].get_actual_qos())
+        self.t_start = datetime.now().timestamp()
         self.t_end = 30
-        self.agent = EstimatingAgent(
+
+    def start_mission_at_timestamp(self, timestamp):
+        super().start_mission_at_timestamp(timestamp)
+        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.agent = FullCommAgent(
             ident='base_station',
             net=ROS2Network(),
             messages_context=generate_mission_context(
                 agents=self.hosts.keys(),
                 data_types=['camera'],
                 t_end=self.t_end,
+                T=0.5,
             ),
-            now_func=time.time,
-            agents={'base_station': lambda t: set()},
-            constraints={},
+            now_func=lambda: time.time() - self.t_start,
+            # agents={'base_station': lambda t: set()},
+            # constraints={},
         )
 
     def start_mission(self, timestamp):
@@ -163,8 +175,6 @@ class CameraSubscriber(Node, MissionExecutor):
                     cv2.FONT_HERSHEY_COMPLEX,
                     .35, (255, 255, 255)
                 )
-        # print('images', self.images)
-        # print('frame', frame)
 
         cv2.imshow(win_name, frame)
         cv2.waitKey(1)
@@ -174,17 +184,15 @@ class CameraSubscriber(Node, MissionExecutor):
             now = datetime.now()
             img = msg.data
             npimg = np.frombuffer(img, dtype=np.uint8)
-            img = cv2.imdecode(npimg, cv2.IMREAD_COLOR)/256
-            # cv2.imwrite('/home/agniewek/longTemp/test.jpg', img)
+            img = cv2.imdecode(npimg, cv2.IMREAD_COLOR)
+            mask = detect_red(img)
+            img = cv2.bitwise_and(img, img, mask=mask)/256
             host_id = self.hosts[host]
             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)
-            # self.figure.canvas.draw()
-            # cv2.waitKey(1)
             t_gen = datetime.fromtimestamp(
                 msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
             )
@@ -204,6 +212,9 @@ class CameraSubscriber(Node, MissionExecutor):
                     'battery_level': 1,
                     'max_depl_rate': 0.1,
                 },
+                stamp=(
+                    msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+                ) - self.t_start
             )
             self.agent.received(native_message)
         return _camera_callback

camera_streaming/detect.py

+#! /usr/bin/env python
+
+import cv2
+import numpy as np
+
+
+def detect_red(img):
+    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+    # Range for lower red
+    lower_red = np.array([0, 120, 70])
+    upper_red = np.array([10, 255, 255])
+    mask_part1 = cv2.inRange(hsv, lower_red, upper_red)
+
+    # Range for upper range
+    lower_red = np.array([170, 120, 70])
+    upper_red = np.array([180, 255, 255])
+    mask_part2 = cv2.inRange(hsv, lower_red, upper_red)
+
+    mask = mask_part1 + mask_part2
+
+    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))
+    mask = cv2.morphologyEx(mask, cv2.MORPH_DILATE, np.ones((3, 3), np.uint8))
+
+    # creating an inverted mask
+    mask_black = cv2.bitwise_not(mask)
+    return mask_black