diff --git a/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h b/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h index 565e7e8..08d261f 100644 --- a/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h +++ b/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h @@ -249,6 +249,7 @@ class DEFAULT if("linear_x_Ki"==(*_i)->name){linear_x_Ki = boost::any_cast(val);} if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast(val);} if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast(val);} + if("speed_corrector_x"==(*_i)->name){speed_corrector_x = boost::any_cast(val);} if("speed_linear_y_Kp"==(*_i)->name){speed_linear_y_Kp = boost::any_cast(val);} if("speed_linear_y_Ki"==(*_i)->name){speed_linear_y_Ki = boost::any_cast(val);} if("speed_linear_y_Kd"==(*_i)->name){speed_linear_y_Kd = boost::any_cast(val);} @@ -256,6 +257,7 @@ class DEFAULT if("linear_y_Ki"==(*_i)->name){linear_y_Ki = boost::any_cast(val);} if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast(val);} if("control_linear_y"==(*_i)->name){control_linear_y = boost::any_cast(val);} + if("speed_corrector_y"==(*_i)->name){speed_corrector_y = boost::any_cast(val);} if("linear_z_Kp"==(*_i)->name){linear_z_Kp = boost::any_cast(val);} if("linear_z_Ki"==(*_i)->name){linear_z_Ki = boost::any_cast(val);} if("linear_z_Kd"==(*_i)->name){linear_z_Kd = boost::any_cast(val);} @@ -280,6 +282,7 @@ double linear_x_Kp; double linear_x_Ki; double linear_x_Kd; bool control_linear_x; +double speed_corrector_x; double speed_linear_y_Kp; double speed_linear_y_Ki; double speed_linear_y_Kd; @@ -287,6 +290,7 @@ double linear_y_Kp; double linear_y_Ki; double linear_y_Kd; bool control_linear_y; +double speed_corrector_y; double linear_z_Kp; double linear_z_Ki; double linear_z_Kd; @@ -330,6 +334,8 @@ 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_corrector_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" @@ -344,6 +350,8 @@ bool control_angular_z; 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 speed_corrector_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" @@ -541,7 +549,7 @@ TriangleParamConfig::GroupDescription("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("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_corrector_x = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_corrector_x = 3.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_corrector_x = 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("speed_corrector_x", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_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("speed_corrector_x", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_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" @@ -698,6 +716,16 @@ TriangleParamConfig::GroupDescription("control_linear_y", "bool", 0, "Controls the facing to target", "", &TriangleParamConfig::control_linear_y))); //#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("control_linear_y", "bool", 0, "Controls the facing to target", "", &TriangleParamConfig::control_linear_y))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_corrector_y = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_corrector_y = 3.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_corrector_y = 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("speed_corrector_y", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_y))); +//#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("speed_corrector_y", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_y))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.linear_z_Kp = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" diff --git a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox index d1d0ed1..ffda151 100644 --- a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox +++ b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox @@ -14,6 +14,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox index 677ef6d..b4e15cd 100644 --- a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox +++ b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox @@ -6,8 +6,8 @@ Reads and maintains the following parameters on the ROS server - \b "~target_width" : \b [double] the real target width (m) min: 0.01, default: 1.0, max: 1.5 - \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 "~max_speed" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 3.0 +- \b "~max_acceleration" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 3.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 @@ -15,6 +15,7 @@ Reads and maintains the following parameters on the ROS server - \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_corrector_x" : \b [double] Distance from which the speed corrector is disabled min: 0.0, default: 0.1, max: 3.0 - \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 @@ -22,6 +23,7 @@ Reads and maintains the following parameters on the ROS server - \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 - \b "~control_linear_y" : \b [bool] Controls the facing to target min: False, default: True, max: True +- \b "~speed_corrector_y" : \b [double] Distance from which the speed corrector is disabled min: 0.0, default: 0.1, max: 3.0 - \b "~linear_z_Kp" : \b [double] linear.z controller Kp min: 0.0, default: 0.01, max: 10.0 - \b "~linear_z_Ki" : \b [double] linear.z controller Ki min: 0.0, default: 0.01, max: 10.0 - \b "~linear_z_Kd" : \b [double] linear.z controller Kd min: 0.0, default: 0.01, max: 10.0 diff --git a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc index 409f7ca..3b7406d 100644 --- a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc +++ b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc @@ -22,11 +22,11 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig 4.name= ~max_speed 4.default= 0.3 4.type= double -4.desc=the maximal linear speed Range: 0.01 to 1.0 +4.desc=the maximal linear speed Range: 0.01 to 3.0 5.name= ~max_acceleration 5.default= 0.3 5.type= double -5.desc=the maximal linear speed Range: 0.01 to 1.0 +5.desc=the maximal linear speed Range: 0.01 to 3.0 6.name= ~speed_linear_x_Kp 6.default= 0.01 6.type= double @@ -55,66 +55,74 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig 12.default= True 12.type= bool 12.desc=Control distance to target -13.name= ~speed_linear_y_Kp -13.default= 0.01 +13.name= ~speed_corrector_x +13.default= 0.1 13.type= double -13.desc=linear.y controller Kp Range: 0.0 to 2.0 -14.name= ~speed_linear_y_Ki +13.desc=Distance from which the speed corrector is disabled Range: 0.0 to 3.0 +14.name= ~speed_linear_y_Kp 14.default= 0.01 14.type= double -14.desc=linear.y controller Ki Range: 0.0 to 2.0 -15.name= ~speed_linear_y_Kd +14.desc=linear.y controller Kp Range: 0.0 to 2.0 +15.name= ~speed_linear_y_Ki 15.default= 0.01 15.type= double -15.desc=linear.y controller Kd Range: 0.0 to 2.0 -16.name= ~linear_y_Kp +15.desc=linear.y controller Ki Range: 0.0 to 2.0 +16.name= ~speed_linear_y_Kd 16.default= 0.01 16.type= double -16.desc=linear.y controller Kp Range: 0.0 to 1.0 -17.name= ~linear_y_Ki +16.desc=linear.y controller Kd Range: 0.0 to 2.0 +17.name= ~linear_y_Kp 17.default= 0.01 17.type= double -17.desc=linear.y controller Ki Range: 0.0 to 1.0 -18.name= ~linear_y_Kd +17.desc=linear.y controller Kp Range: 0.0 to 1.0 +18.name= ~linear_y_Ki 18.default= 0.01 18.type= double -18.desc=linear.y controller Kd Range: 0.0 to 1.0 -19.name= ~control_linear_y -19.default= True -19.type= bool -19.desc=Controls the facing to target -20.name= ~linear_z_Kp -20.default= 0.01 -20.type= double -20.desc=linear.z controller Kp Range: 0.0 to 10.0 -21.name= ~linear_z_Ki -21.default= 0.01 +18.desc=linear.y controller Ki Range: 0.0 to 1.0 +19.name= ~linear_y_Kd +19.default= 0.01 +19.type= double +19.desc=linear.y controller Kd Range: 0.0 to 1.0 +20.name= ~control_linear_y +20.default= True +20.type= bool +20.desc=Controls the facing to target +21.name= ~speed_corrector_y +21.default= 0.1 21.type= double -21.desc=linear.z controller Ki Range: 0.0 to 10.0 -22.name= ~linear_z_Kd +21.desc=Distance from which the speed corrector is disabled Range: 0.0 to 3.0 +22.name= ~linear_z_Kp 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 +22.desc=linear.z controller Kp Range: 0.0 to 10.0 +23.name= ~linear_z_Ki +23.default= 0.01 +23.type= double +23.desc=linear.z controller Ki Range: 0.0 to 10.0 +24.name= ~linear_z_Kd 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 +24.desc=linear.z controller Kd Range: 0.0 to 10.0 +25.name= ~control_linear_z +25.default= True +25.type= bool +25.desc=Controls the facing to target +26.name= ~angular_z_Kp 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 +26.desc=angular.z controller Kp Range: 0.0 to 10.0 +27.name= ~angular_z_Ki +27.default= 0.01 +27.type= double +27.desc=angular.z controller Ki Range: 0.0 to 10.0 +28.name= ~angular_z_Kd +28.default= 0.01 +28.type= double +28.desc=angular.z controller Kd Range: 0.0 to 10.0 +29.name= ~control_angular_z +29.default= True +29.type= bool +29.desc=Controls the facing to target } } # End of autogenerated section. You may edit below. diff --git a/workspace/src/detect_targets/cfg/triangle_control.cfg b/workspace/src/detect_targets/cfg/triangle_control.cfg index 082a11b..4e7c1b3 100755 --- a/workspace/src/detect_targets/cfg/triangle_control.cfg +++ b/workspace/src/detect_targets/cfg/triangle_control.cfg @@ -9,8 +9,8 @@ gen.add("camera_angle", double_t, 0, "The angle corresponding to the image width 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("max_speed", double_t, 0, "the maximal linear speed", .3, .01, 3) +gen.add("max_acceleration", double_t, 0, "the maximal linear speed", .3, .01, 3) gen.add("speed_linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 2) @@ -20,6 +20,7 @@ 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_corrector_x", double_t, 0, "Distance from which the speed corrector is disabled", .1, 0, 3) 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) @@ -28,6 +29,7 @@ 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) gen.add("control_linear_y", bool_t, 0, "Controls the facing to target", True) +gen.add("speed_corrector_y", double_t, 0, "Distance from which the speed corrector is disabled", .1, 0, 3) gen.add("linear_z_Kp", double_t, 0, "linear.z controller Kp", .01, 0, 10) gen.add("linear_z_Ki", double_t, 0, "linear.z controller Ki", .01, 0, 10) diff --git a/workspace/src/detect_targets/scripts/find_targets.pyc b/workspace/src/detect_targets/scripts/find_targets.pyc index 4349b00..b951caa 100644 Binary files a/workspace/src/detect_targets/scripts/find_targets.pyc and b/workspace/src/detect_targets/scripts/find_targets.pyc differ diff --git a/workspace/src/detect_targets/scripts/triangle_control.py b/workspace/src/detect_targets/scripts/triangle_control.py index 9c8650c..502b53a 100755 --- a/workspace/src/detect_targets/scripts/triangle_control.py +++ b/workspace/src/detect_targets/scripts/triangle_control.py @@ -4,6 +4,8 @@ import math import time +import numpy as np + import roslib import rospy from geometry_msgs.msg import Twist @@ -67,6 +69,8 @@ class TriangleControl: -config['max_speed'], config['max_speed'] ) + self.speed_corrector_y = config['speed_corrector_y'] + # 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'] @@ -87,6 +91,7 @@ class TriangleControl: -config['max_speed'], config['max_speed'] ) + self.speed_corrector_x = config['speed_corrector_x'] self.pid_linear_x.setpoint = self.target_distance return config @@ -169,11 +174,15 @@ class TriangleControl: self.linear_z_pub.publish(self.linear_z_info) dt = time.time() - self.pid_linear_y._last_time - last_input_y = self.pid_linear_y._last_input + self.last_values_y = np.concatenate((self.last_values_y[1:7], [self.alpha])) 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 abs(self.alpha) >= self.speed_corrector_y: + self.pid_speed_linear_y.setpoint = target_acceleration_y + speed_y = self.last_values_y.dot(self.savgol_filter) / dt + self.twist.linear.y = self.pid_speed_linear_y(speed_y) + else: + self.twist.linear.y = target_acceleration_y + if self.linear_y_pub.get_num_connections() > 0: self.linear_y_info.target = 0 self.linear_y_info.error = -self.alpha @@ -182,15 +191,20 @@ class TriangleControl: self.linear_y_pub.publish(self.linear_y_info) dt = time.time() - self.pid_linear_x._last_time - last_input_x = self.pid_linear_x._last_input + self.last_values_x = np.concatenate((self.last_values_x[1:7], [self.d])) 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) + speed_x = 0 + if abs(self.target_distance - self.d) >= self.speed_corrector_x: + self.pid_speed_linear_x.setpoint = target_acceleration_x + speed_x = self.last_values_x.dot(self.savgol_filter) / dt + self.twist.linear.x = self.pid_speed_linear_x(speed_x) + else: + self.twist.linear.x = target_acceleration_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 - self.linear_x_info.derror = 0 + self.linear_x_info.derror = speed_x self.linear_x_info.cmd_vel = self.twist.linear.x self.linear_x_pub.publish(self.linear_x_info) @@ -259,6 +273,22 @@ class TriangleControl: sample_time=0.14 ) + self.savgol_filter = 1.0/252.0 * np.array([ + [22], + [-67], + [-58], + [0], + [58], + [67], + [-22] + ], dtype=np.float64) + + self.last_values_x = np.zeros(7, dtype=np.float64) + self.last_values_y = np.zeros(7, dtype=np.float64) + + self.speed_corrector_x = 30 + self.speed_corrector_y = 30 + # Control info self.angular_z_info = control() self.linear_x_info = control()