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

red detection, red marked with black on base station

parent 5bbedf6e
......@@ -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
......
#! /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
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