utilisation de simple-pid.

workspace/build/drone_demo/catkin_generated/installspace/triangle_control.py

@@ -10,6 +10,7 @@ import tf
 
 import sigsim
 import device
+from simple_pid import PID
 
 import dynamic_reconfigure.server
 from drone_demo.cfg import TriangleParamConfig
@@ -17,8 +18,8 @@ from drone_demo.msg import control
 
 from vqimg.msg import component_centers
 
-class TriangleControl:
 
+class TriangleControl:
 
     def on_reconf(self, config, level):
 
@@ -30,21 +31,37 @@ class TriangleControl:
         self.target_distance = config['distance_to_target']
         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
 
@@ -68,8 +85,6 @@ class TriangleControl:
         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:
@@ -96,49 +111,30 @@ class TriangleControl:
         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.
+        # 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('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:
+        self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
-            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.error = 0
-                self.angular_z_info.derror  = self.error_angular_z[1]
+            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)
 
-        if self.control_angular_z:
+        self.twist.linear.z = self.pid_linear_z(self.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]
@@ -146,11 +142,7 @@ class TriangleControl:
             self.linear_z_info.cmd_vel = self.twist.linear.z
             self.linear_z_pub.publish(self.linear_z_info)
 
-        if self.control_linear_y:
+        self.twist.linear.y = self.pid_linear_y(self.alpha)
-            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]
@@ -158,11 +150,7 @@ class TriangleControl:
             self.linear_y_info.cmd_vel = self.twist.linear.y
             self.linear_y_pub.publish(self.linear_y_info)
 
-        if self.control_linear_x:
+        self.twist.linear.x = self.pid_linear_x(self.d)
-            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]
@@ -170,12 +158,8 @@ class TriangleControl:
             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
@@ -189,30 +173,41 @@ class TriangleControl:
         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()
 
-        # PD params
+        self.pid_angular_z = PID(
-
+            1,
-        self.angular_z_Kp      = .1
+            0,
-        self.angular_z_Kd      = .1
+            0,
-        self.control_angular_z = True
+            auto_mode=True,
-
+            sample_time=0.14  # 7 Hz
-        self.linear_y_Kp      = .1
+        )
-        self.linear_y_Kd      = .1
+        self.pid_linear_z = PID(
-        self.control_linear_y = True
+            1,
-
+            0,
-        self.linear_z_Kp      = .1
+            0,
-        self.linear_z_Kd      = .1
+            auto_mode=True,
-        self.control_linear_z = True
+            sample_time=0.14
-
+        )
-        self.linear_x_Kp      = .1
+        self.pid_linear_y = PID(
-        self.linear_x_Kd      = .1
+            1,
-        self.control_linear_x = True
+            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()
@@ -220,28 +215,28 @@ class TriangleControl:
         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)
-
-
         # ROS stuff
 
         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()
 
+
 if __name__ == '__main__':
     print "running"
     rospy.init_node('triangle_control', anonymous=True)

workspace/build/drone_demo/catkin_generated/stamps/drone_demo/triangle_control.py.stamp

@@ -10,6 +10,7 @@ import tf
 
 import sigsim
 import device
+from simple_pid import PID
 
 import dynamic_reconfigure.server
 from drone_demo.cfg import TriangleParamConfig
@@ -17,8 +18,8 @@ from drone_demo.msg import control
 
 from vqimg.msg import component_centers
 
-class TriangleControl:
 
+class TriangleControl:
 
     def on_reconf(self, config, level):
 
@@ -30,21 +31,37 @@ class TriangleControl:
         self.target_distance = config['distance_to_target']
         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
 
@@ -68,8 +85,6 @@ class TriangleControl:
         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:
@@ -96,49 +111,30 @@ class TriangleControl:
         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.
+        # 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('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:
+        self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
-            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.error = 0
-                self.angular_z_info.derror  = self.error_angular_z[1]
+            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)
 
-        if self.control_angular_z:
+        self.twist.linear.z = self.pid_linear_z(self.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]
@@ -146,11 +142,7 @@ class TriangleControl:
             self.linear_z_info.cmd_vel = self.twist.linear.z
             self.linear_z_pub.publish(self.linear_z_info)
 
-        if self.control_linear_y:
+        self.twist.linear.y = self.pid_linear_y(self.alpha)
-            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]
@@ -158,11 +150,7 @@ class TriangleControl:
             self.linear_y_info.cmd_vel = self.twist.linear.y
             self.linear_y_pub.publish(self.linear_y_info)
 
-        if self.control_linear_x:
+        self.twist.linear.x = self.pid_linear_x(self.d)
-            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]
@@ -170,12 +158,8 @@ class TriangleControl:
             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
@@ -189,30 +173,41 @@ class TriangleControl:
         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()
 
-        # PD params
+        self.pid_angular_z = PID(
-
+            1,
-        self.angular_z_Kp      = .1
+            0,
-        self.angular_z_Kd      = .1
+            0,
-        self.control_angular_z = True
+            auto_mode=True,
-
+            sample_time=0.14  # 7 Hz
-        self.linear_y_Kp      = .1
+        )
-        self.linear_y_Kd      = .1
+        self.pid_linear_z = PID(
-        self.control_linear_y = True
+            1,
-
+            0,
-        self.linear_z_Kp      = .1
+            0,
-        self.linear_z_Kd      = .1
+            auto_mode=True,
-        self.control_linear_z = True
+            sample_time=0.14
-
+        )
-        self.linear_x_Kp      = .1
+        self.pid_linear_y = PID(
-        self.linear_x_Kd      = .1
+            1,
-        self.control_linear_x = True
+            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()
@@ -220,28 +215,28 @@ class TriangleControl:
         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)
-
-
         # ROS stuff
 
         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()
 
+
 if __name__ == '__main__':
     print "running"
     rospy.init_node('triangle_control', anonymous=True)

workspace/devel/.private/drone_demo/include/drone_demo/TriangleParamConfig.h

@@ -242,15 +242,19 @@ class DEFAULT
         if("distance_to_target"==(*_i)->name){distance_to_target = boost::any_cast<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"

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

@@ -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>

workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.dox

@@ -8,15 +8,19 @@ Reads and maintains the following parameters on the ROS server
 - \b "~distance_to_target" : \b [double] The required distance to the target (m) min: 1.0, default: 2.0, max: 5.0
 - \b "~max_speed" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0
 - \b "~linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.1, max: 2.0
+- \b "~linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.1, max: 2.0
 - \b "~linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.1, max: 2.0
 - \b "~control_linear_x" : \b [bool] Control distance to target min: False, default: True, max: True
 - \b "~linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.1, max: 2.0
+- \b "~linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.1, max: 2.0
 - \b "~linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.1, max: 2.0
 - \b "~control_linear_y" : \b [bool] Controls the facing to target min: False, default: True, max: True
 - \b "~linear_z_Kp" : \b [double] linear.z controller Kp min: 0.0, default: 0.1, max: 2.0
+- \b "~linear_z_Ki" : \b [double] linear.z controller Ki min: 0.0, default: 0.1, max: 2.0
 - \b "~linear_z_Kd" : \b [double] linear.z controller Kd min: 0.0, default: 0.1, max: 2.0
 - \b "~control_linear_z" : \b [bool] Controls the facing to target min: False, default: True, max: True
 - \b "~angular_z_Kp" : \b [double] angular.z controller Kp min: 0.0, default: 0.1, max: 2.0
+- \b "~angular_z_Ki" : \b [double] angular.z controller Ki min: 0.0, default: 0.1, max: 2.0
 - \b "~angular_z_Kd" : \b [double] angular.z controller Kd min: 0.0, default: 0.1, max: 2.0
 - \b "~control_angular_z" : \b [bool] Controls the facing to target min: False, default: True, max: True
 

workspace/devel/.private/drone_demo/share/drone_demo/docs/TriangleParamConfig.wikidoc

@@ -27,50 +27,66 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
 5.default= 0.1
 5.type= double
 5.desc=linear.x controller Kp Range: 0.0 to 2.0
-6.name= ~linear_x_Kd
+6.name= ~linear_x_Ki
 6.default= 0.1
 6.type= double
-6.desc=linear.x controller Kd Range: 0.0 to 2.0
+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.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.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.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.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.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.