#! /usr/bin/env python # -*- coding: utf-8 -*- import math import time import roslib import rospy from geometry_msgs.msg import Twist import tf from simple_pid import PID 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'] self.target_distance = config['distance_to_target'] self.max_speed = config['max_speed'] #gains are reversed because of the chosen angle direction self.pid_angular_z.Kp = - config['angular_z_Kp'] self.pid_angular_z.Ki = - config['angular_z_Ki'] self.pid_angular_z.Kd = - config['angular_z_Kd'] self.pid_angular_z.set_auto_mode(config['control_angular_z'], last_output=0.0) if not config['control_angular_z']: self.pid_angular_z._last_output = 0.0 self.pid_linear_z.Kp = config['linear_z_Kp'] self.pid_linear_z.Ki = config['linear_z_Ki'] self.pid_linear_z.Kd = config['linear_z_Kd'] self.pid_linear_z.set_auto_mode(config['control_linear_z'], last_output=0.0) if not config['control_linear_z']: self.pid_linear_z._last_output = 0.0 self.pid_linear_z.output_limits = ( -config['max_speed'], config['max_speed'] ) self.pid_linear_y.Kp = config['linear_y_Kp'] self.pid_linear_y.Ki = config['linear_y_Ki'] self.pid_linear_y.Kd = config['linear_y_Kd'] self.pid_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0) self.pid_speed_linear_y.Kp = config['speed_linear_y_Kp'] self.pid_speed_linear_y.Ki = config['speed_linear_y_Ki'] self.pid_speed_linear_y.Kd = config['speed_linear_y_Kd'] self.pid_speed_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0) if not config['control_linear_y']: self.pid_linear_y._last_output = 0.0 self.pid_speed_linear_y._last_output = 0.0 self.pid_linear_y.output_limits = ( -config['max_acceleration'], config['max_acceleration'] ) self.pid_speed_linear_y.output_limits = ( -config['max_speed'], config['max_speed'] ) # X gains are reversed because of the chosen axis self.pid_linear_x.Kp = - config['linear_x_Kp'] self.pid_linear_x.Ki = - config['linear_x_Ki'] self.pid_linear_x.Kd = - config['linear_x_Kd'] self.pid_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0) self.pid_speed_linear_x.Kp = config['speed_linear_x_Kp'] self.pid_speed_linear_x.Ki = config['speed_linear_x_Ki'] self.pid_speed_linear_x.Kd = config['speed_linear_x_Kd'] self.pid_speed_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0) if not config['control_linear_x']: self.pid_linear_x._last_output = 0.0 self.pid_speed_linear_x._last_output = 0.0 self.pid_linear_x.output_limits = ( -config['max_acceleration'], config['max_acceleration'] ) self.pid_speed_linear_x.output_limits = ( -config['max_speed'], config['max_speed'] ) self.pid_linear_x.setpoint = self.target_distance 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) # 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 # 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') self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle) if self.angular_z_pub.get_num_connections() > 0: self.angular_z_info.target = 0 self.angular_z_info.error = 0 self.angular_z_info.derror = 0 self.angular_z_info.cmd_vel = self.twist.angular.z self.angular_z_pub.publish(self.angular_z_info) self.twist.linear.z = self.pid_linear_z(self.z) if self.linear_z_pub.get_num_connections() > 0: self.linear_z_info.target = 0 self.linear_z_info.error = self.z self.linear_z_info.derror = 0 self.linear_z_info.cmd_vel = self.twist.linear.z self.linear_z_pub.publish(self.linear_z_info) dt = time.time() - self.pid_linear_y._last_time last_input_y = self.pid_linear_y._last_input target_acceleration_y = self.pid_linear_y(-self.alpha) self.pid_speed_linear_y.setpoint = target_acceleration_y speed_y = (last_input_y - self.pid_linear_y._last_input) / dt self.twist.linear.y = self.pid_speed_linear_y(speed_y) if self.linear_y_pub.get_num_connections() > 0: self.linear_y_info.target = 0 self.linear_y_info.error = -self.alpha self.linear_y_info.derror = 0 self.linear_y_info.cmd_vel = self.twist.linear.y self.linear_y_pub.publish(self.linear_y_info) dt = time.time() - self.pid_linear_x._last_time last_input_x = self.pid_linear_x._last_input target_acceleration_x = self.pid_linear_x(self.d) self.pid_speed_linear_x.setpoint = target_acceleration_x speed_x = (last_input_x - self.pid_linear_x._last_input) / dt self.twist.linear.x = self.pid_speed_linear_x(speed_x) if self.linear_x_pub.get_num_connections() > 0: self.linear_x_info.target = self.pid_linear_x.setpoint self.linear_x_info.error = self.target_distance - self.d self.linear_x_info.derror = 0 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() self.pid_angular_z = PID( 1, 0, 0, auto_mode=True, sample_time=0.14 # 7 Hz ) self.pid_linear_z = PID( 1, 0, 0, auto_mode=True, sample_time=0.14 ) self.pid_linear_y = PID( 1, 0, 0, auto_mode=True, sample_time=0.14 ) self.pid_speed_linear_y = PID( 1, 0, 0, auto_mode=True, sample_time=0.14 ) self.pid_linear_x = PID( 1, 0, 0, auto_mode=True, sample_time=0.14, setpoint=self.target_distance, ) self.pid_speed_linear_x = PID( 1, 0, 0, auto_mode=True, sample_time=0.14 ) # Control info self.angular_z_info = control() self.linear_x_info = control() self.linear_y_info = control() self.linear_z_info = control() # 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()