diff --git a/workspace/build/drone_demo/catkin_generated/installspace/triangle_control.py b/workspace/build/drone_demo/catkin_generated/installspace/triangle_control.py index 5bdce3f..f3e4ded 100644 --- a/workspace/build/drone_demo/catkin_generated/installspace/triangle_control.py +++ b/workspace/build/drone_demo/catkin_generated/installspace/triangle_control.py @@ -10,6 +10,7 @@ import tf import sigsim import device +from simple_pid import PID import dynamic_reconfigure.server from drone_demo.cfg import TriangleParamConfig @@ -17,59 +18,73 @@ from drone_demo.msg import control from vqimg.msg import component_centers -class TriangleControl: +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.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.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.angular_z_Kp = config['angular_z_Kp'] - self.angular_z_Kd = config['angular_z_Kd'] - self.control_angular_z = config['control_angular_z'] + 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.auto_mode = config['control_angular_z'] - self.linear_y_Kp = config['linear_y_Kp'] - self.linear_y_Kd = config['linear_y_Kd'] - self.control_linear_y = config['control_linear_y'] + 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.auto_mode = config['control_linear_z'] + 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.auto_mode = config['control_linear_y'] + self.pid_linear_y.output_limits = ( + -config['max_speed'], + config['max_speed'] + ) - self.linear_z_Kp = config['linear_z_Kp'] - self.linear_z_Kd = config['linear_z_Kd'] - self.control_linear_z = config['control_linear_z'] - - self.linear_x_Kp = config['linear_x_Kp'] - self.linear_x_Kd = config['linear_x_Kd'] - self.control_linear_x = config['control_linear_x'] + 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.auto_mode = config['control_linear_x'] + self.pid_linear_x.output_limits = ( + -config['max_speed'], + config['max_speed'] + ) + self.pid_linear_x.setpoint = self.target_distance return config - def clear_controls(self) : + 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 : + 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 : + elif self.twist.linear.x < - self.max_speed: self.twist.linear.x = - self.max_speed - if self.twist.linear.y > 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 : + elif self.twist.linear.y < - self.max_speed: self.twist.linear.y = - self.max_speed - if self.twist.linear.z > 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 : + 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: @@ -90,157 +105,137 @@ class TriangleControl: 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) + 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) - self.d = self.target_width*ca/(w*self.tan_cam) *.5 # why *.5.... I don't know. - self.z = -Gy*self.d*self.tan_cam + 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('#######') #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), + tf.transformations.quaternion_from_euler( + 0, 0, self.alpha + math.pi), now, 'drone', 'target') - if self.control_angular_z: - dt = t-self.last_time_angular_z - self.last_time_angular_z = t - self.error_angular_z.next(dt) - self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1] - if self.angular_z_pub.get_num_connections() > 0 : - self.angular_z_info.target = 0 - self.angular_z_info.error = self.error_angular_z[0] - self.angular_z_info.derror = self.error_angular_z[1] - self.angular_z_info.cmd_vel = self.twist.angular.z - self.angular_z_pub.publish(self.angular_z_info) - - if self.control_angular_z: - dt = t-self.last_time_angular_z - self.last_time_angular_z = t - self.error_angular_z.next(dt) - self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1] - if self.angular_z_pub.get_num_connections() > 0 : - self.angular_z_info.target = 0 - self.angular_z_info.error = self.error_angular_z[0] - self.angular_z_info.derror = self.error_angular_z[1] - self.angular_z_info.cmd_vel = self.twist.angular.z - self.angular_z_pub.publish(self.angular_z_info) - - if self.control_linear_z: - dt = t-self.last_time_linear_z - self.last_time_linear_z = t - self.error_linear_z.next(dt) - self.twist.linear.z = self.linear_z_Kp * self.error_linear_z[0] + self.linear_z_Kd * self.error_linear_z[1] - if self.linear_z_pub.get_num_connections() > 0 : - self.linear_z_info.target = 0 - self.linear_z_info.error = self.error_linear_z[0] - self.linear_z_info.derror = self.error_linear_z[1] - self.linear_z_info.cmd_vel = self.twist.linear.z - self.linear_z_pub.publish(self.linear_z_info) - - if self.control_linear_y: - dt = t-self.last_time_linear_y - self.last_time_linear_y = t - self.error_linear_y.next(dt) - self.twist.linear.y = self.linear_y_Kp * self.error_linear_y[0] + self.linear_y_Kd * self.error_linear_y[1] - if self.linear_y_pub.get_num_connections() > 0 : - self.linear_y_info.target = 0 - self.linear_y_info.error = self.error_linear_y[0] - self.linear_y_info.derror = self.error_linear_y[1] - self.linear_y_info.cmd_vel = self.twist.linear.y - self.linear_y_pub.publish(self.linear_y_info) - - if self.control_linear_x: - dt = t-self.last_time_linear_x - self.last_time_linear_x = t - self.error_linear_x.next(dt) - self.twist.linear.x = self.linear_x_Kp * self.error_linear_x[0] + self.linear_x_Kd * self.error_linear_x[1] - if self.linear_x_pub.get_num_connections() > 0 : - self.linear_x_info.target = 0 - self.linear_x_info.error = self.error_linear_x[0] - self.linear_x_info.derror = self.error_linear_x[1] - self.linear_x_info.cmd_vel = self.twist.linear.x - self.linear_x_pub.publish(self.linear_x_info) + 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.error_linear_z[0] + self.linear_z_info.derror = self.error_linear_z[1] + self.linear_z_info.cmd_vel = self.twist.linear.z + self.linear_z_pub.publish(self.linear_z_info) + self.twist.linear.y = self.pid_linear_y(self.alpha) + if self.linear_y_pub.get_num_connections() > 0: + self.linear_y_info.target = 0 + self.linear_y_info.error = self.error_linear_y[0] + self.linear_y_info.derror = self.error_linear_y[1] + self.linear_y_info.cmd_vel = self.twist.linear.y + self.linear_y_pub.publish(self.linear_y_info) + self.twist.linear.x = self.pid_linear_x(self.d) + if self.linear_x_pub.get_num_connections() > 0: + self.linear_x_info.target = 0 + self.linear_x_info.error = self.error_linear_x[0] + self.linear_x_info.derror = self.error_linear_x[1] + 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.d = 0 + self.z = 0 self.camera_angle = 80*math.pi/180./2.0 - self.tan_cam = math.tan(self.camera_angle) + 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.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() - - # PD params - - self.angular_z_Kp = .1 - self.angular_z_Kd = .1 - self.control_angular_z = True - - self.linear_y_Kp = .1 - self.linear_y_Kd = .1 - self.control_linear_y = True - - self.linear_z_Kp = .1 - self.linear_z_Kd = .1 - self.control_linear_z = True - - self.linear_x_Kp = .1 - self.linear_x_Kd = .1 - self.control_linear_x = True + 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_linear_x = PID( + 1, + 0, + 0, + auto_mode=True, + sample_time=0.14, + setpoint=self.target_distance, + ) # Control info self.angular_z_info = control() - self.linear_x_info = control() - self.linear_y_info = control() - self.linear_z_info = control() - - # Errors - - self.error_angular_z = sigsim.Smoothed(lambda me : -self.Gx * self.camera_angle , 1, 2, 1) - self.error_linear_z = sigsim.Smoothed(lambda me : -self.z , 1, 2, 1) - self.error_linear_y = sigsim.Smoothed(lambda me : self.alpha , 1, 2, 1) - self.error_linear_x = sigsim.Smoothed(lambda me : self.d - self.target_distance, 1, 2, 1) - + 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.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.config_srv = dynamic_reconfigure.server.Server( + TriangleParamConfig, self.on_reconf) + + self.br = tf.TransformBroadcaster() - self.br = tf.TransformBroadcaster() if __name__ == '__main__': print "running" diff --git a/workspace/build/drone_demo/catkin_generated/stamps/drone_demo/triangle_control.py.stamp b/workspace/build/drone_demo/catkin_generated/stamps/drone_demo/triangle_control.py.stamp index 5bdce3f..f3e4ded 100755 --- a/workspace/build/drone_demo/catkin_generated/stamps/drone_demo/triangle_control.py.stamp +++ b/workspace/build/drone_demo/catkin_generated/stamps/drone_demo/triangle_control.py.stamp @@ -10,6 +10,7 @@ import tf import sigsim import device +from simple_pid import PID import dynamic_reconfigure.server from drone_demo.cfg import TriangleParamConfig @@ -17,59 +18,73 @@ from drone_demo.msg import control from vqimg.msg import component_centers -class TriangleControl: +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.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.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.angular_z_Kp = config['angular_z_Kp'] - self.angular_z_Kd = config['angular_z_Kd'] - self.control_angular_z = config['control_angular_z'] + 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.auto_mode = config['control_angular_z'] - self.linear_y_Kp = config['linear_y_Kp'] - self.linear_y_Kd = config['linear_y_Kd'] - self.control_linear_y = config['control_linear_y'] + 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.auto_mode = config['control_linear_z'] + 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.auto_mode = config['control_linear_y'] + self.pid_linear_y.output_limits = ( + -config['max_speed'], + config['max_speed'] + ) - self.linear_z_Kp = config['linear_z_Kp'] - self.linear_z_Kd = config['linear_z_Kd'] - self.control_linear_z = config['control_linear_z'] - - self.linear_x_Kp = config['linear_x_Kp'] - self.linear_x_Kd = config['linear_x_Kd'] - self.control_linear_x = config['control_linear_x'] + 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.auto_mode = config['control_linear_x'] + self.pid_linear_x.output_limits = ( + -config['max_speed'], + config['max_speed'] + ) + self.pid_linear_x.setpoint = self.target_distance return config - def clear_controls(self) : + 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 : + 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 : + elif self.twist.linear.x < - self.max_speed: self.twist.linear.x = - self.max_speed - if self.twist.linear.y > 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 : + elif self.twist.linear.y < - self.max_speed: self.twist.linear.y = - self.max_speed - if self.twist.linear.z > 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 : + 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: @@ -90,157 +105,137 @@ class TriangleControl: 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) + 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) - self.d = self.target_width*ca/(w*self.tan_cam) *.5 # why *.5.... I don't know. - self.z = -Gy*self.d*self.tan_cam + 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('#######') #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), + tf.transformations.quaternion_from_euler( + 0, 0, self.alpha + math.pi), now, 'drone', 'target') - if self.control_angular_z: - dt = t-self.last_time_angular_z - self.last_time_angular_z = t - self.error_angular_z.next(dt) - self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1] - if self.angular_z_pub.get_num_connections() > 0 : - self.angular_z_info.target = 0 - self.angular_z_info.error = self.error_angular_z[0] - self.angular_z_info.derror = self.error_angular_z[1] - self.angular_z_info.cmd_vel = self.twist.angular.z - self.angular_z_pub.publish(self.angular_z_info) - - if self.control_angular_z: - dt = t-self.last_time_angular_z - self.last_time_angular_z = t - self.error_angular_z.next(dt) - self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1] - if self.angular_z_pub.get_num_connections() > 0 : - self.angular_z_info.target = 0 - self.angular_z_info.error = self.error_angular_z[0] - self.angular_z_info.derror = self.error_angular_z[1] - self.angular_z_info.cmd_vel = self.twist.angular.z - self.angular_z_pub.publish(self.angular_z_info) - - if self.control_linear_z: - dt = t-self.last_time_linear_z - self.last_time_linear_z = t - self.error_linear_z.next(dt) - self.twist.linear.z = self.linear_z_Kp * self.error_linear_z[0] + self.linear_z_Kd * self.error_linear_z[1] - if self.linear_z_pub.get_num_connections() > 0 : - self.linear_z_info.target = 0 - self.linear_z_info.error = self.error_linear_z[0] - self.linear_z_info.derror = self.error_linear_z[1] - self.linear_z_info.cmd_vel = self.twist.linear.z - self.linear_z_pub.publish(self.linear_z_info) - - if self.control_linear_y: - dt = t-self.last_time_linear_y - self.last_time_linear_y = t - self.error_linear_y.next(dt) - self.twist.linear.y = self.linear_y_Kp * self.error_linear_y[0] + self.linear_y_Kd * self.error_linear_y[1] - if self.linear_y_pub.get_num_connections() > 0 : - self.linear_y_info.target = 0 - self.linear_y_info.error = self.error_linear_y[0] - self.linear_y_info.derror = self.error_linear_y[1] - self.linear_y_info.cmd_vel = self.twist.linear.y - self.linear_y_pub.publish(self.linear_y_info) - - if self.control_linear_x: - dt = t-self.last_time_linear_x - self.last_time_linear_x = t - self.error_linear_x.next(dt) - self.twist.linear.x = self.linear_x_Kp * self.error_linear_x[0] + self.linear_x_Kd * self.error_linear_x[1] - if self.linear_x_pub.get_num_connections() > 0 : - self.linear_x_info.target = 0 - self.linear_x_info.error = self.error_linear_x[0] - self.linear_x_info.derror = self.error_linear_x[1] - self.linear_x_info.cmd_vel = self.twist.linear.x - self.linear_x_pub.publish(self.linear_x_info) + 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.error_linear_z[0] + self.linear_z_info.derror = self.error_linear_z[1] + self.linear_z_info.cmd_vel = self.twist.linear.z + self.linear_z_pub.publish(self.linear_z_info) + self.twist.linear.y = self.pid_linear_y(self.alpha) + if self.linear_y_pub.get_num_connections() > 0: + self.linear_y_info.target = 0 + self.linear_y_info.error = self.error_linear_y[0] + self.linear_y_info.derror = self.error_linear_y[1] + self.linear_y_info.cmd_vel = self.twist.linear.y + self.linear_y_pub.publish(self.linear_y_info) + self.twist.linear.x = self.pid_linear_x(self.d) + if self.linear_x_pub.get_num_connections() > 0: + self.linear_x_info.target = 0 + self.linear_x_info.error = self.error_linear_x[0] + self.linear_x_info.derror = self.error_linear_x[1] + 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.d = 0 + self.z = 0 self.camera_angle = 80*math.pi/180./2.0 - self.tan_cam = math.tan(self.camera_angle) + 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.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() - - # PD params - - self.angular_z_Kp = .1 - self.angular_z_Kd = .1 - self.control_angular_z = True - - self.linear_y_Kp = .1 - self.linear_y_Kd = .1 - self.control_linear_y = True - - self.linear_z_Kp = .1 - self.linear_z_Kd = .1 - self.control_linear_z = True - - self.linear_x_Kp = .1 - self.linear_x_Kd = .1 - self.control_linear_x = True + 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_linear_x = PID( + 1, + 0, + 0, + auto_mode=True, + sample_time=0.14, + setpoint=self.target_distance, + ) # Control info self.angular_z_info = control() - self.linear_x_info = control() - self.linear_y_info = control() - self.linear_z_info = control() - - # Errors - - self.error_angular_z = sigsim.Smoothed(lambda me : -self.Gx * self.camera_angle , 1, 2, 1) - self.error_linear_z = sigsim.Smoothed(lambda me : -self.z , 1, 2, 1) - self.error_linear_y = sigsim.Smoothed(lambda me : self.alpha , 1, 2, 1) - self.error_linear_x = sigsim.Smoothed(lambda me : self.d - self.target_distance, 1, 2, 1) - + 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.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.config_srv = dynamic_reconfigure.server.Server( + TriangleParamConfig, self.on_reconf) + + self.br = tf.TransformBroadcaster() - self.br = tf.TransformBroadcaster() if __name__ == '__main__': print "running" diff --git a/workspace/devel/.private/drone_demo/include/drone_demo/TriangleParamConfig.h b/workspace/devel/.private/drone_demo/include/drone_demo/TriangleParamConfig.h index d4d15dc..c6008fc 100644 --- a/workspace/devel/.private/drone_demo/include/drone_demo/TriangleParamConfig.h +++ b/workspace/devel/.private/drone_demo/include/drone_demo/TriangleParamConfig.h @@ -242,15 +242,19 @@ class DEFAULT if("distance_to_target"==(*_i)->name){distance_to_target = boost::any_cast(val);} if("max_speed"==(*_i)->name){max_speed = boost::any_cast(val);} if("linear_x_Kp"==(*_i)->name){linear_x_Kp = boost::any_cast(val);} + if("linear_x_Ki"==(*_i)->name){linear_x_Ki = boost::any_cast(val);} if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast(val);} if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast(val);} if("linear_y_Kp"==(*_i)->name){linear_y_Kp = boost::any_cast(val);} + if("linear_y_Ki"==(*_i)->name){linear_y_Ki = boost::any_cast(val);} if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast(val);} if("control_linear_y"==(*_i)->name){control_linear_y = boost::any_cast(val);} if("linear_z_Kp"==(*_i)->name){linear_z_Kp = boost::any_cast(val);} + if("linear_z_Ki"==(*_i)->name){linear_z_Ki = boost::any_cast(val);} if("linear_z_Kd"==(*_i)->name){linear_z_Kd = boost::any_cast(val);} if("control_linear_z"==(*_i)->name){control_linear_z = boost::any_cast(val);} if("angular_z_Kp"==(*_i)->name){angular_z_Kp = boost::any_cast(val);} + if("angular_z_Ki"==(*_i)->name){angular_z_Ki = boost::any_cast(val);} if("angular_z_Kd"==(*_i)->name){angular_z_Kd = boost::any_cast(val);} if("control_angular_z"==(*_i)->name){control_angular_z = boost::any_cast(val);} } @@ -262,15 +266,19 @@ double target_depth; double distance_to_target; double max_speed; double linear_x_Kp; +double linear_x_Ki; double linear_x_Kd; bool control_linear_x; double linear_y_Kp; +double linear_y_Ki; double linear_y_Kd; bool control_linear_y; double linear_z_Kp; +double linear_z_Ki; double linear_z_Kd; bool control_linear_z; double angular_z_Kp; +double angular_z_Ki; double angular_z_Kd; bool control_angular_z; @@ -294,24 +302,32 @@ bool control_angular_z; double max_speed; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_x_Kp; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double linear_x_Ki; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_x_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" bool control_linear_x; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_y_Kp; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double linear_y_Ki; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_y_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" bool control_linear_y; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_z_Kp; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double linear_z_Ki; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_z_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" bool control_linear_z; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double angular_z_Kp; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double angular_z_Ki; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double angular_z_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -514,6 +530,16 @@ TriangleParamConfig::GroupDescription("linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::linear_x_Kp))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::linear_x_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.linear_x_Ki = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.linear_x_Ki = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.linear_x_Ki = 0.1; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_x_Ki", "double", 0, "linear.x controller Ki", "", &TriangleParamConfig::linear_x_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_x_Ki", "double", 0, "linear.x controller Ki", "", &TriangleParamConfig::linear_x_Ki))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.linear_x_Kd = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -544,6 +570,16 @@ TriangleParamConfig::GroupDescription("linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::linear_y_Kp))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::linear_y_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.linear_y_Ki = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.linear_y_Ki = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.linear_y_Ki = 0.1; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_y_Ki", "double", 0, "linear.y controller Ki", "", &TriangleParamConfig::linear_y_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_y_Ki", "double", 0, "linear.y controller Ki", "", &TriangleParamConfig::linear_y_Ki))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.linear_y_Kd = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -574,6 +610,16 @@ TriangleParamConfig::GroupDescription("linear_z_Kp", "double", 0, "linear.z controller Kp", "", &TriangleParamConfig::linear_z_Kp))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_z_Kp", "double", 0, "linear.z controller Kp", "", &TriangleParamConfig::linear_z_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.linear_z_Ki = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.linear_z_Ki = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.linear_z_Ki = 0.1; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_z_Ki", "double", 0, "linear.z controller Ki", "", &TriangleParamConfig::linear_z_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("linear_z_Ki", "double", 0, "linear.z controller Ki", "", &TriangleParamConfig::linear_z_Ki))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.linear_z_Kd = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -604,6 +650,16 @@ TriangleParamConfig::GroupDescription("angular_z_Kp", "double", 0, "angular.z controller Kp", "", &TriangleParamConfig::angular_z_Kp))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("angular_z_Kp", "double", 0, "angular.z controller Kp", "", &TriangleParamConfig::angular_z_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.angular_z_Ki = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.angular_z_Ki = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.angular_z_Ki = 0.1; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("angular_z_Ki", "double", 0, "angular.z controller Ki", "", &TriangleParamConfig::angular_z_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("angular_z_Ki", "double", 0, "angular.z controller Ki", "", &TriangleParamConfig::angular_z_Ki))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.angular_z_Kd = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" diff --git a/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig-usage.dox b/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig-usage.dox index 8914f30..a11835f 100644 --- a/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig-usage.dox +++ b/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig-usage.dox @@ -7,15 +7,19 @@ + + + + diff --git a/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.dox b/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.dox index c327a9c..381cf60 100644 --- a/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.dox +++ b/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.dox @@ -8,15 +8,19 @@ Reads and maintains the following parameters on the ROS server - \b "~distance_to_target" : \b [double] The required distance to the target (m) min: 1.0, default: 2.0, max: 5.0 - \b "~max_speed" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0 - \b "~linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.1, max: 2.0 +- \b "~linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.1, max: 2.0 - \b "~linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.1, max: 2.0 - \b "~control_linear_x" : \b [bool] Control distance to target min: False, default: True, max: True - \b "~linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.1, max: 2.0 +- \b "~linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.1, max: 2.0 - \b "~linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.1, max: 2.0 - \b "~control_linear_y" : \b [bool] Controls the facing to target min: False, default: True, max: True - \b "~linear_z_Kp" : \b [double] linear.z controller Kp min: 0.0, default: 0.1, max: 2.0 +- \b "~linear_z_Ki" : \b [double] linear.z controller Ki min: 0.0, default: 0.1, max: 2.0 - \b "~linear_z_Kd" : \b [double] linear.z controller Kd min: 0.0, default: 0.1, max: 2.0 - \b "~control_linear_z" : \b [bool] Controls the facing to target min: False, default: True, max: True - \b "~angular_z_Kp" : \b [double] angular.z controller Kp min: 0.0, default: 0.1, max: 2.0 +- \b "~angular_z_Ki" : \b [double] angular.z controller Ki min: 0.0, default: 0.1, max: 2.0 - \b "~angular_z_Kd" : \b [double] angular.z controller Kd min: 0.0, default: 0.1, max: 2.0 - \b "~control_angular_z" : \b [bool] Controls the facing to target min: False, default: True, max: True diff --git a/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.wikidoc b/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.wikidoc index fd9ceab..008bf47 100644 --- a/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.wikidoc +++ b/workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.wikidoc @@ -27,50 +27,66 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig 5.default= 0.1 5.type= double 5.desc=linear.x controller Kp Range: 0.0 to 2.0 -6.name= ~linear_x_Kd +6.name= ~linear_x_Ki 6.default= 0.1 6.type= double -6.desc=linear.x controller Kd Range: 0.0 to 2.0 -7.name= ~control_linear_x -7.default= True -7.type= bool -7.desc=Control distance to target -8.name= ~linear_y_Kp -8.default= 0.1 -8.type= double -8.desc=linear.y controller Kp Range: 0.0 to 2.0 -9.name= ~linear_y_Kd +6.desc=linear.x controller Ki Range: 0.0 to 2.0 +7.name= ~linear_x_Kd +7.default= 0.1 +7.type= double +7.desc=linear.x controller Kd Range: 0.0 to 2.0 +8.name= ~control_linear_x +8.default= True +8.type= bool +8.desc=Control distance to target +9.name= ~linear_y_Kp 9.default= 0.1 9.type= double -9.desc=linear.y controller Kd Range: 0.0 to 2.0 -10.name= ~control_linear_y -10.default= True -10.type= bool -10.desc=Controls the facing to target -11.name= ~linear_z_Kp +9.desc=linear.y controller Kp Range: 0.0 to 2.0 +10.name= ~linear_y_Ki +10.default= 0.1 +10.type= double +10.desc=linear.y controller Ki Range: 0.0 to 2.0 +11.name= ~linear_y_Kd 11.default= 0.1 11.type= double -11.desc=linear.z controller Kp Range: 0.0 to 2.0 -12.name= ~linear_z_Kd -12.default= 0.1 -12.type= double -12.desc=linear.z controller Kd Range: 0.0 to 2.0 -13.name= ~control_linear_z -13.default= True -13.type= bool -13.desc=Controls the facing to target -14.name= ~angular_z_Kp +11.desc=linear.y controller Kd Range: 0.0 to 2.0 +12.name= ~control_linear_y +12.default= True +12.type= bool +12.desc=Controls the facing to target +13.name= ~linear_z_Kp +13.default= 0.1 +13.type= double +13.desc=linear.z controller Kp Range: 0.0 to 2.0 +14.name= ~linear_z_Ki 14.default= 0.1 14.type= double -14.desc=angular.z controller Kp Range: 0.0 to 2.0 -15.name= ~angular_z_Kd +14.desc=linear.z controller Ki Range: 0.0 to 2.0 +15.name= ~linear_z_Kd 15.default= 0.1 15.type= double -15.desc=angular.z controller Kd Range: 0.0 to 2.0 -16.name= ~control_angular_z +15.desc=linear.z controller Kd Range: 0.0 to 2.0 +16.name= ~control_linear_z 16.default= True 16.type= bool 16.desc=Controls the facing to target +17.name= ~angular_z_Kp +17.default= 0.1 +17.type= double +17.desc=angular.z controller Kp Range: 0.0 to 2.0 +18.name= ~angular_z_Ki +18.default= 0.1 +18.type= double +18.desc=angular.z controller Ki Range: 0.0 to 2.0 +19.name= ~angular_z_Kd +19.default= 0.1 +19.type= double +19.desc=angular.z controller Kd Range: 0.0 to 2.0 +20.name= ~control_angular_z +20.default= True +20.type= bool +20.desc=Controls the facing to target } } # End of autogenerated section. You may edit below. diff --git a/workspace/src/detect_targets/launch/.bebop-triangle-control.launch.swp b/workspace/src/detect_targets/launch/.bebop-triangle-control.launch.swp deleted file mode 100644 index d65cb46..0000000 Binary files a/workspace/src/detect_targets/launch/.bebop-triangle-control.launch.swp and /dev/null differ