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

298 lines
11 KiB
Python
Raw Normal View History

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import math
2019-03-22 15:59:55 +00:00
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)
2019-03-22 15:59:55 +00:00
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
2019-03-22 15:59:55 +00:00
self.pid_speed_linear_y._last_output = 0.0
self.pid_linear_y.output_limits = (
2019-03-22 15:59:55 +00:00
-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)
2019-03-22 15:59:55 +00:00
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
2019-03-22 15:59:55 +00:00
self.pid_speed_linear_x._last_output = 0.0
self.pid_linear_x.output_limits = (
2019-03-22 15:59:55 +00:00
-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)
2019-03-22 15:59:55 +00:00
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)
2019-03-22 15:59:55 +00:00
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
)
2019-03-22 15:59:55 +00:00
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,
)
2019-03-22 15:59:55 +00:00
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()