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 
250  void reset(double d_error, double i_error);
251 
260  void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
261  void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
262 
267  Gains getGains();
268 
277  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
278 
283  void setGains(const Gains &gains);
284 
288  void updateDynamicReconfig();
289  void updateDynamicReconfig(Gains gains_config);
290  void updateDynamicReconfig(control_toolbox::ParametersConfig config);
291 
295  void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/);
296 
307  double computeCommand(double error, ros::Duration dt);
308 
320  double computeCommand(double error, double error_dot, ros::Duration dt);
321 
334  ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt);
335 
350  ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt);
351 
355  void setCurrentCmd(double cmd);
356 
360  double getCurrentCmd();
361 
368  void getCurrentPIDErrors(double *pe, double *ie, double *de);
369 
370 
374  void printValues();
375 
380  Pid &operator =(const Pid& source)
381  {
382  if (this == &source)
383  return *this;
384 
385  // Copy the realtime buffer to then new PID class
386  gains_buffer_ = source.gains_buffer_;
387 
388  // Reset the state of this PID controller
389  reset();
390 
391  return *this;
392  }
393 
394 private:
395 
396  // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
397  // blocking the realtime update loop
399 
402 
404  double p_error_last_;
405  double p_error_;
406  double i_error_;
407  double d_error_;
408  double cmd_;
410  // Dynamics reconfigure
412  typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> DynamicReconfigServer;
414  DynamicReconfigServer::CallbackType param_reconfig_callback_;
415 
416  boost::recursive_mutex param_reconfig_mutex_;
417 
418 };
419 
420 }
421 
422 #endif
realtime_tools::RealtimeBuffer< Gains > gains_buffer_
Definition: pid.h:398
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:407
void getCurrentPIDErrors(double *pe, double *ie, double *de)
Return PID error terms for the controller.
Definition: pid.cpp:411
DynamicReconfigServer::CallbackType param_reconfig_callback_
Definition: pid.h:414
void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t)
Update the PID parameters from dynamics reconfigure.
Definition: pid.cpp:297
ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt)
Update the Pid loop with nonuniform time step size.
Definition: pid.cpp:326
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:236
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:400
double i_error_
Definition: pid.h:406
boost::recursive_mutex param_reconfig_mutex_
Definition: pid.h:416
Pid & operator=(const Pid &source)
Custom assignment operator Does not initialize dynamic reconfigure for PID gains. ...
Definition: pid.h:380
double p_error_last_
Definition: pid.h:404
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
bool valid_p_error_last_
Definition: pid.h:403
~Pid()
Destructor of Pid class.
Definition: pid.cpp:69
bool publish_state_
Definition: pid.h:401
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:305
Gains(double p, double i, double d, double i_max, double i_min)
Definition: pid.h:127
bool dynamic_reconfig_initialized_
Definition: pid.h:411
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:412
#define ROS_DEPRECATED
A basic pid class.
Definition: pid.h:117
void printValues()
Print to console the current parameters.
Definition: pid.cpp:421
Gains getGains()
Get PID gains for the controller.
Definition: pid.cpp:231
void setCurrentCmd(double cmd)
Set current command for this PID controller.
Definition: pid.cpp:401
void updateDynamicReconfig()
Set Dynamic Reconfigure&#39;s gains to Pid&#39;s values.
Definition: pid.cpp:251
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:413
double getCurrentCmd()
Return current command for this PID controller.
Definition: pid.cpp:406
double p_error_
Definition: pid.h:405
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 Wed May 11 2022 02:11:48