drone-rigide/scripts/target_publisher.py
2019-06-02 18:02:15 +02:00

87 lines
3.3 KiB
Python
Executable file

#! /usr/bin/python
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import dynamic_reconfigure.server
from vqimg.msg import component_centers, component_center
from find_targets import find_targets, normalize_coordinates
from detect_targets.cfg import DetectTargetsConfig
class Publisher:
def __init__(self):
self.threshold_blue = (140, 255)
self.threshold_red = (0, 120)
self.threshold_green = (0, 190)
self.binary_view = False
self.draw_targets = False
self.centers = component_centers()
self.pub = rospy.Publisher('targets', component_centers, queue_size=10)
self.img_pub = rospy.Publisher('img_targets', Image, queue_size=0)
rospy.Subscriber("/bebop/image_raw", Image, self.on_picture)
self.bridge = CvBridge()
self.config_srv = dynamic_reconfigure.server.Server(DetectTargetsConfig, self.on_reconf)
def on_reconf(self, config, level):
self.threshold_red = (config['red_min'], config['red_max'])
self.threshold_blue = (config['blue_min'], config['blue_max'])
self.threshold_green = (config['green_min'], config['green_max'])
self.binary_view = config['binary']
self.draw_targets = config['targets']
return config
def on_picture(self, msg):
img = np.array(np.asarray(self.bridge.imgmsg_to_cv2(msg)), copy=True)
try:
if not self.binary_view:
H, L, R = find_targets(
img,
threshold_blue=self.threshold_blue,
threshold_red=self.threshold_red,
threshold_green=self.threshold_green
)
else:
H, L, R, points = find_targets(
img,
threshold_blue=self.threshold_blue,
threshold_red=self.threshold_red,
threshold_green=self.threshold_green,
return_binary=True
)
except ValueError:
return
pub_H = normalize_coordinates(H, msg.width, msg.height)
pub_L = normalize_coordinates(L, msg.width, msg.height)
pub_R = normalize_coordinates(R, msg.width, msg.height)
self.centers.data = [
component_center(x=pub_H[0], y=pub_H[1], nb_vertex=1, label=1),
component_center(x=pub_L[0], y=pub_L[1], nb_vertex=1, label=2),
component_center(x=pub_R[0], y=pub_R[1], nb_vertex=1, label=3),
]
self.pub.publish(self.centers)
if self.binary_view:
img[:,:,1] = points * 255
img[:,:,0] = np.zeros((len(img), len(img[0])))
img[:,:,2] = np.zeros((len(img), len(img[0])))
if self.draw_targets:
for i in range(-20, 20):
for j in range(-20, 20):
try:
img[int(H[1])-i, int(H[0])-j] = [0, 255, 0]
img[int(L[1])-i, int(L[0])-j] = [255, 0, 0]
img[int(R[1])-i, int(R[0])-j] = [0, 0, 255]
except Exception:
pass
self.img_pub.publish(self.bridge.cv2_to_imgmsg(img, "rgb8"))
if __name__ == '__main__':
rospy.init_node('target_publisher')
publisher = Publisher()
rospy.spin()