Double PID

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

@@ -241,10 +241,17 @@ class DEFAULT
         if("target_depth"==(*_i)->name){target_depth = 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_acceleration"==(*_i)->name){max_acceleration = boost::any_cast<double>(val);}
+        if("speed_linear_x_Kp"==(*_i)->name){speed_linear_x_Kp = boost::any_cast<double>(val);}
+        if("speed_linear_x_Ki"==(*_i)->name){speed_linear_x_Ki = boost::any_cast<double>(val);}
+        if("speed_linear_x_Kd"==(*_i)->name){speed_linear_x_Kd = 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("speed_linear_y_Kp"==(*_i)->name){speed_linear_y_Kp = boost::any_cast<double>(val);}
+        if("speed_linear_y_Ki"==(*_i)->name){speed_linear_y_Ki = boost::any_cast<double>(val);}
+        if("speed_linear_y_Kd"==(*_i)->name){speed_linear_y_Kd = 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);}
@@ -265,10 +272,17 @@ double target_width;
 double target_depth;
 double distance_to_target;
 double max_speed;
+double max_acceleration;
+double speed_linear_x_Kp;
+double speed_linear_x_Ki;
+double speed_linear_x_Kd;
 double linear_x_Kp;
 double linear_x_Ki;
 double linear_x_Kd;
 bool control_linear_x;
+double speed_linear_y_Kp;
+double speed_linear_y_Ki;
+double speed_linear_y_Kd;
 double linear_y_Kp;
 double linear_y_Ki;
 double linear_y_Kd;
@@ -300,6 +314,14 @@ bool control_angular_z;
       double distance_to_target;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       double max_speed;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double max_acceleration;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_linear_x_Kp;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_linear_x_Ki;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_linear_x_Kd;
 //#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"
@@ -308,6 +330,12 @@ bool control_angular_z;
       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 speed_linear_y_Kp;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_linear_y_Ki;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_linear_y_Kd;
 //#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"
@@ -520,6 +548,46 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
       Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("max_speed", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_speed)));
 //#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>("max_speed", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_speed)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.max_acceleration = 0.01;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.max_acceleration = 1.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.max_acceleration = 0.3;
+//#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>("max_acceleration", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_acceleration)));
+//#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>("max_acceleration", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_acceleration)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_linear_x_Kp = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_linear_x_Kp = 2.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_linear_x_Kp = 0.01;
+//#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>("speed_linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::speed_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>("speed_linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::speed_linear_x_Kp)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_linear_x_Ki = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_linear_x_Ki = 2.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_linear_x_Ki = 0.01;
+//#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>("speed_linear_x_Ki", "double", 0, "linear.x controller Ki", "", &TriangleParamConfig::speed_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>("speed_linear_x_Ki", "double", 0, "linear.x controller Ki", "", &TriangleParamConfig::speed_linear_x_Ki)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_linear_x_Kd = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_linear_x_Kd = 2.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_linear_x_Kd = 0.01;
+//#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>("speed_linear_x_Kd", "double", 0, "linear.x controller Kd", "", &TriangleParamConfig::speed_linear_x_Kd)));
+//#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>("speed_linear_x_Kd", "double", 0, "linear.x controller Kd", "", &TriangleParamConfig::speed_linear_x_Kd)));
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __min__.linear_x_Kp = 0.0;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@@ -560,6 +628,36 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
       Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<bool>("control_linear_x", "bool", 0, "Control distance to target", "", &TriangleParamConfig::control_linear_x)));
 //#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<bool>("control_linear_x", "bool", 0, "Control distance to target", "", &TriangleParamConfig::control_linear_x)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_linear_y_Kp = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_linear_y_Kp = 2.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_linear_y_Kp = 0.01;
+//#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>("speed_linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::speed_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>("speed_linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::speed_linear_y_Kp)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_linear_y_Ki = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_linear_y_Ki = 2.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_linear_y_Ki = 0.01;
+//#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>("speed_linear_y_Ki", "double", 0, "linear.y controller Ki", "", &TriangleParamConfig::speed_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>("speed_linear_y_Ki", "double", 0, "linear.y controller Ki", "", &TriangleParamConfig::speed_linear_y_Ki)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_linear_y_Kd = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_linear_y_Kd = 2.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_linear_y_Kd = 0.01;
+//#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>("speed_linear_y_Kd", "double", 0, "linear.y controller Kd", "", &TriangleParamConfig::speed_linear_y_Kd)));
+//#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>("speed_linear_y_Kd", "double", 0, "linear.y controller Kd", "", &TriangleParamConfig::speed_linear_y_Kd)));
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __min__.linear_y_Kp = 0.0;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"

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

@@ -6,10 +6,17 @@
   <param name="target_depth" type="double" value="0.2" />
   <param name="distance_to_target" type="double" value="2.0" />
   <param name="max_speed" type="double" value="0.3" />
+  <param name="max_acceleration" type="double" value="0.3" />
+  <param name="speed_linear_x_Kp" type="double" value="0.01" />
+  <param name="speed_linear_x_Ki" type="double" value="0.01" />
+  <param name="speed_linear_x_Kd" type="double" value="0.01" />
   <param name="linear_x_Kp" type="double" value="0.01" />
   <param name="linear_x_Ki" type="double" value="0.01" />
   <param name="linear_x_Kd" type="double" value="0.01" />
   <param name="control_linear_x" type="bool" value="True" />
+  <param name="speed_linear_y_Kp" type="double" value="0.01" />
+  <param name="speed_linear_y_Ki" type="double" value="0.01" />
+  <param name="speed_linear_y_Kd" type="double" value="0.01" />
   <param name="linear_y_Kp" type="double" value="0.01" />
   <param name="linear_y_Ki" type="double" value="0.01" />
   <param name="linear_y_Kd" type="double" value="0.01" />

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

@@ -7,10 +7,17 @@ Reads and maintains the following parameters on the ROS server
 - \b "~target_depth" : \b [double] the real target depth (m) min: 0.01, default: 0.2, max: 0.5
 - \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_acceleration" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0
+- \b "~speed_linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.01, max: 2.0
+- \b "~speed_linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.01, max: 2.0
+- \b "~speed_linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.01, max: 2.0
 - \b "~linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.01, max: 1.0
 - \b "~linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.01, max: 1.0
 - \b "~linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.01, max: 1.0
 - \b "~control_linear_x" : \b [bool] Control distance to target min: False, default: True, max: True
+- \b "~speed_linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.01, max: 2.0
+- \b "~speed_linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.01, max: 2.0
+- \b "~speed_linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.01, max: 2.0
 - \b "~linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.01, max: 1.0
 - \b "~linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.01, max: 1.0
 - \b "~linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.01, max: 1.0

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

@@ -23,70 +23,98 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
 4.default= 0.3
 4.type= double
 4.desc=the maximal linear speed Range: 0.01 to 1.0
-5.name= ~linear_x_Kp
+5.name= ~max_acceleration
-5.default= 0.01
+5.default= 0.3
 5.type= double
-5.desc=linear.x controller Kp Range: 0.0 to 1.0
+5.desc=the maximal linear speed Range: 0.01 to 1.0
-6.name= ~linear_x_Ki
+6.name= ~speed_linear_x_Kp
 6.default= 0.01
 6.type= double
-6.desc=linear.x controller Ki Range: 0.0 to 1.0
+6.desc=linear.x controller Kp Range: 0.0 to 2.0
-7.name= ~linear_x_Kd
+7.name= ~speed_linear_x_Ki
 7.default= 0.01
 7.type= double
-7.desc=linear.x controller Kd Range: 0.0 to 1.0
+7.desc=linear.x controller Ki Range: 0.0 to 2.0
-8.name= ~control_linear_x
+8.name= ~speed_linear_x_Kd
-8.default= True
+8.default= 0.01
-8.type= bool
+8.type= double
-8.desc=Control distance to target 
+8.desc=linear.x controller Kd Range: 0.0 to 2.0
-9.name= ~linear_y_Kp
+9.name= ~linear_x_Kp
 9.default= 0.01
 9.type= double
-9.desc=linear.y controller Kp Range: 0.0 to 1.0
+9.desc=linear.x controller Kp Range: 0.0 to 1.0
-10.name= ~linear_y_Ki
+10.name= ~linear_x_Ki
 10.default= 0.01
 10.type= double
-10.desc=linear.y controller Ki Range: 0.0 to 1.0
+10.desc=linear.x controller Ki Range: 0.0 to 1.0
-11.name= ~linear_y_Kd
+11.name= ~linear_x_Kd
 11.default= 0.01
 11.type= double
-11.desc=linear.y controller Kd Range: 0.0 to 1.0
+11.desc=linear.x controller Kd Range: 0.0 to 1.0
-12.name= ~control_linear_y
+12.name= ~control_linear_x
 12.default= True
 12.type= bool
-12.desc=Controls the facing to target 
+12.desc=Control distance to target 
-13.name= ~linear_z_Kp
+13.name= ~speed_linear_y_Kp
 13.default= 0.01
 13.type= double
-13.desc=linear.z controller Kp Range: 0.0 to 10.0
+13.desc=linear.y controller Kp Range: 0.0 to 2.0
-14.name= ~linear_z_Ki
+14.name= ~speed_linear_y_Ki
 14.default= 0.01
 14.type= double
-14.desc=linear.z controller Ki Range: 0.0 to 10.0
+14.desc=linear.y controller Ki Range: 0.0 to 2.0
-15.name= ~linear_z_Kd
+15.name= ~speed_linear_y_Kd
 15.default= 0.01
 15.type= double
-15.desc=linear.z controller Kd Range: 0.0 to 10.0
+15.desc=linear.y controller Kd Range: 0.0 to 2.0
-16.name= ~control_linear_z
+16.name= ~linear_y_Kp
-16.default= True
+16.default= 0.01
-16.type= bool
+16.type= double
-16.desc=Controls the facing to target 
+16.desc=linear.y controller Kp Range: 0.0 to 1.0
-17.name= ~angular_z_Kp
+17.name= ~linear_y_Ki
 17.default= 0.01
 17.type= double
-17.desc=angular.z controller Kp Range: 0.0 to 10.0
+17.desc=linear.y controller Ki Range: 0.0 to 1.0
-18.name= ~angular_z_Ki
+18.name= ~linear_y_Kd
 18.default= 0.01
 18.type= double
-18.desc=angular.z controller Ki Range: 0.0 to 10.0
+18.desc=linear.y controller Kd Range: 0.0 to 1.0
-19.name= ~angular_z_Kd
+19.name= ~control_linear_y
-19.default= 0.01
+19.default= True
-19.type= double
+19.type= bool
-19.desc=angular.z controller Kd Range: 0.0 to 10.0
+19.desc=Controls the facing to target 
-20.name= ~control_angular_z
+20.name= ~linear_z_Kp
-20.default= True
+20.default= 0.01
-20.type= bool
+20.type= double
-20.desc=Controls the facing to target 
+20.desc=linear.z controller Kp Range: 0.0 to 10.0
+21.name= ~linear_z_Ki
+21.default= 0.01
+21.type= double
+21.desc=linear.z controller Ki Range: 0.0 to 10.0
+22.name= ~linear_z_Kd
+22.default= 0.01
+22.type= double
+22.desc=linear.z controller Kd Range: 0.0 to 10.0
+23.name= ~control_linear_z
+23.default= True
+23.type= bool
+23.desc=Controls the facing to target 
+24.name= ~angular_z_Kp
+24.default= 0.01
+24.type= double
+24.desc=angular.z controller Kp Range: 0.0 to 10.0
+25.name= ~angular_z_Ki
+25.default= 0.01
+25.type= double
+25.desc=angular.z controller Ki Range: 0.0 to 10.0
+26.name= ~angular_z_Kd
+26.default= 0.01
+26.type= double
+26.desc=angular.z controller Kd Range: 0.0 to 10.0
+27.name= ~control_angular_z
+27.default= True
+27.type= bool
+27.desc=Controls the facing to target 
 }
 }
 # End of autogenerated section. You may edit below.

workspace/src/detect_targets/cfg/triangle_control.cfg

@@ -10,13 +10,20 @@ gen.add("target_width", double_t, 0, "the real target width (m)", 1, 0.01, 1.5)
 gen.add("target_depth", double_t, 0, "the real target depth (m)", .2, 0.01, 0.5)
 gen.add("distance_to_target", double_t, 0, "The required distance to the target (m)", 2, 1, 5)
 gen.add("max_speed", double_t, 0, "the maximal linear speed", .3, .01, 1)
+gen.add("max_acceleration", double_t, 0, "the maximal linear speed", .3, .01, 1)
 
 
+gen.add("speed_linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 2)
+gen.add("speed_linear_x_Ki", double_t, 0, "linear.x controller Ki", .01, 0, 2)
+gen.add("speed_linear_x_Kd", double_t, 0, "linear.x controller Kd", .01, 0, 2)
 gen.add("linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 1)
 gen.add("linear_x_Ki", double_t, 0, "linear.x controller Ki", .01, 0, 1)
 gen.add("linear_x_Kd", double_t, 0, "linear.x controller Kd", .01, 0, 1)
 gen.add("control_linear_x", bool_t, 0, "Control distance to target", True)
 
+gen.add("speed_linear_y_Kp", double_t, 0, "linear.y controller Kp", .01, 0, 2)
+gen.add("speed_linear_y_Ki", double_t, 0, "linear.y controller Ki", .01, 0, 2)
+gen.add("speed_linear_y_Kd", double_t, 0, "linear.y controller Kd", .01, 0, 2)
 gen.add("linear_y_Kp", double_t, 0, "linear.y controller Kp", .01, 0, 1)
 gen.add("linear_y_Ki", double_t, 0, "linear.y controller Ki", .01, 0, 1)
 gen.add("linear_y_Kd", double_t, 0, "linear.y controller Kd", .01, 0, 1)

workspace/src/detect_targets/scripts/triangle_control.py

@@ -2,6 +2,7 @@
 # -*- coding: utf-8 -*-
 
 import math
+import time
 
 import roslib
 import rospy
@@ -51,21 +52,38 @@ class TriangleControl:
         self.pid_linear_y.Ki = config['linear_y_Ki']
         self.pid_linear_y.Kd = config['linear_y_Kd']
         self.pid_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0)
+        self.pid_speed_linear_y.Kp = config['speed_linear_y_Kp']
+        self.pid_speed_linear_y.Ki = config['speed_linear_y_Ki']
+        self.pid_speed_linear_y.Kd = config['speed_linear_y_Kd']
+        self.pid_speed_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0)
         if not config['control_linear_y']:
             self.pid_linear_y._last_output = 0.0
+            self.pid_speed_linear_y._last_output = 0.0
         self.pid_linear_y.output_limits = (
+            -config['max_acceleration'],
+            config['max_acceleration']
+        )
+        self.pid_speed_linear_y.output_limits = (
             -config['max_speed'],
             config['max_speed']
         )
-
         # X gains are reversed because of the chosen axis
         self.pid_linear_x.Kp = - config['linear_x_Kp']
         self.pid_linear_x.Ki = - config['linear_x_Ki']
         self.pid_linear_x.Kd = - config['linear_x_Kd']
         self.pid_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0)
+        self.pid_speed_linear_x.Kp = config['speed_linear_x_Kp']
+        self.pid_speed_linear_x.Ki = config['speed_linear_x_Ki']
+        self.pid_speed_linear_x.Kd = config['speed_linear_x_Kd']
+        self.pid_speed_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0)
         if not config['control_linear_x']:
             self.pid_linear_x._last_output = 0.0
+            self.pid_speed_linear_x._last_output = 0.0
         self.pid_linear_x.output_limits = (
+            -config['max_acceleration'],
+            config['max_acceleration']
+        )
+        self.pid_speed_linear_x.output_limits = (
             -config['max_speed'],
             config['max_speed']
         )
@@ -150,7 +168,12 @@ class TriangleControl:
             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)
+        dt = time.time() - self.pid_linear_y._last_time
+        last_input_y = self.pid_linear_y._last_input
+        target_acceleration_y = self.pid_linear_y(-self.alpha)
+        self.pid_speed_linear_y.setpoint = target_acceleration_y
+        speed_y = (last_input_y - self.pid_linear_y._last_input) / dt
+        self.twist.linear.y = self.pid_speed_linear_y(speed_y)
         if self.linear_y_pub.get_num_connections() > 0:
             self.linear_y_info.target = 0
             self.linear_y_info.error = -self.alpha
@@ -158,7 +181,12 @@ class TriangleControl:
             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)
+        dt = time.time() - self.pid_linear_x._last_time
+        last_input_x = self.pid_linear_x._last_input
+        target_acceleration_x = self.pid_linear_x(self.d)
+        self.pid_speed_linear_x.setpoint = target_acceleration_x
+        speed_x = (last_input_x - self.pid_linear_x._last_input) / dt
+        self.twist.linear.x = self.pid_speed_linear_x(speed_x)
         if self.linear_x_pub.get_num_connections() > 0:
             self.linear_x_info.target = self.pid_linear_x.setpoint
             self.linear_x_info.error = self.target_distance - self.d
@@ -208,6 +236,13 @@ class TriangleControl:
             auto_mode=True,
             sample_time=0.14
         )
+        self.pid_speed_linear_y = PID(
+            1,
+            0,
+            0,
+            auto_mode=True,
+            sample_time=0.14
+        )
         self.pid_linear_x = PID(
             1,
             0,
@@ -216,6 +251,13 @@ class TriangleControl:
             sample_time=0.14,
             setpoint=self.target_distance,
         )
+        self.pid_speed_linear_x = PID(
+            1,
+            0,
+            0,
+            auto_mode=True,
+            sample_time=0.14
+        )
 
         # Control info
         self.angular_z_info = control()