00001 #include "evarobot_controller/evarobot_controller.h"
00002
00003 #include <dynamic_reconfigure/server.h>
00004 #include <evarobot_controller/ParamsConfig.h>
00005
00006 int i_error_code = 0;
00007
00008 void PIDController::Reset()
00009 {
00010 this->d_proportional_error = 0.0;
00011 this->d_derivative_error = 0.0;
00012 this->d_integral_error = 0.0;
00013 this->d_pre_error = 0.0;
00014
00015 }
00016
00017 PIDController::PIDController()
00018 {
00019 this->Reset();
00020
00021 this->d_integral_constant = 0.0;
00022 this->d_derivative_constant = 0.0;
00023 this->d_proportional_constant = 0.0;
00024
00025 this->read_time = ros::Time::now();
00026 this->d_max_vel = 0.9;
00027
00028 }
00029
00030 PIDController::PIDController(double d_proportional_constant,
00031 double d_integral_constant,
00032 double d_derivative_constant,
00033 double _d_max_vel,
00034 string _str_name):d_max_vel(_d_max_vel), str_name(_str_name)
00035 {
00036 this->Reset();
00037
00038 this->d_integral_constant = d_integral_constant;
00039 this->d_derivative_constant = d_derivative_constant;
00040 this->d_proportional_constant = d_proportional_constant;
00041
00042 this->read_time = ros::Time::now();
00043 }
00044
00045 PIDController::~PIDController(){}
00046
00047 float PIDController::RunController(float f_desired, float f_measured)
00048 {
00049 float f_ret = 0;
00050
00051 double d_current_error = (double)(f_desired - f_measured);
00052
00053 if( (this->d_integral_error < 0.0 && f_desired >= 0.0) || (this->d_integral_error > 0.0 && f_desired <= 0.0) )
00054 {
00055 this->d_integral_error = 0.0;
00056 }
00057
00058 if(fabs(d_current_error) <= 0.01)
00059 {
00060 d_current_error = 0.0;
00061
00062 }
00063
00064
00065 this->dur_time = ros::Time::now() - this->read_time;
00066 this->read_time = ros::Time::now();
00067
00068 this->d_proportional_error = d_current_error;
00069 this->d_derivative_error = (d_current_error - this->d_pre_error) / dur_time.toSec();
00070 this->d_integral_error += d_current_error * dur_time.toSec();
00071
00072 f_ret = this->d_proportional_constant * this->d_proportional_error
00073 + this->d_integral_constant * this->d_integral_error
00074 + this->d_derivative_constant * this->d_derivative_error;
00075
00076 this->d_pre_error = d_current_error;
00077
00078 if( fabs(f_ret) > this->d_max_vel )
00079 {
00080 ROS_DEBUG("EvarobotController: MAX VEL: %f", this->d_max_vel);
00081 f_ret = (f_ret * this->d_max_vel) / fabs(f_ret);
00082 }
00083
00084 return f_ret;
00085
00086 }
00087
00088 void PIDController::UpdateParams(double d_proportional_constant,
00089 double d_integral_constant,
00090 double d_derivative_constant)
00091 {
00092 this->d_integral_constant = d_integral_constant;
00093 this->d_derivative_constant = d_derivative_constant;
00094 this->d_proportional_constant = d_proportional_constant;
00095 }
00096
00097 void PIDController::ProduceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
00098 {
00099 if(i_error_code<0)
00100 {
00101 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "%s", GetErrorDescription(i_error_code).c_str());
00102 i_error_code = 0;
00103 }
00104 else
00105 {
00106 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "%s is OK!", this->str_name.c_str());
00107
00108 stat.add("Proportional Constant", this->d_proportional_constant);
00109 stat.add("Integral Constant", this->d_integral_constant);
00110 stat.add("Derivative Constant", this->d_derivative_constant);
00111
00112 stat.add("Proportional Error", this->d_proportional_error);
00113 stat.add("Integral Error", this->d_integral_error);
00114 stat.add("Derivative Error", this->d_derivative_error);
00115 }
00116 }
00117
00118 void CallbackMeasured(const im_msgs::WheelVel::ConstPtr & msg)
00119 {
00120 g_d_dt = (ros::Time::now() - msg->header.stamp).toSec();
00121 g_f_left_measured = msg->left_vel;
00122 g_f_right_measured = msg->right_vel;
00123 }
00124
00125 void CallbackDesired(const geometry_msgs::Twist::ConstPtr & msg)
00126 {
00127 float f_linear_vel = msg->linear.x;
00128 float f_angular_vel = msg->angular.z;
00129
00130 g_f_left_desired = ((2 * f_linear_vel) - f_angular_vel * g_d_wheel_separation) / 2;
00131 g_f_right_desired = ((2 * f_linear_vel) + f_angular_vel * g_d_wheel_separation) / 2;
00132
00133
00134 }
00135
00136 void CallbackReconfigure(evarobot_controller::ParamsConfig &config, uint32_t level)
00137 {
00138 b_is_received_params = true;
00139 g_d_wheel_separation = config.wheelSeparation;
00140
00141 g_d_p_left = config.proportionalConstLeft;
00142 g_d_i_left = config.integralConstLeft;
00143 g_d_d_left = config.derivativeConstLeft;
00144
00145 g_d_p_right = config.proportionalConstRight;
00146 g_d_i_right = config.integralConstRight;
00147 g_d_d_right = config.derivativeConstRight;
00148 }
00149
00150 bool CallbackResetController(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
00151 {
00152 ROS_DEBUG("EvarobotContoller: Reset Controller");
00153 b_reset_controller = true;
00154 return true;
00155 }
00156
00157 int main(int argc, char **argv)
00158 {
00159
00160 double d_integral_constant_left;
00161 double d_derivative_constant_left;
00162 double d_proportional_constant_left;
00163
00164 double d_integral_constant_right;
00165 double d_derivative_constant_right;
00166 double d_proportional_constant_right;
00167
00168 double d_max_vel;
00169 double d_frequency;
00170 double d_max_freq, d_min_freq;
00171
00172 double d_timeout;
00173
00174
00175
00176 ros::init(argc, argv, "/evarobot_controller");
00177 ros::NodeHandle n;
00178
00179
00180 n.param<double>("evarobot_controller/maxVel", d_max_vel, 0.9);
00181 n.param("evarobot_controller/minFreq", d_min_freq, 0.2);
00182 n.param("evarobot_controller/maxFreq", d_max_freq, 10.0);
00183 n.param("evarobot_controller/timeout", d_timeout, 0.5);
00184
00185 if(!n.getParam("evarobot_controller/wheelSeparation", g_d_wheel_separation))
00186 {
00187 ROS_INFO(GetErrorDescription(-28).c_str());
00188 i_error_code = -28;
00189 }
00190 if(!n.getParam("evarobot_controller/integralConstLeft", d_integral_constant_left))
00191 {
00192 ROS_INFO(GetErrorDescription(-65).c_str());
00193 i_error_code = -65;
00194 }
00195
00196 if(!n.getParam("evarobot_controller/derivativeConstLeft", d_derivative_constant_left))
00197 {
00198 ROS_INFO(GetErrorDescription(-69).c_str());
00199 i_error_code = -69;
00200 }
00201
00202 if(!n.getParam("evarobot_controller/proportionalConstLeft", d_proportional_constant_left))
00203 {
00204 ROS_INFO(GetErrorDescription(-70).c_str());
00205 i_error_code = -70;
00206 }
00207
00208 if(!n.getParam("evarobot_controller/integralConstRight", d_integral_constant_right))
00209 {
00210 ROS_INFO(GetErrorDescription(-71).c_str());
00211 i_error_code = -71;
00212 }
00213
00214 if(!n.getParam("evarobot_controller/derivativeConstRight", d_derivative_constant_right))
00215 {
00216 ROS_INFO(GetErrorDescription(-72).c_str());
00217 i_error_code = -72;
00218 }
00219
00220 if(!n.getParam("evarobot_controller/proportionalConstRight", d_proportional_constant_right))
00221 {
00222 ROS_INFO(GetErrorDescription(-73).c_str());
00223 i_error_code = -73;
00224 }
00225
00226 if(!n.getParam("evarobot_controller/Frequency", d_frequency))
00227 {
00228 ROS_INFO(GetErrorDescription(-74).c_str());
00229 i_error_code = -74;
00230 }
00231
00232 PIDController left_controller = PIDController(d_proportional_constant_left,
00233 d_integral_constant_left,
00234 d_derivative_constant_left,
00235 d_max_vel,
00236 "LeftController");
00237
00238 PIDController right_controller = PIDController(d_proportional_constant_right,
00239 d_integral_constant_right,
00240 d_derivative_constant_right,
00241 d_max_vel,
00242 "RightController");
00243
00244 ros::Subscriber measured_sub = n.subscribe("wheel_vel", 1, CallbackMeasured);
00245 ros::Subscriber desired_sub = n.subscribe("cmd_vel", 1, CallbackDesired);
00246 realtime_tools::RealtimePublisher<im_msgs::WheelVel> * ctrl_pub = new realtime_tools::RealtimePublisher<im_msgs::WheelVel>(n, "cntr_wheel_vel", 10);
00247
00248 ros::ServiceServer service = n.advertiseService("evarobot_controller/reset_controller", CallbackResetController);
00249
00250
00251 dynamic_reconfigure::Server<evarobot_controller::ParamsConfig> srv;
00252 dynamic_reconfigure::Server<evarobot_controller::ParamsConfig>::CallbackType f;
00253 f = boost::bind(&CallbackReconfigure, _1, _2);
00254 srv.setCallback(f);
00256
00257
00258 diagnostic_updater::Updater updater;
00259 updater.setHardwareID("None");
00260 updater.add("LeftController", &left_controller, &PIDController::ProduceDiagnostics);
00261 updater.add("RightController", &right_controller, &PIDController::ProduceDiagnostics);
00262
00263 diagnostic_updater::HeaderlessTopicDiagnostic pub_freq("cntr_wheel_vel", updater,
00264 diagnostic_updater::FrequencyStatusParam(&d_min_freq, &d_max_freq, 0.1, 10));
00265
00266
00267 ros::Rate loop_rate(d_frequency);
00268
00269 while(ros::ok())
00270 {
00271
00272 if(b_is_received_params)
00273 {
00274 ROS_DEBUG("EvarobotController: Updating Controller Params...");
00275 ROS_DEBUG("EvarobotController: %f, %f, %f", g_d_p_left, g_d_i_left, g_d_d_left);
00276 ROS_DEBUG("EvarobotController: %f, %f, %f", g_d_p_right, g_d_i_right, g_d_d_right);
00277
00278 left_controller.UpdateParams(g_d_p_left, g_d_i_left, g_d_d_left);
00279 right_controller.UpdateParams(g_d_p_right, g_d_i_right, g_d_d_right);
00280 b_is_received_params = false;
00281 }
00282
00283 if(b_reset_controller)
00284 {
00285 left_controller.Reset();
00286 right_controller.Reset();
00287 b_reset_controller = false;
00288 }
00289
00290 ctrl_pub->msg_.header.stamp = ros::Time::now();
00291 ctrl_pub->msg_.left_vel = left_controller.RunController(g_f_left_desired, g_f_left_measured);
00292 ctrl_pub->msg_.right_vel = right_controller.RunController(g_f_right_desired, g_f_right_measured);
00293
00294 if(g_d_dt > d_timeout)
00295 {
00296 ROS_INFO(GetErrorDescription(-75).c_str());
00297 i_error_code = -75;
00298 ctrl_pub->msg_.left_vel = 0.0;
00299 ctrl_pub->msg_.right_vel = 0.0;
00300 left_controller.Reset();
00301 right_controller.Reset();
00302 }
00303 else if(g_d_dt < 0)
00304 {
00305 ROS_INFO(GetErrorDescription(-76).c_str());
00306 i_error_code = -76;
00307 ctrl_pub->msg_.left_vel = 0.0;
00308 ctrl_pub->msg_.right_vel = 0.0;
00309 left_controller.Reset();
00310 right_controller.Reset();
00311 }
00312
00313 if (ctrl_pub->trylock())
00314 {
00315 ctrl_pub->unlockAndPublish();
00316 }
00317 pub_freq.tick();
00318
00319
00320 updater.update();
00321 ros::spinOnce();
00322 loop_rate.sleep();
00323 }
00324
00325 return 0;
00326 }