From d5f9923a2e3ddcf354e5fbb1bd6845c78ad551f6 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 10 Jul 2013 18:09:35 -0700 Subject: [PATCH 01/12] Added dynamic reconfigure for PID gains --- CMakeLists.txt | 29 ++++++++++++----- cfg/Parameters.cfg | 54 +++++++++++++++++++++++++++++++ include/control_toolbox/pid.h | 26 ++++++++++++++- package.xml | 3 ++ src/pid.cpp | 61 ++++++++++++++++++++++++++++++++--- 5 files changed, 160 insertions(+), 13 deletions(-) create mode 100755 cfg/Parameters.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index c3f1fe06..273245ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,18 +19,30 @@ if(USE_ROSBUILD) # rosbuild_add_executable(test_linear test/linear.cpp) # Tests - rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp) + rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp) target_link_libraries(test/pid_tests control_toolbox) else() # Load catkin and all dependencies required for this package - find_package(catkin REQUIRED COMPONENTS rosconsole message_generation tf roscpp angles) + find_package(catkin REQUIRED COMPONENTS + rosconsole + message_generation + tf + roscpp + angles + dynamic_reconfigure + ) include_directories( - include + include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) + # Dynamics reconfigure + generate_dynamic_reconfigure_options( + cfg/Parameters.cfg + ) + # Add services and generate them add_service_files( FILES @@ -49,6 +61,7 @@ else() src/sinusoid.cpp src/limited_proxy.cpp ) + add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) target_link_libraries(control_toolbox tinyxml) add_dependencies(control_toolbox ${PROJECT_NAME}_gencpp) @@ -56,13 +69,13 @@ else() # Declare catkin package catkin_package( DEPENDS tinyxml - CATKIN_DEPENDS rosconsole tf roscpp angles - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS rosconsole tf roscpp angles dynamic_reconfigure + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ) # Tests - catkin_add_gtest(pid_tests test/pid_tests.cpp) + catkin_add_gtest(pid_tests test/pid_tests.cpp) target_link_libraries(pid_tests ${PROJECT_NAME}) # add_executable(test_linear test/linear.cpp) @@ -78,6 +91,6 @@ else() ) install(DIRECTORY scripts/ - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) endif() diff --git a/cfg/Parameters.cfg b/cfg/Parameters.cfg new file mode 100755 index 00000000..830a737b --- /dev/null +++ b/cfg/Parameters.cfg @@ -0,0 +1,54 @@ +#! /usr/bin/env python +#********************************************************************* +#* Software License Agreement (BSD License) +#* +#* Copyright (c) 2013, Open Source Robotics Foundation +#* All rights reserved. +#* +#* Redistribution and use in source and binary forms, with or without +#* modification, are permitted provided that the following conditions +#* are met: +#* +#* * Redistributions of source code must retain the above copyright +#* notice, this list of conditions and the following disclaimer. +#* * Redistributions in binary form must reproduce the above +#* copyright notice, this list of conditions and the following +#* disclaimer in the documentation and/or other materials provided +#* with the distribution. +#* * Neither the name of the Open Source Robotics Foundation +#* nor the names of its contributors may be +#* used to endorse or promote products derived +#* from this software without specific prior written permission. +#* +#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +#* POSSIBILITY OF SUCH DAMAGE. +#*********************************************************************/ + +# Author: Dave Coleman +# Desc: Allows PID parameters, etc to be tuned in realtime using dynamic reconfigure + + +PACKAGE='control_toolbox' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# Name Type Level Description Default Min Max +gen.add( "p_gain" , double_t, 1,"Proportional gain.", 10.0 , 0 , 1000) +gen.add( "i_gain" , double_t, 1,"Integral gain.", 10.0 , 0 , 100) +gen.add( "d_gain" , double_t, 1,"Derivative gain.", 10.0 , 0 , 100) +gen.add( "i_clamp_min" , double_t, 1,"Min bounds for the integral windup", 10.0 , 0 , 100) +gen.add( "i_clamp_max" , double_t, 1,"Max bounds for the integral windup", 10.0 , 0 , 100) + # PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py +exit(gen.generate(PACKAGE, "control_toolbox", "Parameters")) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index dceedc7c..73277601 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -36,7 +36,11 @@ #include -#include "ros/node_handle.h" +#include + +// Dynamic reconfigure +#include +#include class TiXmlElement; @@ -246,7 +250,19 @@ class Pid */ ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt); + /** + * \brief Update the PID parameters from dynamics reconfigure + */ + void parameterReconfigureCallback(control_toolbox::ParametersConfig &config, uint32_t level); + + /** + * @brief Start the dynamic reconfigure node and load the default values + */ + void initDynamicReconfigure(ros::NodeHandle &node); + /** + * @brief Custom assignment operator + */ Pid &operator =(const Pid& p) { if (this == &p) @@ -273,6 +289,14 @@ class Pid double i_max_; /**< Maximum allowable integral term. */ double i_min_; /**< Minimum allowable integral term. */ double cmd_; /**< Command to send. */ + + // Dynamics reconfigure + boost::shared_ptr< dynamic_reconfigure::Server + > param_reconfigure_srv_; + dynamic_reconfigure::Server< + control_toolbox::ParametersConfig>::CallbackType param_reconfigure_callback_; + boost::recursive_mutex param_reconfigure_mutex_; + }; } diff --git a/package.xml b/package.xml index 7ee5c0ca..6f8e28cc 100644 --- a/package.xml +++ b/package.xml @@ -20,11 +20,14 @@ roscpp angles message_generation + dynamic_reconfigure + rosconsole tf roscpp angles message_runtime + dynamic_reconfigure diff --git a/src/pid.cpp b/src/pid.cpp index 751b3a7a..0a9de8d7 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -32,7 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -// Original version: Melonee Wise +/* + Author: Melonee Wise + Contributors: Jonathan Bohren, Bob Holmberg, Wim Meeussen, Dave Coleman + Desc: Implements a standard proportional-integral-derivative controller +*/ #include "control_toolbox/pid.h" #include "tinyxml.h" @@ -99,28 +103,77 @@ bool Pid::initXml(TiXmlElement *config) i_min_ = -std::abs(i_clamp); reset(); + + // Create node handle for dynamic reconfigure + std::string prefix = "pid"; // \todo make better default prefix somehow? + ros::NodeHandle nh(prefix); + initDynamicReconfigure(nh); + return true; } bool Pid::init(const ros::NodeHandle &node) { ros::NodeHandle n(node); - if (!n.getParam("p", p_gain_)) { + + // Load PID gains from parameter server + if (!n.getParam("p", p_gain_)) + { ROS_ERROR("No p gain specified for pid. Namespace: %s", n.getNamespace().c_str()); return false; } - n.param("i", i_gain_, 0.0); - n.param("d", d_gain_, 0.0); + if (!n.getParam("i", i_gain_)) + { + ROS_ERROR("No i gain specified for pid. Namespace: %s", n.getNamespace().c_str()); + return false; + } + if (!n.getParam("d", d_gain_)) + { + ROS_ERROR("No d gain specified for pid. Namespace: %s", n.getNamespace().c_str()); + return false; + } + // Load integral clamp from param server or default to 0 double i_clamp; n.param("i_clamp", i_clamp, 0.0); i_max_ = std::abs(i_clamp); i_min_ = -std::abs(i_clamp); reset(); + + initDynamicReconfigure(n); + return true; } +void Pid::initDynamicReconfigure(ros::NodeHandle &node) +{ + ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace " + << node.getNamespace()); + + // Start dynamic reconfigure server + param_reconfigure_srv_.reset(new dynamic_reconfigure::Server + (param_reconfigure_mutex_, node)); + + // Get starting values + control_toolbox::ParametersConfig config; + + getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); + + // Set starting values + param_reconfigure_srv_->updateConfig(config); + + // Set callback + param_reconfigure_callback_ = boost::bind(&Pid::parameterReconfigureCallback, this, _1, _2); + param_reconfigure_srv_->setCallback(param_reconfigure_callback_); +} + +void Pid::parameterReconfigureCallback(control_toolbox::ParametersConfig &config, uint32_t level) +{ + ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); + setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); +} + double Pid::computeCommand(double error, ros::Duration dt) { double p_term, d_term; From 1ec720c2c006a6714a1671871123b1d9ac351182 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 15 Jul 2013 10:59:48 -0700 Subject: [PATCH 02/12] Updated CMakeLists.txt and made fixes per Adolfo --- CMakeLists.txt | 35 +++++----- include/control_toolbox/pid.h | 118 ++++++++++++++++++---------------- src/pid.cpp | 46 ++++++------- 3 files changed, 107 insertions(+), 92 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b2c3d35..b757aacf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ if(USE_ROSBUILD) rosbuild_init() #rosbuild_genmsg() rosbuild_gensrv() - rosbuild_add_library(control_toolbox + rosbuild_add_library(${PROJECT_NAME} src/pid.cpp src/pid_gains_setter.cpp src/sine_sweep.cpp @@ -14,13 +14,13 @@ if(USE_ROSBUILD) src/sinusoid.cpp src/limited_proxy.cpp ) - target_link_libraries(control_toolbox tinyxml) + target_link_libraries(${PROJECT_NAME} tinyxml) # rosbuild_add_executable(test_linear test/linear.cpp) # Tests rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp) - target_link_libraries(test/pid_tests control_toolbox) + target_link_libraries(test/pid_tests ${PROJECT_NAME}) else() @@ -36,7 +36,11 @@ else() include_directories( include - ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} + ) + + find_package(Boost REQUIRED COMPONENTS system thread) # Dynamics reconfigure generate_dynamic_reconfigure_options( @@ -53,6 +57,14 @@ else() DEPENDENCIES std_msgs ) + # Declare catkin package + catkin_package( + DEPENDS tinyxml + CATKIN_DEPENDS rosconsole tf roscpp angles dynamic_reconfigure + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + ) + add_library(${PROJECT_NAME} src/pid.cpp src/pid_gains_setter.cpp @@ -61,21 +73,14 @@ else() src/sinusoid.cpp src/limited_proxy.cpp ) - add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) + add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # wait for dynamic reconfigure - target_link_libraries(control_toolbox tinyxml ${catkin_LIBRARIES}) - add_dependencies(control_toolbox ${PROJECT_NAME}_gencpp) + target_link_libraries(${PROJECT_NAME} tinyxml ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - # Declare catkin package - catkin_package( - DEPENDS tinyxml - CATKIN_DEPENDS rosconsole tf roscpp angles dynamic_reconfigure - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - ) + add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) # wait for msgs # Tests - catkin_add_gtest(pid_tests test/pid_tests.cpp) + catkin_add_gtest(pid_tests test/pid_tests.cpp) target_link_libraries(pid_tests ${catkin_LIBRARIES} ${PROJECT_NAME}) # add_executable(test_linear test/linear.cpp) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 73277601..04312509 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -41,6 +41,7 @@ // Dynamic reconfigure #include #include +#include class TiXmlElement; @@ -48,60 +49,60 @@ namespace control_toolbox { /***************************************************/ /*! \class Pid - \brief A basic pid class. + \brief A basic pid class. - This class implements a generic structure that - can be used to create a wide range of pid - controllers. It can function independently or - be subclassed to provide more specific controls - based on a particular control loop. + This class implements a generic structure that + can be used to create a wide range of pid + controllers. It can function independently or + be subclassed to provide more specific controls + based on a particular control loop. - In particular, this class implements the standard - pid equation: + In particular, this class implements the standard + pid equation: - \f$command = -p_{term} - i_{term} - d_{term} \f$ + \f$command = -p_{term} - i_{term} - d_{term} \f$ - where:
-
    -
  • \f$ p_{term} = p_{gain} * p_{error} \f$ -
  • \f$ i_{term} = i_{term} + \int{i_{gain} * p_{error} * dt} \f$ -
  • \f$ d_{term} = d_{gain} * d_{error} \f$ -
  • \f$ d_{error} = (p_{error} - p_{error last}) / dt \f$ -
+ where:
+
    +
  • \f$ p_{term} = p_{gain} * p_{error} \f$ +
  • \f$ i_{term} = i_{term} + \int{i_{gain} * p_{error} * dt} \f$ +
  • \f$ d_{term} = d_{gain} * d_{error} \f$ +
  • \f$ d_{error} = (p_{error} - p_{error last}) / dt \f$ +
- given:
-
    -
  • \f$ p_{error} = p_{state} - p_{target} \f$. -
+ given:
+
    +
  • \f$ p_{error} = p_{state} - p_{target} \f$. +
- \section ROS ROS interface + \section ROS ROS interface - \param p Proportional gain + \param p Proportional gain - \param d Derivative gain + \param d Derivative gain - \param i Integral gain + \param i Integral gain - \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ + \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ - \section Usage + \section Usage - To use the Pid class, you should first call some version of init() - (in non-realtime) and then call updatePid() at every update step. - For example: + To use the Pid class, you should first call some version of init() + (in non-realtime) and then call updatePid() at every update step. + For example: -\verbatim -control_toolbox::Pid pid; -pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3); -double position_desi_ = 0.5; -... -ros::Time last_time = ros::Time::now(); -while (true) { + \verbatim + control_toolbox::Pid pid; + pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3); + double position_desi_ = 0.5; + ... + ros::Time last_time = ros::Time::now(); + while (true) { ros::Time time = ros::Time::now(); double effort = pid.updatePid(currentPosition() - position_desi_, time - last_time); last_time = time; -} -\endverbatim + } + \endverbatim */ /***************************************************/ @@ -137,18 +138,18 @@ class Pid * \param i_min The min integral windup. */ void initPid(double p, double i, double d, double i_max, double i_min); - - /*! - * \brief Initialize PID with the parameters in a namespace - * + + /*! + * \brief Initialize PID with the parameters in a namespace + * * \param prefix The namespace prefix. */ bool initParam(const std::string& prefix); bool initXml(TiXmlElement *config); - /*! + /*! * \brief Initialize PID with the parameters in a NodeHandle namespace - * - * \param n The NodeHandle which should be used to query parameters. + * + * \param n The NodeHandle which should be used to query parameters. */ bool init(const ros::NodeHandle &n); @@ -210,7 +211,7 @@ class Pid /*! * \brief Set the PID error and compute the PID command with nonuniform * time step size. This also allows the user to pass in a precomputed - * derivative error. + * derivative error. * * \param error Error since last call (error = target - state) * \param error_dot d(Error)/dt since last call @@ -221,7 +222,7 @@ class Pid double computeCommand(double error, double error_dot, ros::Duration dt); /*! - * \brief Update the Pid loop with nonuniform time step size. + * \brief Update the Pid loop with nonuniform time step size. * * \deprecated This function assumes p_error = (state - target) * which is an unconventional definition of the error. Please use \ref @@ -236,7 +237,7 @@ class Pid /*! * \brief Update the Pid loop with nonuniform time step size. This update - * call allows the user to pass in a precomputed derivative error. + * call allows the user to pass in a precomputed derivative error. * * \deprecated This function assumes p_error = (state - target) * which is an unconventional definition of the error. Please use \ref @@ -253,12 +254,18 @@ class Pid /** * \brief Update the PID parameters from dynamics reconfigure */ - void parameterReconfigureCallback(control_toolbox::ParametersConfig &config, uint32_t level); + void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level); /** * @brief Start the dynamic reconfigure node and load the default values + * @param node - a node handle where dynamic reconfigure services will be published */ - void initDynamicReconfigure(ros::NodeHandle &node); + void initDynamicReconfig(ros::NodeHandle &node); + + /** + * @brief Set Dynamic Reconfigure's gains to Pid's values + */ + void updateDynamicReconfig(); /** * @brief Custom assignment operator @@ -291,11 +298,12 @@ class Pid double cmd_; /**< Command to send. */ // Dynamics reconfigure - boost::shared_ptr< dynamic_reconfigure::Server - > param_reconfigure_srv_; - dynamic_reconfigure::Server< - control_toolbox::ParametersConfig>::CallbackType param_reconfigure_callback_; - boost::recursive_mutex param_reconfigure_mutex_; + typedef dynamic_reconfigure::Server DynamicReconfigServer; + + boost::shared_ptr param_reconfig_server_; + DynamicReconfigServer::CallbackType param_reconfig_callback_; + + boost::recursive_mutex param_reconfig_mutex_; }; diff --git a/src/pid.cpp b/src/pid.cpp index 0a9de8d7..1811e8e5 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -84,6 +84,9 @@ void Pid::setGains(double p, double i, double d, double i_max, double i_min) d_gain_ = d; i_max_ = i_max; i_min_ = i_min; + + // Update dynamic reconfigure with the new gains + updateDynamicReconfig(); } bool Pid::initParam(const std::string& prefix) @@ -107,7 +110,7 @@ bool Pid::initXml(TiXmlElement *config) // Create node handle for dynamic reconfigure std::string prefix = "pid"; // \todo make better default prefix somehow? ros::NodeHandle nh(prefix); - initDynamicReconfigure(nh); + initDynamicReconfig(nh); return true; } @@ -122,16 +125,8 @@ bool Pid::init(const ros::NodeHandle &node) ROS_ERROR("No p gain specified for pid. Namespace: %s", n.getNamespace().c_str()); return false; } - if (!n.getParam("i", i_gain_)) - { - ROS_ERROR("No i gain specified for pid. Namespace: %s", n.getNamespace().c_str()); - return false; - } - if (!n.getParam("d", d_gain_)) - { - ROS_ERROR("No d gain specified for pid. Namespace: %s", n.getNamespace().c_str()); - return false; - } + n.param("i", i_gain_, 0.0); + n.param("d", d_gain_, 0.0); // Load integral clamp from param server or default to 0 double i_clamp; @@ -141,34 +136,41 @@ bool Pid::init(const ros::NodeHandle &node) reset(); - initDynamicReconfigure(n); + initDynamicReconfig(n); return true; } -void Pid::initDynamicReconfigure(ros::NodeHandle &node) +void Pid::initDynamicReconfig(ros::NodeHandle &node) { ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace " << node.getNamespace()); // Start dynamic reconfigure server - param_reconfigure_srv_.reset(new dynamic_reconfigure::Server - (param_reconfigure_mutex_, node)); + param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node)); + // Set Dynamic Reconfigure's gains to Pid's values + updateDynamicReconfig(); + + // Set callback + param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2); + param_reconfig_server_->setCallback(param_reconfig_callback_); +} + +void Pid::updateDynamicReconfig() +{ // Get starting values control_toolbox::ParametersConfig config; getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); - // Set starting values - param_reconfigure_srv_->updateConfig(config); - - // Set callback - param_reconfigure_callback_ = boost::bind(&Pid::parameterReconfigureCallback, this, _1, _2); - param_reconfigure_srv_->setCallback(param_reconfigure_callback_); + // Set starting values, using a shared mutex with dynamic reconfig + param_reconfig_mutex_.lock(); + param_reconfig_server_->updateConfig(config); + param_reconfig_mutex_.unlock(); } -void Pid::parameterReconfigureCallback(control_toolbox::ParametersConfig &config, uint32_t level) +void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level) { ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); From 3ed065ae10ca29d71f1ecc389feea422155eeed1 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 15 Jul 2013 15:07:30 -0700 Subject: [PATCH 03/12] Fixes per Austin review --- CMakeLists.txt | 4 ++-- include/control_toolbox/pid.h | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b757aacf..cf9940d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,14 +34,14 @@ else() dynamic_reconfigure ) + find_package(Boost REQUIRED COMPONENTS system thread) + include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ) - find_package(Boost REQUIRED COMPONENTS system thread) - # Dynamics reconfigure generate_dynamic_reconfigure_options( cfg/Parameters.cfg diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 04312509..8ff69d01 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -281,6 +281,9 @@ class Pid i_max_ = p.i_max_; i_min_ = p.i_min_; + // Update dynamic reconfigure with the new gains + updateDynamicReconfig(); + p_error_last_ = p_error_ = i_term_ = d_error_ = cmd_ = 0.0; return *this; } From 8f83f74ec0b41211884f7b8ea251d2de1edbaabe Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 17 Jul 2013 17:43:25 -0700 Subject: [PATCH 04/12] Added realtime buffer to PID, re-ordered functions to more logical order and to match header file --- include/control_toolbox/pid.h | 125 +++++++++++------- src/pid.cpp | 236 ++++++++++++++++++++++------------ 2 files changed, 235 insertions(+), 126 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 8ff69d01..db4ea8a7 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -43,6 +43,9 @@ #include #include +// Realtime buffer +#include + class TiXmlElement; namespace control_toolbox { @@ -107,6 +110,26 @@ namespace control_toolbox { */ /***************************************************/ +// Store gains in a struct to allow easier realtime buffer +struct Gains +{ + // Constructor + Gains(double p, double i, double d, double i_max, double i_min) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min) + {} + Gains() {} + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ +}; + + class Pid { public: @@ -145,7 +168,7 @@ class Pid * \param prefix The namespace prefix. */ bool initParam(const std::string& prefix); - bool initXml(TiXmlElement *config); + /*! * \brief Initialize PID with the parameters in a NodeHandle namespace * @@ -154,47 +177,66 @@ class Pid bool init(const ros::NodeHandle &n); /*! - * \brief Reset the state of this PID controller - */ - void reset(); - - /*! - * \brief Set current command for this PID controller + * \brief Initialize PID with the parameters in an XML element + * + * \param config the XML element */ - void setCurrentCmd(double cmd); + bool initXml(TiXmlElement *config); - /*! - * \brief Return current command for this PID controller + /** + * @brief Start the dynamic reconfigure node and load the default values + * @param node - a node handle where dynamic reconfigure services will be published */ - double getCurrentCmd(); + void initDynamicReconfig(ros::NodeHandle &node); /*! - * \brief Return PID error terms for the controller. - * \param pe The proportional error. - * \param ie The integral error. - * \param de The derivative error. + * \brief Reset the state of this PID controller */ - void getCurrentPIDErrors(double *pe, double *ie, double *de); + void reset(); /*! - * \brief Set PID gains for the controller. + * \brief Get PID gains for the controller. * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void setGains(double p, double i, double d, double i_max, double i_min); + void getGains(double &p, double &i, double &d, double &i_max, double &i_min); /*! * \brief Get PID gains for the controller. + * \param gains A struct of the PID gain values + */ + void getGains(Gains &gains); + + /*! + * \brief Set PID gains for the controller. * \param p The proportional gain. * \param i The integral gain. * \param d The derivative gain. * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void getGains(double &p, double &i, double &d, double &i_max, double &i_min); + void setGains(double p, double i, double d, double i_max, double i_min); + + /*! + * \brief Set PID gains for the controller. + * \param gains A struct of the PID gain values + */ + void setGains(Gains gains); + + /** + * @brief Set Dynamic Reconfigure's gains to Pid's values + */ + void updateDynamicReconfig(); + void updateDynamicReconfig(Gains gains_config); + void updateDynamicReconfig(control_toolbox::ParametersConfig config); + + /** + * \brief Update the PID parameters from dynamics reconfigure + */ + void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -251,21 +293,23 @@ class Pid */ ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt); - /** - * \brief Update the PID parameters from dynamics reconfigure + /*! + * \brief Set current command for this PID controller */ - void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level); + void setCurrentCmd(double cmd); - /** - * @brief Start the dynamic reconfigure node and load the default values - * @param node - a node handle where dynamic reconfigure services will be published + /*! + * \brief Return current command for this PID controller */ - void initDynamicReconfig(ros::NodeHandle &node); + double getCurrentCmd(); - /** - * @brief Set Dynamic Reconfigure's gains to Pid's values + /*! + * \brief Return PID error terms for the controller. + * \param pe The proportional error. + * \param ie The integral error. + * \param de The derivative error. */ - void updateDynamicReconfig(); + void getCurrentPIDErrors(double *pe, double *ie, double *de); /** * @brief Custom assignment operator @@ -275,34 +319,27 @@ class Pid if (this == &p) return *this; - p_gain_ = p.p_gain_; - i_gain_ = p.i_gain_; - d_gain_ = p.d_gain_; - i_max_ = p.i_max_; - i_min_ = p.i_min_; + Gains gains; + // \todo enable this! p.getGains(gains); + setGains(gains); - // Update dynamic reconfigure with the new gains - updateDynamicReconfig(); + reset(); - p_error_last_ = p_error_ = i_term_ = d_error_ = cmd_ = 0.0; return *this; } private: + realtime_tools::RealtimeBuffer gains_buffer_; + double p_error_last_; /**< _Save position state for derivative state calculation. */ double p_error_; /**< Position error. */ double d_error_; /**< Derivative error. */ double i_term_; /**< Integral term. */ - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ double cmd_; /**< Command to send. */ // Dynamics reconfigure - typedef dynamic_reconfigure::Server DynamicReconfigServer; - + bool dynamic_reconfig_initialized_; + typedef dynamic_reconfigure::Server DynamicReconfigServer; boost::shared_ptr param_reconfig_server_; DynamicReconfigServer::CallbackType param_reconfig_callback_; diff --git a/src/pid.cpp b/src/pid.cpp index 1811e8e5..fcf85fba 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -38,105 +38,89 @@ Desc: Implements a standard proportional-integral-derivative controller */ -#include "control_toolbox/pid.h" -#include "tinyxml.h" +#include +#include namespace control_toolbox { -Pid::Pid(double p, double i, double d, double i_max, double i_min) : - p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min) -{ - this->reset(); -} +static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default prefix? -Pid::~Pid() +Pid::Pid(double p, double i, double d, double i_max, double i_min) + : dynamic_reconfig_initialized_(false) { -} + setGains(p,i,d,i_max,i_min); -void Pid::initPid(double p, double i, double d, double i_max, double i_min) -{ - this->setGains(p,i,d,i_max,i_min); reset(); } -void Pid::reset() +Pid::~Pid() { - p_error_last_ = 0.0; - p_error_ = 0.0; - d_error_ = 0.0; - i_term_ = 0.0; - cmd_ = 0.0; } -void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +void Pid::initPid(double p, double i, double d, double i_max, double i_min) { - p = p_gain_; - i = i_gain_; - d = d_gain_; - i_max = i_max_; - i_min = i_min_; -} + // Create node handle for dynamic reconfigure + ros::NodeHandle nh(DEFAULT_NAMESPACE); -void Pid::setGains(double p, double i, double d, double i_max, double i_min) -{ - p_gain_ = p; - i_gain_ = i; - d_gain_ = d; - i_max_ = i_max; - i_min_ = i_min; + setGains(p,i,d,i_max,i_min); - // Update dynamic reconfigure with the new gains - updateDynamicReconfig(); + reset(); + initDynamicReconfig(nh); } bool Pid::initParam(const std::string& prefix) { ros::NodeHandle nh(prefix); - return this->init(nh); + return init(nh); } -bool Pid::initXml(TiXmlElement *config) +bool Pid::init(const ros::NodeHandle &node) { - p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0; - i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0; - d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0; + ros::NodeHandle nh(node); + + Gains gains; + + // Load PID gains from parameter server + if (!nh.getParam("p", gains.p_gain_)) + { + ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str()); + return false; + } + // Only the P gain is required, the I and D gains are optional and default to 0: + nh.param("i", gains.i_gain_, 0.0); + nh.param("d", gains.d_gain_, 0.0); + + // Load integral clamp from param server or default to 0 double i_clamp; - i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0; - i_max_ = std::abs(i_clamp); - i_min_ = -std::abs(i_clamp); + nh.param("i_clamp", i_clamp, 0.0); + gains.i_max_ = std::abs(i_clamp); + gains.i_min_ = -std::abs(i_clamp); + setGains(gains); reset(); - - // Create node handle for dynamic reconfigure - std::string prefix = "pid"; // \todo make better default prefix somehow? - ros::NodeHandle nh(prefix); initDynamicReconfig(nh); return true; } -bool Pid::init(const ros::NodeHandle &node) +bool Pid::initXml(TiXmlElement *config) { - ros::NodeHandle n(node); - - // Load PID gains from parameter server - if (!n.getParam("p", p_gain_)) - { - ROS_ERROR("No p gain specified for pid. Namespace: %s", n.getNamespace().c_str()); - return false; - } - n.param("i", i_gain_, 0.0); - n.param("d", d_gain_, 0.0); + // Create node handle for dynamic reconfigure + ros::NodeHandle nh(DEFAULT_NAMESPACE); - // Load integral clamp from param server or default to 0 double i_clamp; - n.param("i_clamp", i_clamp, 0.0); - i_max_ = std::abs(i_clamp); - i_min_ = -std::abs(i_clamp); + i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0; - reset(); + setGains( + config->Attribute("p") ? atof(config->Attribute("p")) : 0.0, + config->Attribute("i") ? atof(config->Attribute("i")) : 0.0, + config->Attribute("d") ? atof(config->Attribute("d")) : 0.0, + std::abs(i_clamp), + -std::abs(i_clamp) + ); - initDynamicReconfig(n); + reset(); + initDynamicReconfig(nh); return true; } @@ -155,15 +139,89 @@ void Pid::initDynamicReconfig(ros::NodeHandle &node) // Set callback param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2); param_reconfig_server_->setCallback(param_reconfig_callback_); + + dynamic_reconfig_initialized_ = true; +} + +void Pid::reset() +{ + p_error_last_ = 0.0; + p_error_ = 0.0; + d_error_ = 0.0; + i_term_ = 0.0; + cmd_ = 0.0; +} + +void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +{ + Gains gains = *gains_buffer_.readFromRT(); + + p = gains.p_gain_; + i = gains.i_gain_; + d = gains.d_gain_; + i_max = gains.i_max_; + i_min = gains.i_min_; +} + +void Pid::getGains(Gains &gains) +{ + gains = *gains_buffer_.readFromRT(); +} + +void Pid::setGains(double p, double i, double d, double i_max, double i_min) +{ + Gains gains(p,i,d,i_max,i_min); + + setGains(gains); +} + +void Pid::setGains(Gains gains) +{ + gains_buffer_.writeFromNonRT(gains); + + // Update dynamic reconfigure with the new gains + updateDynamicReconfig(gains); } void Pid::updateDynamicReconfig() { + // Make sure dynamic reconfigure is initialized + if(!dynamic_reconfig_initialized_) + return; + // Get starting values control_toolbox::ParametersConfig config; + // Get starting values getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); + updateDynamicReconfig(config); +} + +void Pid::updateDynamicReconfig(Gains gains_config) +{ + // Make sure dynamic reconfigure is initialized + if(!dynamic_reconfig_initialized_) + return; + + control_toolbox::ParametersConfig config; + + // Convert to dynamic reconfigure format + config.p_gain = gains_config.p_gain_; + config.i_gain = gains_config.p_gain_; + config.d_gain = gains_config.p_gain_; + config.i_clamp_max = gains_config.i_max_; + config.i_clamp_min = gains_config.i_min_; + + updateDynamicReconfig(config); +} + +void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config) +{ + // Make sure dynamic reconfigure is initialized + if(!dynamic_reconfig_initialized_) + return; + // Set starting values, using a shared mutex with dynamic reconfig param_reconfig_mutex_.lock(); param_reconfig_server_->updateConfig(config); @@ -173,11 +231,15 @@ void Pid::updateDynamicReconfig() void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level) { ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); + // Set the gains setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); } double Pid::computeCommand(double error, ros::Duration dt) { + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + double p_term, d_term; p_error_ = error; // this is error = target - state @@ -185,13 +247,13 @@ double Pid::computeCommand(double error, ros::Duration dt) return 0.0; // Calculate proportional contribution to command - p_term = p_gain_ * p_error_; + p_term = gains.p_gain_ * p_error_; //Calculate integral contribution to command - i_term_ = i_term_ + i_gain_ * dt.toSec() * p_error_; + i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( i_min_, std::min( i_term_, i_max_) ); + i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); // Calculate the derivative error if (dt.toSec() > 0.0) @@ -200,7 +262,7 @@ double Pid::computeCommand(double error, ros::Duration dt) p_error_last_ = p_error_; } // Calculate derivative contribution to command - d_term = d_gain_ * d_error_; + d_term = gains.d_gain_ * d_error_; cmd_ = p_term + i_term_ + d_term; return cmd_; @@ -208,6 +270,9 @@ double Pid::computeCommand(double error, ros::Duration dt) double Pid::updatePid(double error, ros::Duration dt) { + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + double p_term, d_term; p_error_ = error; //this is pError = pState-pTarget @@ -215,13 +280,13 @@ double Pid::updatePid(double error, ros::Duration dt) return 0.0; // Calculate proportional contribution to command - p_term = p_gain_ * p_error_; + p_term = gains.p_gain_ * p_error_; //Calculate integral contribution to command - i_term_ = i_term_ + i_gain_ * dt.toSec() * p_error_; + i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( i_min_, std::min( i_term_, i_max_) ); + i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); // Calculate the derivative error if (dt.toSec() > 0.0) @@ -230,7 +295,7 @@ double Pid::updatePid(double error, ros::Duration dt) p_error_last_ = p_error_; } // Calculate derivative contribution to command - d_term = d_gain_ * d_error_; + d_term = gains.d_gain_ * d_error_; cmd_ = - p_term - i_term_ - d_term; return cmd_; @@ -238,6 +303,9 @@ double Pid::updatePid(double error, ros::Duration dt) double Pid::computeCommand(double error, double error_dot, ros::Duration dt) { + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + double p_term, d_term; p_error_ = error; // this is error = target - state d_error_ = error_dot; @@ -247,16 +315,16 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt) // Calculate proportional contribution to command - p_term = p_gain_ * p_error_; + p_term = gains.p_gain_ * p_error_; //Calculate integral contribution to command - i_term_ = i_term_ + i_gain_ * dt.toSec() * p_error_; + i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( i_min_, std::min( i_term_, i_max_) ); + i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); // Calculate derivative contribution to command - d_term = d_gain_ * d_error_; + d_term = gains.d_gain_ * d_error_; cmd_ = p_term + i_term_ + d_term; return cmd_; @@ -264,6 +332,9 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt) double Pid::updatePid(double error, double error_dot, ros::Duration dt) { + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + double p_term, d_term; p_error_ = error; //this is pError = pState-pTarget d_error_ = error_dot; @@ -273,23 +344,21 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt) // Calculate proportional contribution to command - p_term = p_gain_ * p_error_; + p_term = gains.p_gain_ * p_error_; //Calculate integral contribution to command - i_term_ = i_term_ + i_gain_ * dt.toSec() * p_error_; + i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( i_min_, std::min( i_term_, i_max_) ); + i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); // Calculate derivative contribution to command - d_term = d_gain_ * d_error_; + d_term = gains.d_gain_ * d_error_; cmd_ = - p_term - i_term_ - d_term; return cmd_; } - - void Pid::setCurrentCmd(double cmd) { cmd_ = cmd; @@ -302,8 +371,11 @@ double Pid::getCurrentCmd() void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de) { + // Get the gain parameters from the realtime buffer + Gains gains = *gains_buffer_.readFromRT(); + *pe = p_error_; - *ie = i_gain_ ? i_term_/i_gain_ : 0.0; + *ie = gains.i_gain_ ? i_term_/gains.i_gain_ : 0.0; *de = d_error_; } From 712f2aa5e792cdbd08af4323c9eb3596aa70f92b Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 17 Jul 2013 17:49:46 -0700 Subject: [PATCH 05/12] Added realtime_tools as a dependency in package.xml and CMakeLists --- CMakeLists.txt | 9 ++++++++- package.xml | 4 +++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cf9940d8..90f44e15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,7 @@ else() roscpp angles dynamic_reconfigure + realtime_tools ) find_package(Boost REQUIRED COMPONENTS system thread) @@ -60,7 +61,13 @@ else() # Declare catkin package catkin_package( DEPENDS tinyxml - CATKIN_DEPENDS rosconsole tf roscpp angles dynamic_reconfigure + CATKIN_DEPENDS + rosconsole + tf + roscpp + angles + dynamic_reconfigure + realtime_tools INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ) diff --git a/package.xml b/package.xml index 4c1404ef..cef1c740 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ message_generation dynamic_reconfigure tinyxml + realtime_tools rosconsole tf @@ -31,7 +32,8 @@ message_runtime dynamic_reconfigure tinyxml - + realtime_tools + From 04f11ae95e277caf5d648fb52322996eb3055c49 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 18 Jul 2013 15:56:57 -0700 Subject: [PATCH 06/12] Added new function getGainsConst that allows one to get the PID gains from a const PID class --- include/control_toolbox/pid.h | 91 +++++++++++++++++++++++------------ src/pid.cpp | 21 ++++++-- 2 files changed, 76 insertions(+), 36 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index db4ea8a7..3670ebd4 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -110,33 +110,36 @@ namespace control_toolbox { */ /***************************************************/ -// Store gains in a struct to allow easier realtime buffer -struct Gains -{ - // Constructor - Gains(double p, double i, double d, double i_max, double i_min) - : p_gain_(p), - i_gain_(i), - d_gain_(d), - i_max_(i_max), - i_min_(i_min) - {} - Gains() {} - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ -}; - - class Pid { public: + /*! + * \brief Store gains in a struct to allow easier realtime buffer usage + */ + struct Gains + { + // Optional constructor for passing in values + Gains(double p, double i, double d, double i_max, double i_min) + : p_gain_(p), + i_gain_(i), + d_gain_(d), + i_max_(i_max), + i_min_(i_min) + {} + // Default constructor + Gains() {} + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + }; + /*! * \brief Constructor, zeros out Pid values when created and - * initialize Pid-gains and integral term limits. + * initialize Pid-gains and integral term limits. + * Does not initialize dynamic reconfigure for PID gains * * \param p The proportional gain. * \param i The integral gain. @@ -152,7 +155,8 @@ class Pid ~Pid(); /*! - * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2] + * \brief Zeros out Pid values and initialize Pid-gains and integral term limits + * Does not initialize dynamic reconfigure for PID gains * * \param p The proportional gain. * \param i The integral gain. @@ -162,8 +166,21 @@ class Pid */ void initPid(double p, double i, double d, double i_max, double i_min); + /*! + * \brief Zeros out Pid values and initialize Pid-gains and integral term limits + * Initializes dynamic reconfigure for PID gains + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + */ + void initPid(double p, double i, double d, double i_max, double i_min, const ros::NodeHandle &node); + /*! * \brief Initialize PID with the parameters in a namespace + * Initializes dynamic reconfigure for PID gains * * \param prefix The namespace prefix. */ @@ -171,6 +188,7 @@ class Pid /*! * \brief Initialize PID with the parameters in a NodeHandle namespace + * Initializes dynamic reconfigure for PID gains * * \param n The NodeHandle which should be used to query parameters. */ @@ -178,6 +196,7 @@ class Pid /*! * \brief Initialize PID with the parameters in an XML element + * Initializes dynamic reconfigure for PID gains * * \param config the XML element */ @@ -206,9 +225,15 @@ class Pid /*! * \brief Get PID gains for the controller. - * \param gains A struct of the PID gain values + * \return gains A struct of the PID gain values */ - void getGains(Gains &gains); + Gains getGains(); + + /*! + * \brief Get PID gains for the const controller, without modifying the realtime buffer + * \return gains A struct of the PID gain values + */ + Gains getGainsConst() const; /*! * \brief Set PID gains for the controller. @@ -266,7 +291,7 @@ class Pid /*! * \brief Update the Pid loop with nonuniform time step size. * - * \deprecated This function assumes p_error = (state - target) + * \deprecated in ROS Hydro. This function assumes p_error = (state - target) * which is an unconventional definition of the error. Please use \ref * computeCommand instead, which assumes error = (target - state) . Note * that calls to \ref computeCommand should not be mixed with calls to \ref @@ -281,7 +306,7 @@ class Pid * \brief Update the Pid loop with nonuniform time step size. This update * call allows the user to pass in a precomputed derivative error. * - * \deprecated This function assumes p_error = (state - target) + * \deprecated in ROS Hydro. This function assumes p_error = (state - target) * which is an unconventional definition of the error. Please use \ref * computeCommand instead, which assumes error = (target - state) . Note * that calls to \ref computeCommand should not be mixed with calls to \ref @@ -313,22 +338,26 @@ class Pid /** * @brief Custom assignment operator + * Does not initialize dynamic reconfigure for PID gains */ Pid &operator =(const Pid& p) { if (this == &p) return *this; - Gains gains; - // \todo enable this! p.getGains(gains); - setGains(gains); - + // Copy the realtime buffer to then new PID class + setGains(p.getGainsConst()); + + // Reset the state of this PID controller reset(); - + return *this; } private: + + // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without + // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; double p_error_last_; /**< _Save position state for derivative state calculation. */ diff --git a/src/pid.cpp b/src/pid.cpp index fcf85fba..998fd79a 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -34,7 +34,7 @@ /* Author: Melonee Wise - Contributors: Jonathan Bohren, Bob Holmberg, Wim Meeussen, Dave Coleman + Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen Desc: Implements a standard proportional-integral-derivative controller */ @@ -57,15 +57,21 @@ Pid::~Pid() { } -void Pid::initPid(double p, double i, double d, double i_max, double i_min) +void Pid::initPid(double p, double i, double d, double i_max, double i_min, + const ros::NodeHandle &node) { + initPid(p, i, d, i_max, i_min); + // Create node handle for dynamic reconfigure ros::NodeHandle nh(DEFAULT_NAMESPACE); + initDynamicReconfig(nh); +} +void Pid::initPid(double p, double i, double d, double i_max, double i_min) +{ setGains(p,i,d,i_max,i_min); reset(); - initDynamicReconfig(nh); } bool Pid::initParam(const std::string& prefix) @@ -163,9 +169,14 @@ void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min i_min = gains.i_min_; } -void Pid::getGains(Gains &gains) +Pid::Gains Pid::getGains() +{ + return *gains_buffer_.readFromRT(); +} + +Pid::Gains Pid::getGainsConst() const { - gains = *gains_buffer_.readFromRT(); + return *gains_buffer_.readFromRTConst(); } void Pid::setGains(double p, double i, double d, double i_max, double i_min) From 90ab218e9323e4974bed1f10e318ad885f015973 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 19 Jul 2013 13:09:22 -0700 Subject: [PATCH 07/12] Removed const read, added copy constructor and print values function --- include/control_toolbox/pid.h | 22 +++++++++++++++++----- src/pid.cpp | 30 +++++++++++++++++++++++++++++- 2 files changed, 46 insertions(+), 6 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 3670ebd4..f44cf9d4 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -149,6 +149,12 @@ class Pid */ Pid(double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0); + /** + * \brief Copy constructor required for preventing mutexes from being copied + * \param source - Pid to copy + */ + Pid(Pid &source); + /*! * \brief Destructor of Pid class. */ @@ -336,21 +342,27 @@ class Pid */ void getCurrentPIDErrors(double *pe, double *ie, double *de); - /** + + /*! + * \brief Print to console the current parameters + */ + void printValues() const; + + /*! * @brief Custom assignment operator * Does not initialize dynamic reconfigure for PID gains */ - Pid &operator =(const Pid& p) + Pid &operator =(const Pid& source) { - if (this == &p) + if (this == &source) return *this; // Copy the realtime buffer to then new PID class - setGains(p.getGainsConst()); + setGains(source.getGainsConst()); // Reset the state of this PID controller reset(); - + return *this; } diff --git a/src/pid.cpp b/src/pid.cpp index 998fd79a..afaccc84 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -53,6 +53,15 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min) reset(); } +Pid::Pid(Pid &source) +{ + // Copy the realtime buffer to then new PID class + setGains(source.getGainsConst()); + + // Reset the state of this PID controller + reset(); +} + Pid::~Pid() { } @@ -176,7 +185,7 @@ Pid::Gains Pid::getGains() Pid::Gains Pid::getGainsConst() const { - return *gains_buffer_.readFromRTConst(); + return *gains_buffer_.readFromRT(); } void Pid::setGains(double p, double i, double d, double i_max, double i_min) @@ -390,4 +399,23 @@ void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de) *de = d_error_; } +void Pid::printValues() const +{ + const Gains gains = getGainsConst(); + + ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n" + << " P Gain: " << gains.p_gain_ << "\n" + << " I Gain: " << gains.p_gain_ << "\n" + << " D Gain: " << gains.p_gain_ << "\n" + << " I_Max: " << gains.i_max_ << "\n" + << " I_Min: " << gains.i_min_ << "\n" + << " P_Error_Last: " << p_error_last_ << "\n" + << " P_Error: " << p_error_ << "\n" + << " D_Error: " << d_error_ << "\n" + << " I_Term: " << i_term_ << "\n" + << " Command: " << cmd_ + ); + } + +} // namespace From 88d6bc8a06812f3b55c068b3ec11ee3d332ba3cd Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 22 Jul 2013 13:44:49 -0700 Subject: [PATCH 08/12] Added test for getting/settings gains, copying/assigning pid class --- test/pid_tests.cpp | 125 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 117 insertions(+), 8 deletions(-) diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index e8a44afd..baba5a9d 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -18,12 +18,13 @@ TEST(ParameterTest, zeroITermBadIBoundsTest) double cmd = 0.0; double pe,ie,de; - cmd = pid.updatePid(-1.0, ros::Duration(1.0)); + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); + pid.getCurrentPIDErrors(&pe,&ie,&de); EXPECT_FALSE(boost::math::isinf(ie)); EXPECT_FALSE(boost::math::isnan(cmd)); - cmd = pid.updatePid(-1.0, ros::Duration(1.0)); + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); pid.getCurrentPIDErrors(&pe,&ie,&de); EXPECT_FALSE(boost::math::isinf(ie)); EXPECT_FALSE(boost::math::isnan(cmd)); @@ -38,15 +39,15 @@ TEST(ParameterTest, integrationWindupTest) double cmd = 0.0; double pe,ie,de; - cmd = pid.updatePid(-1.0, ros::Duration(1.0)); + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); pid.getCurrentPIDErrors(&pe,&ie,&de); EXPECT_EQ(-1.0, ie); - EXPECT_EQ(1.0, cmd); + EXPECT_EQ(-1.0, cmd); - cmd = pid.updatePid(-1.0, ros::Duration(1.0)); + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); pid.getCurrentPIDErrors(&pe,&ie,&de); EXPECT_EQ(-1.0, ie); - EXPECT_EQ(1.0, cmd); + EXPECT_EQ(-1.0, cmd); } TEST(ParameterTest, integrationWindupZeroGainTest) @@ -61,19 +62,127 @@ TEST(ParameterTest, integrationWindupZeroGainTest) double cmd = 0.0; double pe,ie,de; - cmd = pid.updatePid(-1.0, ros::Duration(1.0)); + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); pid.getCurrentPIDErrors(&pe,&ie,&de); EXPECT_LE(i_min, ie); EXPECT_LE(ie, i_max); EXPECT_EQ(0.0, cmd); - cmd = pid.updatePid(-1.0, ros::Duration(1.0)); + cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); pid.getCurrentPIDErrors(&pe,&ie,&de); EXPECT_LE(i_min, ie); EXPECT_LE(ie, i_max); EXPECT_EQ(0.0, cmd); } +TEST(ParameterTest, gainSettingCopyPIDTest) +{ + RecordProperty("description","This test succeeds if a PID object has its gain set at different points in time then the values are get-ed and still remain the same, as well as when PID is copied."); + + // Test values + double p_gain = rand() % 100; + double i_gain = rand() % 100; + double d_gain = rand() % 100; + double i_max = rand() % 100; + double i_min = -1 * rand() % 100; + + // Initialize the default way + Pid pid1(p_gain, i_gain, d_gain, i_max, i_min); + + // Test return values ------------------------------------------------- + double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; + pid1.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + + EXPECT_EQ(p_gain, p_gain_return); + EXPECT_EQ(i_gain, i_gain_return); + EXPECT_EQ(d_gain, d_gain_return); + EXPECT_EQ(i_max, i_max_return); + EXPECT_EQ(i_min, i_min_return); + + // Test return values using struct ------------------------------------------------- + + // New values + p_gain = rand() % 100; + i_gain = rand() % 100; + d_gain = rand() % 100; + i_max = rand() % 100; + i_min = -1 * rand() % 100; + pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min); + + Pid::Gains g1 = pid1.getGains(); + EXPECT_EQ(p_gain, g1.p_gain_); + EXPECT_EQ(i_gain, g1.i_gain_); + EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(i_max, g1.i_max_); + EXPECT_EQ(i_min, g1.i_min_); + + // Test return values using struct - const version ------------------------------------------------- + + // New values + p_gain = rand() % 100; + i_gain = rand() % 100; + d_gain = rand() % 100; + i_max = rand() % 100; + i_min = -1 * rand() % 100; + pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min); + + Pid::Gains g2 = pid1.getGainsConst(); + EXPECT_EQ(p_gain, g2.p_gain_); + EXPECT_EQ(i_gain, g2.i_gain_); + EXPECT_EQ(d_gain, g2.d_gain_); + EXPECT_EQ(i_max, g2.i_max_); + EXPECT_EQ(i_min, g2.i_min_); + + // \todo test initParam() ------------------------------------------------- + + + // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- + + + // Send update command to populate errors ------------------------------------------------- + pid1.setCurrentCmd(10); + pid1.computeCommand(20, ros::Duration(1.0)); + + // Test copy constructor ------------------------------------------------- + Pid pid2(pid1); + + pid2.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + + EXPECT_EQ(p_gain, p_gain_return); + EXPECT_EQ(i_gain, i_gain_return); + EXPECT_EQ(d_gain, d_gain_return); + EXPECT_EQ(i_max, i_max_return); + EXPECT_EQ(i_min, i_min_return); + + // Test that errors are zero + double pe, ie, de; + pid2.getCurrentPIDErrors(&pe, &ie, &de); + EXPECT_EQ(0.0, pe); + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, de); + + // Test assignment constructor ------------------------------------------------- + Pid pid3; + pid3 = pid1; + + pid3.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + + EXPECT_EQ(p_gain, p_gain_return); + EXPECT_EQ(i_gain, i_gain_return); + EXPECT_EQ(d_gain, d_gain_return); + EXPECT_EQ(i_max, i_max_return); + EXPECT_EQ(i_min, i_min_return); + + // Test that errors are zero + double pe2, ie2, de2; + pid3.getCurrentPIDErrors(&pe2, &ie2, &de2); + EXPECT_EQ(0.0, pe2); + EXPECT_EQ(0.0, ie2); + EXPECT_EQ(0.0, de2); +} + + + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From c17668d49406feee5662d54cc0e608e6a7583a4f Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 22 Jul 2013 13:45:07 -0700 Subject: [PATCH 09/12] Made realtime_buffer copiable --- include/control_toolbox/pid.h | 2 +- src/pid.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index f44cf9d4..730ec644 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -358,7 +358,7 @@ class Pid return *this; // Copy the realtime buffer to then new PID class - setGains(source.getGainsConst()); + gains_buffer_ = source.gains_buffer_; // Reset the state of this PID controller reset(); diff --git a/src/pid.cpp b/src/pid.cpp index afaccc84..fcef2079 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -56,8 +56,9 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min) Pid::Pid(Pid &source) { // Copy the realtime buffer to then new PID class - setGains(source.getGainsConst()); - + //setGains(source.getGainsConst()); + gains_buffer_ = source.gains_buffer_; + // Reset the state of this PID controller reset(); } @@ -185,7 +186,7 @@ Pid::Gains Pid::getGains() Pid::Gains Pid::getGainsConst() const { - return *gains_buffer_.readFromRT(); + return *gains_buffer_.readFromRTConst(); } void Pid::setGains(double p, double i, double d, double i_max, double i_min) From d30b8166d495e97d5ff57a74a995f342a9e1690b Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 22 Jul 2013 15:50:28 -0700 Subject: [PATCH 10/12] Compatibility changes for realtime_tools, tweaked getests --- include/control_toolbox/pid.h | 2 +- src/pid.cpp | 7 +++---- test/pid_tests.cpp | 30 +++++++++++++----------------- 3 files changed, 17 insertions(+), 22 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 730ec644..3ad64c02 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -239,7 +239,7 @@ class Pid * \brief Get PID gains for the const controller, without modifying the realtime buffer * \return gains A struct of the PID gain values */ - Gains getGainsConst() const; + Gains getGains() const; /*! * \brief Set PID gains for the controller. diff --git a/src/pid.cpp b/src/pid.cpp index fcef2079..eedd4a78 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -56,7 +56,6 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min) Pid::Pid(Pid &source) { // Copy the realtime buffer to then new PID class - //setGains(source.getGainsConst()); gains_buffer_ = source.gains_buffer_; // Reset the state of this PID controller @@ -184,9 +183,9 @@ Pid::Gains Pid::getGains() return *gains_buffer_.readFromRT(); } -Pid::Gains Pid::getGainsConst() const +Pid::Gains Pid::getGains() const { - return *gains_buffer_.readFromRTConst(); + return *gains_buffer_.readFromNonRT(); } void Pid::setGains(double p, double i, double d, double i_max, double i_min) @@ -402,7 +401,7 @@ void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de) void Pid::printValues() const { - const Gains gains = getGainsConst(); + const Gains gains = getGains(); ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n" << " P Gain: " << gains.p_gain_ << "\n" diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index baba5a9d..7b639e7d 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -116,23 +116,6 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); - // Test return values using struct - const version ------------------------------------------------- - - // New values - p_gain = rand() % 100; - i_gain = rand() % 100; - d_gain = rand() % 100; - i_max = rand() % 100; - i_min = -1 * rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min); - - Pid::Gains g2 = pid1.getGainsConst(); - EXPECT_EQ(p_gain, g2.p_gain_); - EXPECT_EQ(i_gain, g2.i_gain_); - EXPECT_EQ(d_gain, g2.d_gain_); - EXPECT_EQ(i_max, g2.i_max_); - EXPECT_EQ(i_min, g2.i_min_); - // \todo test initParam() ------------------------------------------------- @@ -179,6 +162,19 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(0.0, pe2); EXPECT_EQ(0.0, ie2); EXPECT_EQ(0.0, de2); + + // Test return values using struct - const version ------------------------------------------------- + + const Pid pid4(pid3); + + Pid::Gains g3 = pid4.getGains(); + EXPECT_EQ(p_gain, g3.p_gain_); + EXPECT_EQ(i_gain, g3.i_gain_); + EXPECT_EQ(d_gain, g3.d_gain_); + EXPECT_EQ(i_max, g3.i_max_); + EXPECT_EQ(i_min, g3.i_min_); + + } From 76761cd9fe4bf6c474c3f15dc5fcca1c40b5f092 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Mon, 22 Jul 2013 19:17:54 -0700 Subject: [PATCH 11/12] Small fixes --- include/control_toolbox/pid.h | 2 +- src/pid.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 3ad64c02..fa03bf07 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -255,7 +255,7 @@ class Pid * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values */ - void setGains(Gains gains); + void setGains(const Gains &gains); /** * @brief Set Dynamic Reconfigure's gains to Pid's values diff --git a/src/pid.cpp b/src/pid.cpp index eedd4a78..043b5ddd 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -147,6 +147,7 @@ void Pid::initDynamicReconfig(ros::NodeHandle &node) // Start dynamic reconfigure server param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node)); + dynamic_reconfig_initialized_ = true; // Set Dynamic Reconfigure's gains to Pid's values updateDynamicReconfig(); @@ -154,8 +155,6 @@ void Pid::initDynamicReconfig(ros::NodeHandle &node) // Set callback param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2); param_reconfig_server_->setCallback(param_reconfig_callback_); - - dynamic_reconfig_initialized_ = true; } void Pid::reset() @@ -195,7 +194,7 @@ void Pid::setGains(double p, double i, double d, double i_max, double i_min) setGains(gains); } -void Pid::setGains(Gains gains) +void Pid::setGains(const Gains &gains) { gains_buffer_.writeFromNonRT(gains); @@ -228,8 +227,8 @@ void Pid::updateDynamicReconfig(Gains gains_config) // Convert to dynamic reconfigure format config.p_gain = gains_config.p_gain_; - config.i_gain = gains_config.p_gain_; - config.d_gain = gains_config.p_gain_; + config.i_gain = gains_config.i_gain_; + config.d_gain = gains_config.d_gain_; config.i_clamp_max = gains_config.i_max_; config.i_clamp_min = gains_config.i_min_; @@ -251,6 +250,7 @@ void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config) void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level) { ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); + // Set the gains setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min); } @@ -405,8 +405,8 @@ void Pid::printValues() const ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n" << " P Gain: " << gains.p_gain_ << "\n" - << " I Gain: " << gains.p_gain_ << "\n" - << " D Gain: " << gains.p_gain_ << "\n" + << " I Gain: " << gains.i_gain_ << "\n" + << " D Gain: " << gains.d_gain_ << "\n" << " I_Max: " << gains.i_max_ << "\n" << " I_Min: " << gains.i_min_ << "\n" << " P_Error_Last: " << p_error_last_ << "\n" From 17e41b50e6cc7c23a4ff4b41d0971d3bde994be6 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 26 Jul 2013 15:39:28 -0700 Subject: [PATCH 12/12] Removed const getGains function --- include/control_toolbox/pid.h | 8 +------- src/pid.cpp | 9 ++------- 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index fa03bf07..a1847a39 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -235,12 +235,6 @@ class Pid */ Gains getGains(); - /*! - * \brief Get PID gains for the const controller, without modifying the realtime buffer - * \return gains A struct of the PID gain values - */ - Gains getGains() const; - /*! * \brief Set PID gains for the controller. * \param p The proportional gain. @@ -346,7 +340,7 @@ class Pid /*! * \brief Print to console the current parameters */ - void printValues() const; + void printValues(); /*! * @brief Custom assignment operator diff --git a/src/pid.cpp b/src/pid.cpp index 043b5ddd..989266ce 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -182,11 +182,6 @@ Pid::Gains Pid::getGains() return *gains_buffer_.readFromRT(); } -Pid::Gains Pid::getGains() const -{ - return *gains_buffer_.readFromNonRT(); -} - void Pid::setGains(double p, double i, double d, double i_max, double i_min) { Gains gains(p,i,d,i_max,i_min); @@ -399,9 +394,9 @@ void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de) *de = d_error_; } -void Pid::printValues() const +void Pid::printValues() { - const Gains gains = getGains(); + Gains gains = getGains(); ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n" << " P Gain: " << gains.p_gain_ << "\n"