utilisation de simple-pid.
This commit is contained in:
Hugo LEVY-FALK
parent
eeb352a2a4
commit
7c56e5362a
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -242,15 +242,19 @@ class DEFAULT
|
|||
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("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("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_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("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_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("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_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("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 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<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)));
|
||||
//#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)));
|
||||
//#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"
|
||||
__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<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)));
|
||||
//#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)));
|
||||
//#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"
|
||||
__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<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)));
|
||||
//#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)));
|
||||
//#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"
|
||||
__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<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)));
|
||||
//#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)));
|
||||
//#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"
|
||||
__min__.angular_z_Kd = 0.0;
|
||||
//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
|
||||
|
|
|
|||
|
|
@ -7,15 +7,19 @@
|
|||
<param name="distance_to_target" type="double" value="2.0" />
|
||||
<param name="max_speed" type="double" value="0.3" />
|
||||
<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="control_linear_x" type="bool" value="True" />
|
||||
<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="control_linear_y" type="bool" value="True" />
|
||||
<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="control_linear_z" type="bool" value="True" />
|
||||
<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="control_angular_z" type="bool" value="True" />
|
||||
</node>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in a new issue