drone-rigide/workspace/src/detect_targets/scripts/triangle_control.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import math
import time

import numpy as np

import roslib
import rospy

from std_msgs.msg import Float64
import tf

import dynamic_reconfigure.server

from detect_targets.cfg import TriangleParamConfig
from detect_targets.msg import control

from detect_targets.msg import component_centers


class TriangleControl:

    def on_reconf(self, config, level):

        self.camera_angle = config['camera_angle']*math.pi/360.0  # theta/2
        self.tan_cam = math.tan(self.camera_angle)

        self.target_width = config['target_width']
        self.target_depth = config['target_depth']
        return config

    def on_comp(self, msg):
        if len(msg.data) > 2:
            msg.data.sort(key=lambda component: -component.nb_vertex)
            pts = msg.data[0:3]
            pts.sort(key=lambda component: -component.y)
            H = pts[0]
            L = pts[2]
            R = pts[1]
            if pts[1].x < pts[2].x:
                L = pts[1]
                R = pts[2]
            self.triangle(L, H, R)

    def triangle(self, L, H, R):
        now = rospy.Time.now()
        t = (now - self.first_time).to_sec()
        self.Gx = (L.x + H.x + R.x)*.333333
        Gy = (L.y + H.y + R.y)*.333333
        w = R.x - L.x
        h = H.x - .5 * (R.x + L.x)

        self.alpha = math.atan(h*self.target_width/(1e-5+w*self.target_depth))
        ca = math.cos(self.alpha)
        sa = math.sin(self.alpha)
        # why *.5.... I don't know.
        self.d = self.target_width*ca/(w*self.tan_cam) * .5
        self.z = -Gy*self.d*self.tan_cam

        self.br.sendTransform((self.d * ca, self.d * sa, self.z),
                              tf.transformations.quaternion_from_euler(
                                  0, 0, self.alpha + math.pi),
                              now,
                              'drone', 'target')

        self.angular_z_pub.publish(data=-self.Gx * self.camera_angle)
        self.linear_z_pub.publish(data=self.z)
        self.linear_y_pub.publish(data=-self.alpha)
        self.linear_x_pub.publish(data=self.d)

    def __init__(self):

        self.Gx = 0

        self.alpha = 0
        self.d = 0
        self.z = 0

        self.camera_angle = 80*math.pi/180./2.0
        self.tan_cam = math.tan(self.camera_angle)
        self.target_width = 1
        self.target_depth = .2

        self.angular_z_pub = rospy.Publisher(
            'angular_z', Float64,                         queue_size=1)
        self.linear_z_pub = rospy.Publisher(
            'linear_z',  Float64,                         queue_size=1)
        self.linear_y_pub = rospy.Publisher(
            'linear_y',  Float64,                         queue_size=1)
        self.linear_x_pub = rospy.Publisher(
            'linear_x',  Float64,                         queue_size=1)
        self.comp_sub = rospy.Subscriber(
            "component_centers", component_centers, self.on_comp, queue_size=1)

        self.config_srv = dynamic_reconfigure.server.Server(
            TriangleParamConfig, self.on_reconf)

        self.br = tf.TransformBroadcaster()


if __name__ == '__main__':
    rospy.init_node('triangle_control', anonymous=True)

    triangle = TriangleControl()
    rospy.spin()