From 875e6fae243e3592fa7290ad2516f0225585a0a1 Mon Sep 17 00:00:00 2001 From: Hugo LEVY-FALK Date: Sat, 25 May 2019 21:33:36 +0200 Subject: [PATCH] triangle_control -> triangle --- .../launch/bebop-triangle-control.launch | 2 +- .../src/detect_targets/scripts/triangle.py | 107 ++++++ .../scripts/triangle_control.py | 256 ++++++++++++- .../scripts/triangle_control_save.py | 341 ------------------ 4 files changed, 353 insertions(+), 353 deletions(-) create mode 100755 workspace/src/detect_targets/scripts/triangle.py mode change 100755 => 100644 workspace/src/detect_targets/scripts/triangle_control.py delete mode 100644 workspace/src/detect_targets/scripts/triangle_control_save.py diff --git a/workspace/src/detect_targets/launch/bebop-triangle-control.launch b/workspace/src/detect_targets/launch/bebop-triangle-control.launch index 6f7817a..4597596 100644 --- a/workspace/src/detect_targets/launch/bebop-triangle-control.launch +++ b/workspace/src/detect_targets/launch/bebop-triangle-control.launch @@ -5,7 +5,7 @@ - + diff --git a/workspace/src/detect_targets/scripts/triangle.py b/workspace/src/detect_targets/scripts/triangle.py new file mode 100755 index 0000000..7132391 --- /dev/null +++ b/workspace/src/detect_targets/scripts/triangle.py @@ -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() diff --git a/workspace/src/detect_targets/scripts/triangle_control.py b/workspace/src/detect_targets/scripts/triangle_control.py old mode 100755 new mode 100644 index f034241..8fcda00 --- a/workspace/src/detect_targets/scripts/triangle_control.py +++ b/workspace/src/detect_targets/scripts/triangle_control.py @@ -8,12 +8,12 @@ import numpy as np import roslib import rospy - -from std_msgs.msg import Float64 +from geometry_msgs.msg import Twist import tf -import dynamic_reconfigure.server +from simple_pid import PID +import dynamic_reconfigure.server from detect_targets.cfg import TriangleParamConfig from detect_targets.msg import control @@ -29,9 +29,109 @@ class TriangleControl: 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] @@ -43,6 +143,7 @@ class TriangleControl: 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() @@ -65,10 +166,58 @@ class TriangleControl: 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) + 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): @@ -82,15 +231,97 @@ class TriangleControl: 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', Float64, queue_size=1) + 'angular_z_control', control, queue_size=1) self.linear_z_pub = rospy.Publisher( - 'linear_z', Float64, queue_size=1) + 'linear_z_control', control, queue_size=1) self.linear_y_pub = rospy.Publisher( - 'linear_y', Float64, queue_size=1) + 'linear_y_control', control, queue_size=1) self.linear_x_pub = rospy.Publisher( - 'linear_x', Float64, queue_size=1) + 'linear_x_control', control, queue_size=1) self.comp_sub = rospy.Subscriber( "component_centers", component_centers, self.on_comp, queue_size=1) @@ -101,7 +332,10 @@ class TriangleControl: if __name__ == '__main__': + print "running" rospy.init_node('triangle_control', anonymous=True) + print "node created" + triangle = TriangleControl() rospy.spin() diff --git a/workspace/src/detect_targets/scripts/triangle_control_save.py b/workspace/src/detect_targets/scripts/triangle_control_save.py deleted file mode 100644 index 8fcda00..0000000 --- a/workspace/src/detect_targets/scripts/triangle_control_save.py +++ /dev/null @@ -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()