Hugo LEVY-FALK
7 years ago
4 changed files with 353 additions and 353 deletions
@ -0,0 +1,107 @@ |
|||
#! /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 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() |
|||
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 = Triangle() |
|||
rospy.spin() |
|||
@ -1,341 +0,0 @@ |
|||
#! /usr/bin/env python |
|||
# -*- coding: utf-8 -*- |
|||
|
|||
import math |
|||
import time |
|||
|
|||
import numpy as np |
|||
|
|||
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'] |
|||
self.sample_time = config['sample_time'] |
|||
self.double_loop = config['double_loop'] |
|||
|
|||
#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) |
|||
self.pid_angular_z.sample_time = self.sample_time |
|||
self.pid_angular_z._integral = 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) |
|||
self.pid_linear_z.sample_time = self.sample_time |
|||
self.pid_linear_z._integral = 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_linear_y.sample_time = self.sample_time |
|||
self.pid_linear_y._integral = 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) |
|||
self.pid_speed_linear_y.sample_time = self.sample_time |
|||
self.pid_speed_linear_y._integral = 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'] |
|||
) |
|||
self.speed_corrector_y = config['speed_corrector_y'] |
|||
|
|||
# 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_linear_x.sample_time = self.sample_time |
|||
self.pid_linear_x._integral = 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) |
|||
self.pid_speed_linear_x.sample_time = self.sample_time |
|||
self.pid_speed_linear_x._integral = 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.speed_corrector_x = config['speed_corrector_x'] |
|||
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 |
|||
|
|||
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 |
|||
self.last_values_y = np.concatenate((self.last_values_y[1:7], [self.alpha])) |
|||
target_acceleration_y = self.pid_linear_y(-self.alpha) |
|||
self.pid_speed_linear_y.setpoint = target_acceleration_y |
|||
speed_y = self.last_values_y.dot(self.savgol_filter) / self.sample_time |
|||
if self.double_loop: |
|||
self.twist.linear.y = self.pid_speed_linear_y(speed_y) |
|||
else: |
|||
self.pid_speed_linear_y(speed_y) |
|||
self.twist.linear.y = target_acceleration_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 |
|||
self.last_values_x = np.concatenate((self.last_values_x[1:7], [self.d])) |
|||
target_acceleration_x = self.pid_linear_x(self.d) |
|||
speed_x = 0 |
|||
self.pid_speed_linear_x.setpoint = target_acceleration_x |
|||
speed_x = self.last_values_x.dot(self.savgol_filter) / self.sample_time |
|||
if self.double_loop: |
|||
self.twist.linear.x = self.pid_speed_linear_x(speed_x) |
|||
else: |
|||
self.pid_speed_linear_x(speed_x) |
|||
self.twist.linear.x = target_acceleration_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 = speed_x |
|||
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.sample_time = 0.20 |
|||
self.double_loop = True |
|||
|
|||
self.pid_angular_z = PID( |
|||
1, |
|||
0, |
|||
0, |
|||
auto_mode=True, |
|||
sample_time=self.sample_time |
|||
) |
|||
self.pid_linear_z = PID( |
|||
1, |
|||
0, |
|||
0, |
|||
auto_mode=True, |
|||
sample_time=self.sample_time |
|||
) |
|||
self.pid_linear_y = PID( |
|||
1, |
|||
0, |
|||
0, |
|||
auto_mode=True, |
|||
sample_time=self.sample_time |
|||
) |
|||
self.pid_speed_linear_y = PID( |
|||
1, |
|||
0, |
|||
0, |
|||
auto_mode=True, |
|||
sample_time=self.sample_time |
|||
) |
|||
self.pid_linear_x = PID( |
|||
1, |
|||
0, |
|||
0, |
|||
auto_mode=True, |
|||
sample_time=self.sample_time, |
|||
setpoint=self.target_distance, |
|||
) |
|||
self.pid_speed_linear_x = PID( |
|||
1, |
|||
0, |
|||
0, |
|||
auto_mode=True, |
|||
sample_time=self.sample_time |
|||
) |
|||
|
|||
self.savgol_filter = 1.0/28 * np.array([ |
|||
[-3], |
|||
[-2], |
|||
[-1], |
|||
[0], |
|||
[1], |
|||
[2], |
|||
[3] |
|||
], dtype=np.float64) |
|||
|
|||
self.last_values_x = np.zeros(7, dtype=np.float64) |
|||
self.last_values_y = np.zeros(7, dtype=np.float64) |
|||
|
|||
self.speed_corrector_x = 30 |
|||
self.speed_corrector_y = 30 |
|||
|
|||
# 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() |
|||
Loading…
Reference in new issue