#! /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()