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