106 lines
3.1 KiB
Python
Executable file
106 lines
3.1 KiB
Python
Executable file
#! /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 TriangleConfig
|
|
from detect_targets.msg import control
|
|
|
|
from detect_targets.msg import component_centers
|
|
|
|
|
|
class Triangle:
|
|
|
|
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()
|
|
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 = Triangle()
|
|
rospy.spin()
|