41 lines
1.1 KiB
Python
41 lines
1.1 KiB
Python
|
#! /usr/bin/python
|
||
|
|
|
||
|
|
import rospy
|
||
|
|
from geometry_msgs.msg import Point
|
||
|
|
from bebop_driver.sensor_msgs import Image
|
||
|
|
|
||
|
|
from drone_demo.msg import component_centers
|
||
|
|
from find_targets import find_targets
|
||
|
|
|
||
|
|
|
||
|
|
class Publisher:
|
||
|
|
def __init__(self):
|
||
|
|
self.threshold_blue = 140
|
||
|
|
self.threshold_red = 120
|
||
|
|
self.threshold_green = 190
|
||
|
|
self.centers = component_centers()
|
||
|
|
self.pub = rospy.Publisher('targets', component_centers, queue_size=10)
|
||
|
|
rospy.Subscriber("image_raw", Image, self.on_picture)
|
||
|
|
|
||
|
|
def on_picture(self, msg):
|
||
|
|
H, L, R = find_targets(
|
||
|
|
msg.data,
|
||
|
|
threshold_blue=self.threshold_blue,
|
||
|
|
threshold_red=self.threshold_red,
|
||
|
|
threshold_green=self.threshold_green
|
||
|
|
)
|
||
|
|
self.centers.centers = [
|
||
|
|
Point(x=H[0], y=H[1], z=0),
|
||
|
|
Point(x=L[0], y=L[1], z=0),
|
||
|
|
Point(x=R[0], y=R[1], z=0),
|
||
|
|
]
|
||
|
|
self.pub.publish(self.centers)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
rospy.init_node('target_publisher')
|
||
|
|
|
||
|
|
publisher = Publisher()
|
||
|
|
rospy.spin()
|
||
|
|
|