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

red detection, red marked with black on base station

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