pid.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 #ifndef CONTROL_TOOLBOX__PID_H
35 #define CONTROL_TOOLBOX__PID_H
36 
37 
38 #include <string>
39 #include <ros/ros.h>
40 #include <control_msgs/PidState.h>
41 
42 // Dynamic reconfigure
43 #include <dynamic_reconfigure/server.h>
44 #include <control_toolbox/ParametersConfig.h>
45 #include <boost/thread/mutex.hpp>
46 
47 // Realtime buffer
50 
51 class TiXmlElement;
52 
53 namespace control_toolbox {
54 
55 /***************************************************/
115 /***************************************************/
116 
117 class Pid
118 {
119 public:
120 
124  struct Gains
125  {
126  // Optional constructor for passing in values without antiwindup
127  Gains(double p, double i, double d, double i_max, double i_min)
128  : p_gain_(p),
129  i_gain_(i),
130  d_gain_(d),
131  i_max_(i_max),
132  i_min_(i_min),
133  antiwindup_(false)
134  {}
135  // Optional constructor for passing in values
136  Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
137  : p_gain_(p),
138  i_gain_(i),
139  d_gain_(d),
140  i_max_(i_max),
141  i_min_(i_min),
142  antiwindup_(antiwindup)
143  {}
144  // Default constructor
146  : p_gain_(0.0),
147  i_gain_(0.0),
148  d_gain_(0.0),
149  i_max_(0.0),
150  i_min_(0.0),
151  antiwindup_(false)
152  {}
153  double p_gain_;
154  double i_gain_;
155  double d_gain_;
156  double i_max_;
157  double i_min_;
158  bool antiwindup_;
159  };
160 
172  Pid(double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, bool antiwindup = false);
173 
178  Pid(const Pid &source);
179 
183  ~Pid();
184 
195  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
196 
207  void initPid(double p, double i, double d, double i_max, double i_min, const ros::NodeHandle& /*node*/);
208  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle& /*node*/);
209 
217  bool initParam(const std::string& prefix, const bool quiet=false);
218 
226  bool init(const ros::NodeHandle &n, const bool quiet=false);
227 
234  bool initXml(TiXmlElement *config);
235 
241 
245  void reset();
246 
255  void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
256  void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
257 
262  Gains getGains();
263 
272  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
273 
278  void setGains(const Gains &gains);
279 
283  void updateDynamicReconfig();
284  void updateDynamicReconfig(Gains gains_config);
285  void updateDynamicReconfig(control_toolbox::ParametersConfig config);
286 
290  void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/);
291 
302  double computeCommand(double error, ros::Duration dt);
303 
315  double computeCommand(double error, double error_dot, ros::Duration dt);
316 
329  ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt);
330 
345  ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt);
346 
350  void setCurrentCmd(double cmd);
351 
355  double getCurrentCmd();
356 
363  void getCurrentPIDErrors(double *pe, double *ie, double *de);
364 
365 
369  void printValues();
370 
375  Pid &operator =(const Pid& source)
376  {
377  if (this == &source)
378  return *this;
379 
380  // Copy the realtime buffer to then new PID class
381  gains_buffer_ = source.gains_buffer_;
382 
383  // Reset the state of this PID controller
384  reset();
385 
386  return *this;
387  }
388 
389 private:
390 
391  // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
392  // blocking the realtime update loop
394 
397 
398  double p_error_last_;
399  double p_error_;
400  double i_error_;
401  double d_error_;
402  double cmd_;
404  // Dynamics reconfigure
406  typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> DynamicReconfigServer;
408  DynamicReconfigServer::CallbackType param_reconfig_callback_;
409 
410  boost::recursive_mutex param_reconfig_mutex_;
411 
412 };
413 
414 }
415 
416 #endif
realtime_tools::RealtimeBuffer< Gains > gains_buffer_
Definition: pid.h:393
bool initParam(const std::string &prefix, const bool quiet=false)
Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains...
Definition: pid.cpp:100
double d_error_
Definition: pid.h:401
void getCurrentPIDErrors(double *pe, double *ie, double *de)
Return PID error terms for the controller.
Definition: pid.cpp:401
DynamicReconfigServer::CallbackType param_reconfig_callback_
Definition: pid.h:408
void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t)
Update the PID parameters from dynamics reconfigure.
Definition: pid.cpp:290
ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt)
Update the Pid loop with nonuniform time step size.
Definition: pid.cpp:316
bool initXml(TiXmlElement *config)
Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains...
Definition: pid.cpp:157
void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
Set PID gains for the controller.
Definition: pid.cpp:229
Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
Definition: pid.h:136
boost::shared_ptr< realtime_tools::RealtimePublisher< control_msgs::PidState > > state_publisher_
Definition: pid.h:395
double i_error_
Definition: pid.h:400
boost::recursive_mutex param_reconfig_mutex_
Definition: pid.h:410
Pid & operator=(const Pid &source)
Custom assignment operator Does not initialize dynamic reconfigure for PID gains. ...
Definition: pid.h:375
double p_error_last_
Definition: pid.h:398
bool init(const ros::NodeHandle &n, const bool quiet=false)
Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID ...
Definition: pid.cpp:106
void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
Zeros out Pid values and initialize Pid-gains and integral term limits Does not initialize dynamic re...
Definition: pid.cpp:93
~Pid()
Destructor of Pid class.
Definition: pid.cpp:69
bool publish_state_
Definition: pid.h:396
double computeCommand(double error, ros::Duration dt)
Set the PID error and compute the PID command with nonuniform time step size. The derivative error is...
Definition: pid.cpp:298
Gains(double p, double i, double d, double i_max, double i_min)
Definition: pid.h:127
bool dynamic_reconfig_initialized_
Definition: pid.h:405
Pid(double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=-0.0, bool antiwindup=false)
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits...
Definition: pid.cpp:51
Store gains in a struct to allow easier realtime buffer usage.
Definition: pid.h:124
dynamic_reconfigure::Server< control_toolbox::ParametersConfig > DynamicReconfigServer
Definition: pid.h:406
#define ROS_DEPRECATED
A basic pid class.
Definition: pid.h:117
void printValues()
Print to console the current parameters.
Definition: pid.cpp:411
Gains getGains()
Get PID gains for the controller.
Definition: pid.cpp:224
void setCurrentCmd(double cmd)
Set current command for this PID controller.
Definition: pid.cpp:391
void updateDynamicReconfig()
Set Dynamic Reconfigure&#39;s gains to Pid&#39;s values.
Definition: pid.cpp:244
void initDynamicReconfig(ros::NodeHandle &node)
Start the dynamic reconfigure node and load the default values.
Definition: pid.cpp:180
boost::shared_ptr< DynamicReconfigServer > param_reconfig_server_
Definition: pid.h:407
double getCurrentCmd()
Return current command for this PID controller.
Definition: pid.cpp:396
double p_error_
Definition: pid.h:399
void reset()
Reset the state of this PID controller.
Definition: pid.cpp:197


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Fri Feb 1 2019 03:25:58