klafyvel/drone-rigide
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

utilisation de simple-pid.

Browse source
This commit is contained in:
Hugo LEVY-FALK 2019-03-11 17:46:36 +01:00
parent eeb352a2a4
commit 7c56e5362a
7 changed files with 389 additions and 319 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

283
workspace/build/drone_demo/catkin_generated/installspace/triangle_control.py
View file

@ -10,6 +10,7 @@ import tf
import sigsim import sigsim
import device import device
from simple_pid import PID
import dynamic_reconfigure.server import dynamic_reconfigure.server
from drone_demo.cfg import TriangleParamConfig from drone_demo.cfg import TriangleParamConfig
@ -17,59 +18,73 @@ from drone_demo.msg import control
from vqimg.msg import component_centers from vqimg.msg import component_centers
class TriangleControl:
class TriangleControl:
def on_reconf(self, config, level): def on_reconf(self, config, level):
self.camera_angle = config['camera_angle']*math.pi/360.0 # theta/2 self.camera_angle = config['camera_angle']*math.pi/360.0 # theta/2
self.tan_cam = math.tan(self.camera_angle) self.tan_cam = math.tan(self.camera_angle)
self.target_width = config['target_width'] self.target_width = config['target_width']
self.target_depth = config['target_depth'] self.target_depth = config['target_depth']
self.target_distance = config['distance_to_target'] self.target_distance = config['distance_to_target']
self.max_speed = config['max_speed'] self.max_speed = config['max_speed']
self.angular_z_Kp = config['angular_z_Kp'] self.pid_angular_z.Kp = config['angular_z_Kp']
self.angular_z_Kd = config['angular_z_Kd'] self.pid_angular_z.Ki = config['angular_z_Ki']
self.control_angular_z = config['control_angular_z'] 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.pid_linear_z.Kp = config['linear_z_Kp']
self.linear_y_Kd = config['linear_y_Kd'] self.pid_linear_z.Ki = config['linear_z_Ki']
self.control_linear_y = config['control_linear_y'] 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.pid_linear_x.Kp = config['linear_x_Kp']
self.linear_z_Kd = config['linear_z_Kd'] self.pid_linear_x.Ki = config['linear_x_Ki']
self.control_linear_z = config['control_linear_z'] self.pid_linear_x.Kd = config['linear_x_Kd']
self.pid_linear_x.auto_mode = config['control_linear_x']
self.linear_x_Kp = config['linear_x_Kp'] self.pid_linear_x.output_limits = (
self.linear_x_Kd = config['linear_x_Kd'] -config['max_speed'],
self.control_linear_x = config['control_linear_x'] config['max_speed']
)
self.pid_linear_x.setpoint = self.target_distance
return config return config
def clear_controls(self) : def clear_controls(self):
self.error_angular_z.clear() self.error_angular_z.clear()
self.error_linear_z.clear() self.error_linear_z.clear()
self.error_linear_y.clear() self.error_linear_y.clear()
self.error_linear_x.clear() self.error_linear_x.clear()
def saturate_twist() : def saturate_twist():
if self.twist.linear.x > self.max_speed : if self.twist.linear.x > self.max_speed:
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 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 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 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 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 self.twist.linear.z = - self.max_speed
def on_comp(self, msg): def on_comp(self, msg):
self.twist = Twist() self.twist = Twist()
if len(msg.data) > 2: if len(msg.data) > 2:
@ -90,157 +105,137 @@ class TriangleControl:
t = (now - self.first_time).to_sec() t = (now - self.first_time).to_sec()
self.Gx = (L.x + H.x + R.x)*.333333 self.Gx = (L.x + H.x + R.x)*.333333
Gy = (L.y + H.y + R.y)*.333333 Gy = (L.y + H.y + R.y)*.333333
w = R.x - L.x w = R.x - L.x
h = H.x - .5 * (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)) self.alpha = math.atan(h*self.target_width/(1e-5+w*self.target_depth))
ca = math.cos(self.alpha) ca = math.cos(self.alpha)
sa = math.sin(self.alpha) sa = math.sin(self.alpha)
self.d = self.target_width*ca/(w*self.tan_cam) *.5 # why *.5.... I don't know. # why *.5.... I don't know.
self.z = -Gy*self.d*self.tan_cam 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('d = {}, z = {}, alpha = {}'.format(self.d, self.z, self.alpha*180/math.pi))
#print('w = {}, h = {}, Gy = {}'.format(w, h, Gy)) #print('w = {}, h = {}, Gy = {}'.format(w, h, Gy))
#print('L = {}, l = {}'.format(self.target_width, self.target_depth)) #print('L = {}, l = {}'.format(self.target_width, self.target_depth))
self.br.sendTransform((self.d * ca, self.d * sa, self.z), 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, now,
'drone', 'target') 'drone', 'target')
if self.control_angular_z: self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
dt = t-self.last_time_angular_z if self.angular_z_pub.get_num_connections() > 0:
self.last_time_angular_z = t self.angular_z_info.target = 0
self.error_angular_z.next(dt) self.angular_z_info.error = 0
self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1] self.angular_z_info.derror = 0
if self.angular_z_pub.get_num_connections() > 0 : self.angular_z_info.cmd_vel = self.twist.angular.z
self.angular_z_info.target = 0 self.angular_z_pub.publish(self.angular_z_info)
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.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): def __init__(self):
self.Gx = 0 self.Gx = 0
self.alpha = 0 self.alpha = 0
self.d = 0 self.d = 0
self.z = 0 self.z = 0
self.camera_angle = 80*math.pi/180./2.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_width = 1
self.target_depth = .2 self.target_depth = .2
self.target_distance = 2 self.target_distance = 2
self.max_speed = .3 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.pid_angular_z = PID(
self.last_time_linear_z = 0 1,
self.last_time_linear_y = 0 0,
self.last_time_linear_x = 0 0,
self.first_time = rospy.Time.now() auto_mode=True,
sample_time=0.14 # 7 Hz
# PD params )
self.pid_linear_z = PID(
self.angular_z_Kp = .1 1,
self.angular_z_Kd = .1 0,
self.control_angular_z = True 0,
auto_mode=True,
self.linear_y_Kp = .1 sample_time=0.14
self.linear_y_Kd = .1 )
self.control_linear_y = True self.pid_linear_y = PID(
1,
self.linear_z_Kp = .1 0,
self.linear_z_Kd = .1 0,
self.control_linear_z = True auto_mode=True,
sample_time=0.14
self.linear_x_Kp = .1 )
self.linear_x_Kd = .1 self.pid_linear_x = PID(
self.control_linear_x = True 1,
0,
0,
auto_mode=True,
sample_time=0.14,
setpoint=self.target_distance,
)
# Control info # Control info
self.angular_z_info = control() self.angular_z_info = control()
self.linear_x_info = control() self.linear_x_info = control()
self.linear_y_info = control() self.linear_y_info = control()
self.linear_z_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)
# ROS stuff # ROS stuff
self.twist = Twist() self.twist = Twist()
self.twist_pub = rospy.Publisher ('cmd_vel', Twist, queue_size = 1) self.twist_pub = rospy.Publisher(
self.angular_z_pub = rospy.Publisher ('angular_z_control', control, queue_size = 1) 'cmd_vel', Twist, queue_size=1)
self.linear_z_pub = rospy.Publisher ('linear_z_control', control, queue_size = 1) self.angular_z_pub = rospy.Publisher(
self.linear_y_pub = rospy.Publisher ('linear_y_control', control, queue_size = 1) 'angular_z_control', control, queue_size=1)
self.linear_x_pub = rospy.Publisher ('linear_x_control', control, queue_size = 1) self.linear_z_pub = rospy.Publisher(
self.comp_sub = rospy.Subscriber("component_centers", component_centers, self.on_comp, queue_size = 1) '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__': if __name__ == '__main__':
print "running" print "running"

283
workspace/build/drone_demo/catkin_generated/stamps/drone_demo/triangle_control.py.stamp
View file

@ -10,6 +10,7 @@ import tf
import sigsim import sigsim
import device import device
from simple_pid import PID
import dynamic_reconfigure.server import dynamic_reconfigure.server
from drone_demo.cfg import TriangleParamConfig from drone_demo.cfg import TriangleParamConfig
@ -17,59 +18,73 @@ from drone_demo.msg import control
from vqimg.msg import component_centers from vqimg.msg import component_centers
class TriangleControl:
class TriangleControl:
def on_reconf(self, config, level): def on_reconf(self, config, level):
self.camera_angle = config['camera_angle']*math.pi/360.0 # theta/2 self.camera_angle = config['camera_angle']*math.pi/360.0 # theta/2
self.tan_cam = math.tan(self.camera_angle) self.tan_cam = math.tan(self.camera_angle)
self.target_width = config['target_width'] self.target_width = config['target_width']
self.target_depth = config['target_depth'] self.target_depth = config['target_depth']
self.target_distance = config['distance_to_target'] self.target_distance = config['distance_to_target']
self.max_speed = config['max_speed'] self.max_speed = config['max_speed']
self.angular_z_Kp = config['angular_z_Kp'] self.pid_angular_z.Kp = config['angular_z_Kp']
self.angular_z_Kd = config['angular_z_Kd'] self.pid_angular_z.Ki = config['angular_z_Ki']
self.control_angular_z = config['control_angular_z'] 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.pid_linear_z.Kp = config['linear_z_Kp']
self.linear_y_Kd = config['linear_y_Kd'] self.pid_linear_z.Ki = config['linear_z_Ki']
self.control_linear_y = config['control_linear_y'] 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.pid_linear_x.Kp = config['linear_x_Kp']
self.linear_z_Kd = config['linear_z_Kd'] self.pid_linear_x.Ki = config['linear_x_Ki']
self.control_linear_z = config['control_linear_z'] self.pid_linear_x.Kd = config['linear_x_Kd']
self.pid_linear_x.auto_mode = config['control_linear_x']
self.linear_x_Kp = config['linear_x_Kp'] self.pid_linear_x.output_limits = (
self.linear_x_Kd = config['linear_x_Kd'] -config['max_speed'],
self.control_linear_x = config['control_linear_x'] config['max_speed']
)
self.pid_linear_x.setpoint = self.target_distance
return config return config
def clear_controls(self) : def clear_controls(self):
self.error_angular_z.clear() self.error_angular_z.clear()
self.error_linear_z.clear() self.error_linear_z.clear()
self.error_linear_y.clear() self.error_linear_y.clear()
self.error_linear_x.clear() self.error_linear_x.clear()
def saturate_twist() : def saturate_twist():
if self.twist.linear.x > self.max_speed : if self.twist.linear.x > self.max_speed:
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 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 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 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 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 self.twist.linear.z = - self.max_speed
def on_comp(self, msg): def on_comp(self, msg):
self.twist = Twist() self.twist = Twist()
if len(msg.data) > 2: if len(msg.data) > 2:
@ -90,157 +105,137 @@ class TriangleControl:
t = (now - self.first_time).to_sec() t = (now - self.first_time).to_sec()
self.Gx = (L.x + H.x + R.x)*.333333 self.Gx = (L.x + H.x + R.x)*.333333
Gy = (L.y + H.y + R.y)*.333333 Gy = (L.y + H.y + R.y)*.333333
w = R.x - L.x w = R.x - L.x
h = H.x - .5 * (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)) self.alpha = math.atan(h*self.target_width/(1e-5+w*self.target_depth))
ca = math.cos(self.alpha) ca = math.cos(self.alpha)
sa = math.sin(self.alpha) sa = math.sin(self.alpha)
self.d = self.target_width*ca/(w*self.tan_cam) *.5 # why *.5.... I don't know. # why *.5.... I don't know.
self.z = -Gy*self.d*self.tan_cam 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('d = {}, z = {}, alpha = {}'.format(self.d, self.z, self.alpha*180/math.pi))
#print('w = {}, h = {}, Gy = {}'.format(w, h, Gy)) #print('w = {}, h = {}, Gy = {}'.format(w, h, Gy))
#print('L = {}, l = {}'.format(self.target_width, self.target_depth)) #print('L = {}, l = {}'.format(self.target_width, self.target_depth))
self.br.sendTransform((self.d * ca, self.d * sa, self.z), 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, now,
'drone', 'target') 'drone', 'target')
if self.control_angular_z: self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
dt = t-self.last_time_angular_z if self.angular_z_pub.get_num_connections() > 0:
self.last_time_angular_z = t self.angular_z_info.target = 0
self.error_angular_z.next(dt) self.angular_z_info.error = 0
self.twist.angular.z = self.angular_z_Kp * self.error_angular_z[0] + self.angular_z_Kd * self.error_angular_z[1] self.angular_z_info.derror = 0
if self.angular_z_pub.get_num_connections() > 0 : self.angular_z_info.cmd_vel = self.twist.angular.z
self.angular_z_info.target = 0 self.angular_z_pub.publish(self.angular_z_info)
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.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): def __init__(self):
self.Gx = 0 self.Gx = 0
self.alpha = 0 self.alpha = 0
self.d = 0 self.d = 0
self.z = 0 self.z = 0
self.camera_angle = 80*math.pi/180./2.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_width = 1
self.target_depth = .2 self.target_depth = .2
self.target_distance = 2 self.target_distance = 2
self.max_speed = .3 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.pid_angular_z = PID(
self.last_time_linear_z = 0 1,
self.last_time_linear_y = 0 0,
self.last_time_linear_x = 0 0,
self.first_time = rospy.Time.now() auto_mode=True,
sample_time=0.14 # 7 Hz
# PD params )
self.pid_linear_z = PID(
self.angular_z_Kp = .1 1,
self.angular_z_Kd = .1 0,
self.control_angular_z = True 0,
auto_mode=True,
self.linear_y_Kp = .1 sample_time=0.14
self.linear_y_Kd = .1 )
self.control_linear_y = True self.pid_linear_y = PID(
1,
self.linear_z_Kp = .1 0,
self.linear_z_Kd = .1 0,
self.control_linear_z = True auto_mode=True,
sample_time=0.14
self.linear_x_Kp = .1 )
self.linear_x_Kd = .1 self.pid_linear_x = PID(
self.control_linear_x = True 1,
0,
0,
auto_mode=True,
sample_time=0.14,
setpoint=self.target_distance,
)
# Control info # Control info
self.angular_z_info = control() self.angular_z_info = control()
self.linear_x_info = control() self.linear_x_info = control()
self.linear_y_info = control() self.linear_y_info = control()
self.linear_z_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)
# ROS stuff # ROS stuff
self.twist = Twist() self.twist = Twist()
self.twist_pub = rospy.Publisher ('cmd_vel', Twist, queue_size = 1) self.twist_pub = rospy.Publisher(
self.angular_z_pub = rospy.Publisher ('angular_z_control', control, queue_size = 1) 'cmd_vel', Twist, queue_size=1)
self.linear_z_pub = rospy.Publisher ('linear_z_control', control, queue_size = 1) self.angular_z_pub = rospy.Publisher(
self.linear_y_pub = rospy.Publisher ('linear_y_control', control, queue_size = 1) 'angular_z_control', control, queue_size=1)
self.linear_x_pub = rospy.Publisher ('linear_x_control', control, queue_size = 1) self.linear_z_pub = rospy.Publisher(
self.comp_sub = rospy.Subscriber("component_centers", component_centers, self.on_comp, queue_size = 1) '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__': if __name__ == '__main__':
print "running" print "running"

56
workspace/devel/.private/drone_demo/include/drone_demo/TriangleParamConfig.h
View file

@ -242,15 +242,19 @@ class DEFAULT
if("distance_to_target"==(*_i)->name){distance_to_target = boost::any_cast<double>(val);} if("distance_to_target"==(*_i)->name){distance_to_target = boost::any_cast<double>(val);}
if("max_speed"==(*_i)->name){max_speed = boost::any_cast<double>(val);} if("max_speed"==(*_i)->name){max_speed = boost::any_cast<double>(val);}
if("linear_x_Kp"==(*_i)->name){linear_x_Kp = boost::any_cast<double>(val);} if("linear_x_Kp"==(*_i)->name){linear_x_Kp = boost::any_cast<double>(val);}
if("linear_x_Ki"==(*_i)->name){linear_x_Ki = boost::any_cast<double>(val);}
if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast<double>(val);} if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast<double>(val);}
if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast<bool>(val);} if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast<bool>(val);}
if("linear_y_Kp"==(*_i)->name){linear_y_Kp = boost::any_cast<double>(val);} if("linear_y_Kp"==(*_i)->name){linear_y_Kp = boost::any_cast<double>(val);}
if("linear_y_Ki"==(*_i)->name){linear_y_Ki = boost::any_cast<double>(val);}
if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast<double>(val);} if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast<double>(val);}
if("control_linear_y"==(*_i)->name){control_linear_y = boost::any_cast<bool>(val);} if("control_linear_y"==(*_i)->name){control_linear_y = boost::any_cast<bool>(val);}
if("linear_z_Kp"==(*_i)->name){linear_z_Kp = boost::any_cast<double>(val);} if("linear_z_Kp"==(*_i)->name){linear_z_Kp = boost::any_cast<double>(val);}
if("linear_z_Ki"==(*_i)->name){linear_z_Ki = boost::any_cast<double>(val);}
if("linear_z_Kd"==(*_i)->name){linear_z_Kd = boost::any_cast<double>(val);} if("linear_z_Kd"==(*_i)->name){linear_z_Kd = boost::any_cast<double>(val);}
if("control_linear_z"==(*_i)->name){control_linear_z = boost::any_cast<bool>(val);} if("control_linear_z"==(*_i)->name){control_linear_z = boost::any_cast<bool>(val);}
if("angular_z_Kp"==(*_i)->name){angular_z_Kp = boost::any_cast<double>(val);} if("angular_z_Kp"==(*_i)->name){angular_z_Kp = boost::any_cast<double>(val);}
if("angular_z_Ki"==(*_i)->name){angular_z_Ki = boost::any_cast<double>(val);}
if("angular_z_Kd"==(*_i)->name){angular_z_Kd = boost::any_cast<double>(val);} if("angular_z_Kd"==(*_i)->name){angular_z_Kd = boost::any_cast<double>(val);}
if("control_angular_z"==(*_i)->name){control_angular_z = boost::any_cast<bool>(val);} if("control_angular_z"==(*_i)->name){control_angular_z = boost::any_cast<bool>(val);}
} }
@ -262,15 +266,19 @@ double target_depth;
double distance_to_target; double distance_to_target;
double max_speed; double max_speed;
double linear_x_Kp; double linear_x_Kp;
double linear_x_Ki;
double linear_x_Kd; double linear_x_Kd;
bool control_linear_x; bool control_linear_x;
double linear_y_Kp; double linear_y_Kp;
double linear_y_Ki;
double linear_y_Kd; double linear_y_Kd;
bool control_linear_y; bool control_linear_y;
double linear_z_Kp; double linear_z_Kp;
double linear_z_Ki;
double linear_z_Kd; double linear_z_Kd;
bool control_linear_z; bool control_linear_z;
double angular_z_Kp; double angular_z_Kp;
double angular_z_Ki;
double angular_z_Kd; double angular_z_Kd;
bool control_angular_z; bool control_angular_z;
@ -294,24 +302,32 @@ bool control_angular_z;
double max_speed; double max_speed;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double linear_x_Kp; 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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double linear_x_Kd; double linear_x_Kd;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
bool control_linear_x; bool control_linear_x;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double linear_y_Kp; 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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double linear_y_Kd; double linear_y_Kd;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
bool control_linear_y; bool control_linear_y;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double linear_z_Kp; 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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double linear_z_Kd; double linear_z_Kd;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
bool control_linear_z; bool control_linear_z;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double angular_z_Kp; 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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
double angular_z_Kd; double angular_z_Kd;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@ -514,6 +530,16 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::linear_x_Kp))); Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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" //#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<double>("linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::linear_x_Kp))); __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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<double>("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<double>("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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
__min__.linear_x_Kd = 0.0; __min__.linear_x_Kd = 0.0;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@ -544,6 +570,16 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::linear_y_Kp))); Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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" //#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<double>("linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::linear_y_Kp))); __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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<double>("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<double>("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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
__min__.linear_y_Kd = 0.0; __min__.linear_y_Kd = 0.0;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@ -574,6 +610,16 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("linear_z_Kp", "double", 0, "linear.z controller Kp", "", &TriangleParamConfig::linear_z_Kp))); Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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" //#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<double>("linear_z_Kp", "double", 0, "linear.z controller Kp", "", &TriangleParamConfig::linear_z_Kp))); __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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<double>("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<double>("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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
__min__.linear_z_Kd = 0.0; __min__.linear_z_Kd = 0.0;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@ -604,6 +650,16 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("angular_z_Kp", "double", 0, "angular.z controller Kp", "", &TriangleParamConfig::angular_z_Kp))); Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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" //#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<double>("angular_z_Kp", "double", 0, "angular.z controller Kp", "", &TriangleParamConfig::angular_z_Kp))); __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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<double>("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<double>("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" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
__min__.angular_z_Kd = 0.0; __min__.angular_z_Kd = 0.0;
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"

4
workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig-usage.dox
View file

@ -7,15 +7,19 @@
<param name="distance_to_target" type="double" value="2.0" /> <param name="distance_to_target" type="double" value="2.0" />
<param name="max_speed" type="double" value="0.3" /> <param name="max_speed" type="double" value="0.3" />
<param name="linear_x_Kp" type="double" value="0.1" /> <param name="linear_x_Kp" type="double" value="0.1" />
<param name="linear_x_Ki" type="double" value="0.1" />
<param name="linear_x_Kd" type="double" value="0.1" /> <param name="linear_x_Kd" type="double" value="0.1" />
<param name="control_linear_x" type="bool" value="True" /> <param name="control_linear_x" type="bool" value="True" />
<param name="linear_y_Kp" type="double" value="0.1" /> <param name="linear_y_Kp" type="double" value="0.1" />
<param name="linear_y_Ki" type="double" value="0.1" />
<param name="linear_y_Kd" type="double" value="0.1" /> <param name="linear_y_Kd" type="double" value="0.1" />
<param name="control_linear_y" type="bool" value="True" /> <param name="control_linear_y" type="bool" value="True" />
<param name="linear_z_Kp" type="double" value="0.1" /> <param name="linear_z_Kp" type="double" value="0.1" />
<param name="linear_z_Ki" type="double" value="0.1" />
<param name="linear_z_Kd" type="double" value="0.1" /> <param name="linear_z_Kd" type="double" value="0.1" />
<param name="control_linear_z" type="bool" value="True" /> <param name="control_linear_z" type="bool" value="True" />
<param name="angular_z_Kp" type="double" value="0.1" /> <param name="angular_z_Kp" type="double" value="0.1" />
<param name="angular_z_Ki" type="double" value="0.1" />
<param name="angular_z_Kd" type="double" value="0.1" /> <param name="angular_z_Kd" type="double" value="0.1" />
<param name="control_angular_z" type="bool" value="True" /> <param name="control_angular_z" type="bool" value="True" />
</node> </node>

4
workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.dox
View file

@ -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 "~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 "~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_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 "~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 "~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_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 "~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 "~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_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 "~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 "~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_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 "~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 - \b "~control_angular_z" : \b [bool] Controls the facing to target min: False, default: True, max: True

78
workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.wikidoc
View file

@ -27,50 +27,66 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
5.default= 0.1 5.default= 0.1
5.type= double 5.type= double
5.desc=linear.x controller Kp Range: 0.0 to 2.0 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.default= 0.1
6.type= double 6.type= double
6.desc=linear.x controller Kd Range: 0.0 to 2.0 6.desc=linear.x controller Ki Range: 0.0 to 2.0
7.name= ~control_linear_x 7.name= ~linear_x_Kd
7.default= True 7.default= 0.1
7.type= bool 7.type= double
7.desc=Control distance to target 7.desc=linear.x controller Kd Range: 0.0 to 2.0
8.name= ~linear_y_Kp 8.name= ~control_linear_x
8.default= 0.1 8.default= True
8.type= double 8.type= bool
8.desc=linear.y controller Kp Range: 0.0 to 2.0 8.desc=Control distance to target
9.name= ~linear_y_Kd 9.name= ~linear_y_Kp
9.default= 0.1 9.default= 0.1
9.type= double 9.type= double
9.desc=linear.y controller Kd Range: 0.0 to 2.0 9.desc=linear.y controller Kp Range: 0.0 to 2.0
10.name= ~control_linear_y 10.name= ~linear_y_Ki
10.default= True 10.default= 0.1
10.type= bool 10.type= double
10.desc=Controls the facing to target 10.desc=linear.y controller Ki Range: 0.0 to 2.0
11.name= ~linear_z_Kp 11.name= ~linear_y_Kd
11.default= 0.1 11.default= 0.1
11.type= double 11.type= double
11.desc=linear.z controller Kp Range: 0.0 to 2.0 11.desc=linear.y controller Kd Range: 0.0 to 2.0
12.name= ~linear_z_Kd 12.name= ~control_linear_y
12.default= 0.1 12.default= True
12.type= double 12.type= bool
12.desc=linear.z controller Kd Range: 0.0 to 2.0 12.desc=Controls the facing to target
13.name= ~control_linear_z 13.name= ~linear_z_Kp
13.default= True 13.default= 0.1
13.type= bool 13.type= double
13.desc=Controls the facing to target 13.desc=linear.z controller Kp Range: 0.0 to 2.0
14.name= ~angular_z_Kp 14.name= ~linear_z_Ki
14.default= 0.1 14.default= 0.1
14.type= double 14.type= double
14.desc=angular.z controller Kp Range: 0.0 to 2.0 14.desc=linear.z controller Ki Range: 0.0 to 2.0
15.name= ~angular_z_Kd 15.name= ~linear_z_Kd
15.default= 0.1 15.default= 0.1
15.type= double 15.type= double
15.desc=angular.z controller Kd Range: 0.0 to 2.0 15.desc=linear.z controller Kd Range: 0.0 to 2.0
16.name= ~control_angular_z 16.name= ~control_linear_z
16.default= True 16.default= True
16.type= bool 16.type= bool
16.desc=Controls the facing to target 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. # End of autogenerated section. You may edit below.

BIN
workspace/src/detect_targets/launch/.bebop-triangle-control.launch.swp
View file

Binary file not shown.