253 lines
9.6 KiB
Python
Executable file
253 lines
9.6 KiB
Python
Executable file
#! /usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
import math
|
|
|
|
import roslib
|
|
import rospy
|
|
from geometry_msgs.msg import Twist
|
|
import tf
|
|
|
|
import sigsim
|
|
import device
|
|
|
|
import dynamic_reconfigure.server
|
|
from drone_demo.cfg import TriangleParamConfig
|
|
from drone_demo.msg import control
|
|
|
|
from vqimg.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']
|
|
self.target_distance = config['distance_to_target']
|
|
self.max_speed = config['max_speed']
|
|
|
|
self.angular_z_Kp = config['angular_z_Kp']
|
|
self.angular_z_Kd = config['angular_z_Kd']
|
|
self.control_angular_z = config['control_angular_z']
|
|
|
|
self.linear_y_Kp = config['linear_y_Kp']
|
|
self.linear_y_Kd = config['linear_y_Kd']
|
|
self.control_linear_y = config['control_linear_y']
|
|
|
|
self.linear_z_Kp = config['linear_z_Kp']
|
|
self.linear_z_Kd = config['linear_z_Kd']
|
|
self.control_linear_z = config['control_linear_z']
|
|
|
|
self.linear_x_Kp = config['linear_x_Kp']
|
|
self.linear_x_Kd = config['linear_x_Kd']
|
|
self.control_linear_x = config['control_linear_x']
|
|
|
|
return config
|
|
|
|
def clear_controls(self) :
|
|
self.error_angular_z.clear()
|
|
self.error_linear_z.clear()
|
|
self.error_linear_y.clear()
|
|
self.error_linear_x.clear()
|
|
|
|
def saturate_twist() :
|
|
if self.twist.linear.x > self.max_speed :
|
|
self.twist.linear.x = self.max_speed
|
|
elif self.twist.linear.x < - self.max_speed :
|
|
self.twist.linear.x = - self.max_speed
|
|
if self.twist.linear.y > self.max_speed :
|
|
self.twist.linear.y = self.max_speed
|
|
elif self.twist.linear.y < - self.max_speed :
|
|
self.twist.linear.y = - self.max_speed
|
|
if self.twist.linear.z > self.max_speed :
|
|
self.twist.linear.z = self.max_speed
|
|
elif self.twist.linear.z < - self.max_speed :
|
|
self.twist.linear.z = - self.max_speed
|
|
|
|
|
|
|
|
def on_comp(self, msg):
|
|
self.twist = Twist()
|
|
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)
|
|
self.twist_pub.publish(self.twist)
|
|
|
|
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)
|
|
self.d = self.target_width*ca/(w*self.tan_cam) *.5 # why *.5.... I don't know.
|
|
self.z = -Gy*self.d*self.tan_cam
|
|
|
|
|
|
#print('#######')
|
|
#print('d = {}, z = {}, alpha = {}'.format(self.d, self.z, self.alpha*180/math.pi))
|
|
#print('w = {}, h = {}, Gy = {}'.format(w, h, Gy))
|
|
#print('L = {}, l = {}'.format(self.target_width, self.target_depth))
|
|
|
|
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')
|
|
|
|
if self.control_angular_z:
|
|
dt = t-self.last_time_angular_z
|
|
self.last_time_angular_z = t
|
|
self.error_angular_z.next(dt)
|
|
self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1]
|
|
if self.angular_z_pub.get_num_connections() > 0 :
|
|
self.angular_z_info.target = 0
|
|
self.angular_z_info.error = self.error_angular_z[0]
|
|
self.angular_z_info.derror = self.error_angular_z[1]
|
|
self.angular_z_info.cmd_vel = self.twist.angular.z
|
|
self.angular_z_pub.publish(self.angular_z_info)
|
|
|
|
if self.control_angular_z:
|
|
dt = t-self.last_time_angular_z
|
|
self.last_time_angular_z = t
|
|
self.error_angular_z.next(dt)
|
|
self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1]
|
|
if self.angular_z_pub.get_num_connections() > 0 :
|
|
self.angular_z_info.target = 0
|
|
self.angular_z_info.error = self.error_angular_z[0]
|
|
self.angular_z_info.derror = self.error_angular_z[1]
|
|
self.angular_z_info.cmd_vel = self.twist.angular.z
|
|
self.angular_z_pub.publish(self.angular_z_info)
|
|
|
|
if self.control_linear_z:
|
|
dt = t-self.last_time_linear_z
|
|
self.last_time_linear_z = t
|
|
self.error_linear_z.next(dt)
|
|
self.twist.linear.z = self.linear_z_Kp * self.error_linear_z[0] + self.linear_z_Kd * self.error_linear_z[1]
|
|
if self.linear_z_pub.get_num_connections() > 0 :
|
|
self.linear_z_info.target = 0
|
|
self.linear_z_info.error = self.error_linear_z[0]
|
|
self.linear_z_info.derror = self.error_linear_z[1]
|
|
self.linear_z_info.cmd_vel = self.twist.linear.z
|
|
self.linear_z_pub.publish(self.linear_z_info)
|
|
|
|
if self.control_linear_y:
|
|
dt = t-self.last_time_linear_y
|
|
self.last_time_linear_y = t
|
|
self.error_linear_y.next(dt)
|
|
self.twist.linear.y = self.linear_y_Kp * self.error_linear_y[0] + self.linear_y_Kd * self.error_linear_y[1]
|
|
if self.linear_y_pub.get_num_connections() > 0 :
|
|
self.linear_y_info.target = 0
|
|
self.linear_y_info.error = self.error_linear_y[0]
|
|
self.linear_y_info.derror = self.error_linear_y[1]
|
|
self.linear_y_info.cmd_vel = self.twist.linear.y
|
|
self.linear_y_pub.publish(self.linear_y_info)
|
|
|
|
if self.control_linear_x:
|
|
dt = t-self.last_time_linear_x
|
|
self.last_time_linear_x = t
|
|
self.error_linear_x.next(dt)
|
|
self.twist.linear.x = self.linear_x_Kp * self.error_linear_x[0] + self.linear_x_Kd * self.error_linear_x[1]
|
|
if self.linear_x_pub.get_num_connections() > 0 :
|
|
self.linear_x_info.target = 0
|
|
self.linear_x_info.error = self.error_linear_x[0]
|
|
self.linear_x_info.derror = self.error_linear_x[1]
|
|
self.linear_x_info.cmd_vel = self.twist.linear.x
|
|
self.linear_x_pub.publish(self.linear_x_info)
|
|
|
|
|
|
|
|
|
|
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.target_distance = 2
|
|
self.max_speed = .3
|
|
|
|
|
|
self.last_time_angular_z = 0
|
|
self.last_time_linear_z = 0
|
|
self.last_time_linear_y = 0
|
|
self.last_time_linear_x = 0
|
|
self.first_time = rospy.Time.now()
|
|
|
|
# PD params
|
|
|
|
self.angular_z_Kp = .1
|
|
self.angular_z_Kd = .1
|
|
self.control_angular_z = True
|
|
|
|
self.linear_y_Kp = .1
|
|
self.linear_y_Kd = .1
|
|
self.control_linear_y = True
|
|
|
|
self.linear_z_Kp = .1
|
|
self.linear_z_Kd = .1
|
|
self.control_linear_z = True
|
|
|
|
self.linear_x_Kp = .1
|
|
self.linear_x_Kd = .1
|
|
self.control_linear_x = True
|
|
|
|
# Control info
|
|
self.angular_z_info = control()
|
|
self.linear_x_info = control()
|
|
self.linear_y_info = control()
|
|
self.linear_z_info = control()
|
|
|
|
# Errors
|
|
|
|
self.error_angular_z = sigsim.Smoothed(lambda me : -self.Gx * self.camera_angle , 1, 2, 1)
|
|
self.error_linear_z = sigsim.Smoothed(lambda me : -self.z , 1, 2, 1)
|
|
self.error_linear_y = sigsim.Smoothed(lambda me : self.alpha , 1, 2, 1)
|
|
self.error_linear_x = sigsim.Smoothed(lambda me : self.d - self.target_distance, 1, 2, 1)
|
|
|
|
|
|
# ROS stuff
|
|
|
|
self.twist = Twist()
|
|
self.twist_pub = rospy.Publisher ('cmd_vel', Twist, queue_size = 1)
|
|
self.angular_z_pub = rospy.Publisher ('angular_z_control', control, queue_size = 1)
|
|
self.linear_z_pub = rospy.Publisher ('linear_z_control', control, queue_size = 1)
|
|
self.linear_y_pub = rospy.Publisher ('linear_y_control', control, queue_size = 1)
|
|
self.linear_x_pub = rospy.Publisher ('linear_x_control', control, 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__':
|
|
print "running"
|
|
rospy.init_node('triangle_control', anonymous=True)
|
|
|
|
print "node created"
|
|
|
|
triangle = TriangleControl()
|
|
rospy.spin()
|