diff --git a/CMakeLists.txt b/CMakeLists.txt index cfebe4e9..f433f1a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ add_library(${PROJECT_NAME} SHARED src/dither.cpp src/limited_proxy.cpp src/pid.cpp + src/pidROS.cpp src/sine_sweep.cpp src/sinusoid.cpp) target_include_directories(${PROJECT_NAME} PUBLIC include) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 7159ed6a..6ca071ce 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -76,8 +76,6 @@ namespace control_toolbox
  • \f$ p_{error} = p_{state} - p_{target} \f$. - \section ROS ROS interface - \param p Proportional gain \param d Derivative gain @@ -86,8 +84,6 @@ namespace control_toolbox \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ - \param publish_state Enable publishing internal controller state on the `state` topic. May break real-time guarantees due to clock_gettime system call. - \section Usage To use the Pid class, you should first call some version of init() @@ -110,10 +106,6 @@ namespace control_toolbox */ /***************************************************/ -// alias for convenience -using NodeParamsIfacePtr = rclcpp::node_interfaces::NodeParametersInterface::SharedPtr; -using NodePtr = rclcpp::Node::SharedPtr; - class Pid { public: @@ -150,9 +142,9 @@ class Pid * 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. - * \param d The derivative gain. + * \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. */ @@ -175,39 +167,14 @@ class Pid * \brief Zeros out Pid values and initialize Pid-gains and integral term limits * Does not initialize the node's parameter interface for PID gains * - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. + * \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, bool antiwindup = false); - /*! - * \brief Zeros out Pid values and initialize Pid-gains and integral term limits - * Initializes the node's parameter interface 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, NodeParamsIfacePtr node_param_iface); - - void initPid( - double p, double i, double d, double i_max, double i_min, bool antiwindup, - NodeParamsIfacePtr node_param_iface); - - /*! - * \brief Initialize real-time pid state publisher - * - * \param node A pointer to the node. - */ - - void initPublisher(NodePtr node, std::string topic_prefix = ""); - /*! * \brief Reset the state of this PID controller */ @@ -215,9 +182,9 @@ class Pid /*! * \brief Get PID gains for the controller. - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. + * \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. */ @@ -233,9 +200,9 @@ class Pid /*! * \brief Set PID gains for the controller. - * \param p The proportional gain. - * \param i The integral gain. - * \param d The derivative gain. + * \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. */ @@ -253,11 +220,11 @@ class Pid * and the timestep \c dt. * * \param error Error since last call (error = target - state) - * \param dt Change in time since last call + * \param dt Change in time since last call in seconds * * \returns PID command */ - double computeCommand(double error, rclcpp::Duration dt); + double computeCommand(double error, double dt); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -266,11 +233,11 @@ class Pid * * \param error Error since last call (error = target - state) * \param error_dot d(Error)/dt since last call - * \param dt Change in time since last call + * \param dt Change in time since last call in seconds * * \returns PID command */ - double computeCommand(double error, double error_dot, rclcpp::Duration dt); + double computeCommand(double error, double error_dot, double dt); /*! * \brief Set current command for this PID controller @@ -288,12 +255,7 @@ class Pid * \param ie The integral error. * \param de The derivative error. */ - void getCurrentPIDErrors(double * pe, double * ie, double * de); - - /*! - * \brief Print to console the current parameters - */ - void printValues(const rclcpp::Logger & logger); + void getCurrentPIDErrors(double & pe, double & ie, double & de); /*! * @brief Custom assignment operator @@ -315,30 +277,15 @@ class Pid } private: - void setParameterEventCallback(); - -private: - using ClockPtr = rclcpp::Clock::SharedPtr; - using PidStateMsg = control_msgs::msg::PidState; - // 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_; - std::shared_ptr> state_pub_; - std::shared_ptr> rt_state_pub_; - ClockPtr clock_; - double p_error_last_; /**< _Save position state for derivative state calculation. */ double p_error_; /**< Position error. */ double i_error_; /**< Integral of position error. */ double d_error_; /**< Derivative of position error. */ double cmd_; /**< Command to send. */ - - NodeParamsIfacePtr node_param_iface_; - - using OnSetParamsCallbackPtr = rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr; - OnSetParamsCallbackPtr parameter_callback_; }; } // namespace control_toolbox diff --git a/include/control_toolbox/pidROS.hpp b/include/control_toolbox/pidROS.hpp new file mode 100644 index 00000000..db8a47e8 --- /dev/null +++ b/include/control_toolbox/pidROS.hpp @@ -0,0 +1,176 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 Willow Garage 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. + *********************************************************************/ +#ifndef CONTROL_TOOLBOX__PIDROS_HPP_ +#define CONTROL_TOOLBOX__PIDROS_HPP_ + +#include +#include + +#include "control_msgs/msg/pid_state.hpp" + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" + +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#include "control_toolbox/pid.hpp" + + +namespace control_toolbox +{ + +class PidROS +{ +public: + /*! + * \brief Destructor of PidROS class. + * + * The node is passed to this class to handler the ROS parameters, this class allows + * to add a prefix to the pid parameters + * + * \param node ROS node + * \param topic_prefix prefix to add to the pid parameters. + */ + explicit PidROS(rclcpp::Node::SharedPtr & node, std::string topic_prefix = std::string("")); + + /*! + * \brief Destructor of PidROS class. + */ + ~PidROS(); + + /*! + * \brief Initialize the PID controller and set the paramaters + * \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, bool antiwindup); + + /*! + * \brief Initialize the PID controller based on already setted parameters + * \return True is all parameters are set (p, i, d, i_min and i_max), False otherwise + */ + bool initPid(); + + /*! + * \brief Reset the state of this PID controller + */ + void reset(); + + /*! + * \brief Set the PID error and compute the PID command with nonuniform time + * step size. The derivative error is computed from the change in the error + * and the timestep \c dt. + * + * \param error Error since last call (error = target - state) + * \param dt Change in time since last call in seconds + * + * \returns PID command + */ + double computeCommand(double error, rclcpp::Duration dt); + + /*! + * \brief Get PID gains for the controller. + * \return gains A struct of the PID gain values + */ + Pid::Gains getGains(); + + /*! + * \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 setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + + /*! + * \brief Set PID gains for the controller. + * \param gains A struct of the PID gain values + */ + void setGains(const Pid::Gains & gains); + + /*! + * \brief Set current command for this PID controller + */ + void setCurrentCmd(double cmd); + + /*! + * \brief Return current command for this PID controller + */ + double getCurrentCmd(); + + /*! + * \brief Return PID error terms for the controller. + * \param pe[out] The proportional error. + * \param ie[out] The integral error. + * \param de[out] The derivative error. + */ + void getCurrentPIDErrors(double & pe, double & ie, double & de); + + /*! + * \brief Print to console the current parameters + */ + void printValues(); + +private: + void setParameterEventCallback(); + + void publishPIDState(double cmd, double error, rclcpp::Duration dt); + + void declare_param(const std::string & param_name, rclcpp::ParameterValue param_value); + + bool getDoubleParam(const std::string & param_name, double & value); + + bool getBooleanParam(const std::string & param_name, bool & value); + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; + + rclcpp::Node::SharedPtr node_; + + std::shared_ptr> rt_state_pub_; + std::shared_ptr> state_pub_; + + Pid pid_; + std::string topic_prefix_; +}; + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__PIDROS_HPP_ diff --git a/src/pid.cpp b/src/pid.cpp index 8e727bed..bb3a5c0a 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -61,8 +61,7 @@ T clamp(T val, T low, T high) } Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) -: gains_buffer_(), - node_param_iface_(nullptr), parameter_callback_(nullptr) +: gains_buffer_() { setGains(p, i, d, i_max, i_min, antiwindup); @@ -80,41 +79,6 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::initPid( - double p, double i, double d, double i_max, double i_min, NodeParamsIfacePtr node_param_iface) -{ - const Pid::Gains gains = getGains(); - initPid(p, i, d, i_max, i_min, gains.antiwindup_, node_param_iface); -} - -void Pid::initPid( - double p, double i, double d, double i_max, double i_min, bool antiwindup, - NodeParamsIfacePtr node_param_iface) -{ - initPid(p, i, d, i_max, i_min, antiwindup); - - node_param_iface_ = node_param_iface; - - // declare parameters if necessary - if (node_param_iface_) { - auto declare_param = [this]( - const std::string & param_name, rclcpp::ParameterValue param_value) { - if (!node_param_iface_->has_parameter(param_name)) { - node_param_iface_->declare_parameter(param_name, param_value); - } - }; - - declare_param("p", rclcpp::ParameterValue(p)); - declare_param("i", rclcpp::ParameterValue(i)); - declare_param("d", rclcpp::ParameterValue(d)); - declare_param("i_clamp_max", rclcpp::ParameterValue(i_max)); - declare_param("i_clamp_min", rclcpp::ParameterValue(i_min)); - declare_param("antiwindup", rclcpp::ParameterValue(antiwindup)); - } - - setParameterEventCallback(); -} - void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { setGains(p, i, d, i_max, i_min, antiwindup); @@ -122,18 +86,6 @@ void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool reset(); } -void Pid::initPublisher(NodePtr node, std::string topic_prefix) -{ - rclcpp::QoS qos(10); - qos.reliable().transient_local(); - - state_pub_ = node->create_publisher(topic_prefix + "/pid_state", qos); - rt_state_pub_.reset( - new realtime_tools::RealtimePublisher(state_pub_)); - - clock_ = node->get_clock(); -} - void Pid::reset() { p_error_last_ = 0.0; @@ -177,35 +129,26 @@ void Pid::setGains(double p, double i, double d, double i_max, double i_min, boo void Pid::setGains(const Gains & gains) { gains_buffer_.writeFromNonRT(gains); - - // update node parameters - if (node_param_iface_) { - node_param_iface_->set_parameters( - {rclcpp::Parameter("p", gains.p_gain_), rclcpp::Parameter("i", gains.i_gain_), - rclcpp::Parameter("d", gains.d_gain_), rclcpp::Parameter("i_clamp_max", gains.i_max_), - rclcpp::Parameter("i_clamp_min", gains.i_min_), - rclcpp::Parameter("antiwindup", gains.antiwindup_)}); - } } -double Pid::computeCommand(double error, rclcpp::Duration dt) +double Pid::computeCommand(double error, double dt) { - if (dt == rclcpp::Duration(0, 0) || std::isnan(error) || std::isinf(error)) { + if (dt == 0.0 || std::isnan(error) || std::isinf(error)) { return 0.0; } double error_dot = d_error_; // Calculate the derivative error - if (dt.seconds() > 0.0) { - error_dot = (error - p_error_last_) / dt.seconds(); + if (dt > 0.0) { + error_dot = (error - p_error_last_) / dt; p_error_last_ = error; } return computeCommand(error, error_dot, dt); } -double Pid::computeCommand(double error, double error_dot, rclcpp::Duration dt) +double Pid::computeCommand(double error, double error_dot, double dt) { // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); @@ -215,7 +158,7 @@ double Pid::computeCommand(double error, double error_dot, rclcpp::Duration dt) d_error_ = error_dot; if ( - dt == rclcpp::Duration(0, 0) || std::isnan(error) || std::isinf(error) || + dt == 0.0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot)) { return 0.0; @@ -225,7 +168,7 @@ double Pid::computeCommand(double error, double error_dot, rclcpp::Duration dt) p_term = gains.p_gain_ * p_error_; // Calculate the integral of the position error - i_error_ += dt.seconds() * p_error_; + i_error_ += dt * p_error_; if (gains.antiwindup_ && gains.i_gain_ != 0) { // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_ @@ -248,26 +191,6 @@ double Pid::computeCommand(double error, double error_dot, rclcpp::Duration dt) // Compute the command cmd_ = p_term + i_term + d_term; - // Publish controller state if configured - if (rt_state_pub_) { - if (rt_state_pub_->trylock()) { - rt_state_pub_->msg_.header.stamp = clock_->now(); - rt_state_pub_->msg_.timestep = dt; - rt_state_pub_->msg_.error = error; - rt_state_pub_->msg_.error_dot = error_dot; - rt_state_pub_->msg_.p_error = p_error_; - rt_state_pub_->msg_.i_error = i_error_; - rt_state_pub_->msg_.d_error = d_error_; - rt_state_pub_->msg_.p_term = p_term; - rt_state_pub_->msg_.i_term = i_term; - rt_state_pub_->msg_.d_term = d_term; - rt_state_pub_->msg_.i_max = gains.i_max_; - rt_state_pub_->msg_.i_min = gains.i_min_; - rt_state_pub_->msg_.output = cmd_; - rt_state_pub_->unlockAndPublish(); - } - } - return cmd_; } @@ -275,83 +198,13 @@ void Pid::setCurrentCmd(double cmd) {cmd_ = cmd;} double Pid::getCurrentCmd() {return cmd_;} -void Pid::getCurrentPIDErrors(double * pe, double * ie, double * de) +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_error_; - *de = d_error_; -} - -void Pid::printValues(const rclcpp::Logger & logger) -{ - Gains gains = getGains(); - - RCLCPP_INFO_STREAM( - logger, - "Current Values of PID Class:\n" << - " P 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" << - " Antiwindup: " << gains.antiwindup_ << "\n" << - " P_Error_Last: " << p_error_last_ << "\n" << - " P_Error: " << p_error_ << "\n" << - " I_Error: " << i_error_ << "\n" << - " D_Error: " << d_error_ << "\n" << - " Command: " << cmd_ - ); -} - -void Pid::setParameterEventCallback() -{ - auto on_parameter_event_callback = [this](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - /// @note don't use getGains, it's rt - Gains gains = *gains_buffer_.readFromNonRT(); - - for (auto & parameter : parameters) { - const std::string param_name = parameter.get_name(); - try { - if (param_name == "p") { - gains.p_gain_ = parameter.get_value(); - } else if (param_name == "i") { - gains.i_gain_ = parameter.get_value(); - } else if (param_name == "d") { - gains.d_gain_ = parameter.get_value(); - } else if (param_name == "i_clamp_max") { - gains.i_max_ = parameter.get_value(); - } else if (param_name == "i_clamp_min") { - gains.i_min_ = parameter.get_value(); - } else if (param_name == "antiwindup") { - gains.antiwindup_ = parameter.get_value(); - } else { - result.successful = false; - result.reason = "Invalid parameter"; - } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - RCLCPP_INFO_STREAM( - rclcpp::get_logger("control_toolbox::pid"), "Please use the right type: " << e.what()); - } - } - - if (result.successful) { - /// @note don't call setGains() from inside a callback - gains_buffer_.writeFromNonRT(gains); - } - - return result; - }; - - if (node_param_iface_) { - parameter_callback_ = - node_param_iface_->add_on_set_parameters_callback(on_parameter_event_callback); - } + pe = p_error_; + ie = i_error_; + de = d_error_; } - } // namespace control_toolbox diff --git a/src/pidROS.cpp b/src/pidROS.cpp new file mode 100644 index 00000000..e878c391 --- /dev/null +++ b/src/pidROS.cpp @@ -0,0 +1,299 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 Willow Garage 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: Melonee Wise + Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen + Desc: Implements a standard proportional-integral-derivative controller +*/ + +#include +#include +#include +#include +#include + +#include "control_toolbox/pidROS.hpp" + +namespace control_toolbox +{ + +PidROS::PidROS(rclcpp::Node::SharedPtr & node, std::string topic_prefix) +: node_(node) +{ + if (topic_prefix.back() != '.' && !topic_prefix.empty()) { + topic_prefix_ = topic_prefix + "."; + } else { + topic_prefix_ = topic_prefix; + } + + state_pub_ = node->create_publisher( + topic_prefix + "/pid_state", rclcpp::SensorDataQoS()); + rt_state_pub_.reset( + new realtime_tools::RealtimePublisher(state_pub_)); +} + +PidROS::~PidROS() +{ +} + +bool PidROS::getBooleanParam(const std::string & param_name, bool & value) +{ + rclcpp::Parameter param; + if (node_->has_parameter(param_name)) { + node_->get_parameter(param_name, param); + value = param.as_bool(); + return true; + } else { + return false; + } +} + +bool PidROS::getDoubleParam(const std::string & param_name, double & value) +{ + rclcpp::Parameter param; + if (node_->has_parameter(param_name)) { + node_->get_parameter(param_name, param); + value = param.as_double(); + RCLCPP_INFO_STREAM( + node_->get_logger(), "parameter '" << param_name << "' in node '" << node_->get_name() << + "' value is " << value << std::endl); + return true; + } else { + RCLCPP_INFO_STREAM( + node_->get_logger(), "parameter '" << param_name << "' in node '" << node_->get_name() << + "' does not exists" << std::endl); + return false; + } +} + +bool PidROS::initPid() +{ + double p, i, d, i_min, i_max; + bool antiwindup = false; + bool all_param_available = true; + all_param_available &= getDoubleParam(topic_prefix_ + "p", p); + all_param_available &= getDoubleParam(topic_prefix_ + "i", i); + all_param_available &= getDoubleParam(topic_prefix_ + "d", d); + all_param_available &= getDoubleParam(topic_prefix_ + "i_clamp_max", i_max); + all_param_available &= getDoubleParam(topic_prefix_ + "i_clamp_min", i_min); + + getBooleanParam(topic_prefix_ + "antiwindup", antiwindup); + + if (all_param_available) { + setParameterEventCallback(); + } + + pid_.initPid(p, i, d, i_max, i_min, antiwindup); + + return all_param_available; +} + +void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValue param_value) +{ + if (!node_->has_parameter(param_name)) { + node_->declare_parameter(param_name, param_value); + } +} + +void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + pid_.initPid(p, i, d, i_max, i_min, antiwindup); + + declare_param("p", rclcpp::ParameterValue(p)); + declare_param("i", rclcpp::ParameterValue(i)); + declare_param("d", rclcpp::ParameterValue(d)); + declare_param("i_clamp_max", rclcpp::ParameterValue(i_max)); + declare_param("i_clamp_min", rclcpp::ParameterValue(i_min)); + declare_param("antiwindup", rclcpp::ParameterValue(antiwindup)); + + setParameterEventCallback(); +} + +void PidROS::reset() +{ + pid_.reset(); +} + +double PidROS::computeCommand(double error, rclcpp::Duration dt) +{ + double cmd_ = pid_.computeCommand(error, dt.seconds()); + publishPIDState(cmd_, error, dt); + + return cmd_; +} + +Pid::Gains PidROS::getGains() +{ + return pid_.getGains(); +} + +void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + node_->set_parameters( + { + rclcpp::Parameter(topic_prefix_ + "p", p), + rclcpp::Parameter(topic_prefix_ + "i", i), + rclcpp::Parameter(topic_prefix_ + "d", d), + rclcpp::Parameter(topic_prefix_ + "i_clamp_max", i_max), + rclcpp::Parameter(topic_prefix_ + "i_clamp_min", i_min), + rclcpp::Parameter(topic_prefix_ + "antiwindup", antiwindup) + } + ); + + pid_.setGains(p, i, d, i_max, i_min, antiwindup); +} + +void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) +{ + Pid::Gains gains = pid_.getGains(); + + double p_error_, i_error_, d_error_; + getCurrentPIDErrors(p_error_, i_error_, d_error_); + + // Publish controller state if configured + if (rt_state_pub_) { + if (rt_state_pub_->trylock()) { + rt_state_pub_->msg_.header.stamp = rclcpp::Clock().now(); + rt_state_pub_->msg_.timestep = dt; + rt_state_pub_->msg_.error = error; + // rt_state_pub_->msg_.error_dot = error_dot; + rt_state_pub_->msg_.p_error = p_error_; + rt_state_pub_->msg_.i_error = i_error_; + rt_state_pub_->msg_.d_error = d_error_; + rt_state_pub_->msg_.p_term = gains.p_gain_; + rt_state_pub_->msg_.i_term = gains.i_gain_; + rt_state_pub_->msg_.d_term = gains.d_gain_; + rt_state_pub_->msg_.i_max = gains.i_max_; + rt_state_pub_->msg_.i_min = gains.i_min_; + rt_state_pub_->msg_.output = cmd; + rt_state_pub_->unlockAndPublish(); + } + } +} + +void PidROS::setCurrentCmd(double cmd) +{ + pid_.setCurrentCmd(cmd); +} + +double PidROS::getCurrentCmd() +{ + return pid_.getCurrentCmd(); +} + +void PidROS::getCurrentPIDErrors(double & pe, double & ie, double & de) +{ + double _pe, _ie, _de; + pid_.getCurrentPIDErrors(_pe, _ie, _de); + pe = _pe; + ie = _ie; + de = _de; +} + +void PidROS::printValues() +{ + Pid::Gains gains = pid_.getGains(); + + double p_error_, i_error_, d_error_; + getCurrentPIDErrors(p_error_, i_error_, d_error_); + + RCLCPP_INFO_STREAM( + node_->get_logger(), + "Current Values of PID Class:\n" << + " P 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" << + " Antiwindup: " << gains.antiwindup_ << "\n" << + " P_Error: " << p_error_ << "\n" << + " I_Error: " << i_error_ << "\n" << + " D_Error: " << d_error_ << "\n" << + " Command: " << getCurrentCmd(); + ); +} + +void PidROS::setGains(const Pid::Gains & gains) +{ + pid_.setGains(gains); +} + +void PidROS::setParameterEventCallback() +{ + auto on_parameter_event_callback = [this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + /// @note don't use getGains, it's rt + Pid::Gains gains = pid_.getGains(); + + for (auto & parameter : parameters) { + const std::string param_name = parameter.get_name(); + try { + if (param_name == "p") { + gains.p_gain_ = parameter.get_value(); + } else if (param_name == "i") { + gains.i_gain_ = parameter.get_value(); + } else if (param_name == "d") { + gains.d_gain_ = parameter.get_value(); + } else if (param_name == "i_clamp_max") { + gains.i_max_ = parameter.get_value(); + } else if (param_name == "i_clamp_min") { + gains.i_min_ = parameter.get_value(); + } else if (param_name == "antiwindup") { + gains.antiwindup_ = parameter.get_value(); + } else { + result.successful = false; + result.reason = "Invalid parameter"; + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_INFO_STREAM( + node_->get_logger(), "Please use the right type: " << e.what()); + } + } + + if (result.successful) { + /// @note don't call setGains() from inside a callback + pid_.setGains(gains); + } + + return result; + }; + + parameter_callback_ = + node_->add_on_set_parameters_callback(on_parameter_event_callback); +} + +} // namespace control_toolbox diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 2763f4a0..85584b77 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -11,24 +11,24 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. - #include -#include +#include + +#include "control_toolbox/pidROS.hpp" -#include -#include -#include -#include +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/utilities.hpp" -using control_toolbox::Pid; using rclcpp::executors::MultiThreadedExecutor; TEST(PidParametersTest, InitPidTest) { - rclcpp::Node node("pid_parameters_test"); + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - Pid pid; + control_toolbox::PidROS pid(node); const double P = 1.0; const double I = 2.0; @@ -36,31 +36,31 @@ TEST(PidParametersTest, InitPidTest) const double I_MAX = 10.0; const double I_MIN = -10.0; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, node.get_node_parameters_interface())); + ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, false)); rclcpp::Parameter param; // check paramters were set - ASSERT_TRUE(node.get_parameter("p", param)); + ASSERT_TRUE(node->get_parameter("p", param)); ASSERT_EQ(param.get_value(), P); - ASSERT_TRUE(node.get_parameter("i", param)); + ASSERT_TRUE(node->get_parameter("i", param)); ASSERT_EQ(param.get_value(), I); - ASSERT_TRUE(node.get_parameter("d", param)); + ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); - ASSERT_TRUE(node.get_parameter("i_clamp_max", param)); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); - ASSERT_TRUE(node.get_parameter("i_clamp_min", param)); + ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); - ASSERT_TRUE(node.get_parameter("antiwindup", param)); + ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_FALSE(param.get_value()); // check gains were set - Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.getGains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -71,9 +71,9 @@ TEST(PidParametersTest, InitPidTest) TEST(PidParametersTest, InitPidWithAntiwindupTest) { - rclcpp::Node node("pid_parameters_test"); + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - Pid pid; + control_toolbox::PidROS pid(node); const double P = 1.0; const double I = 2.0; @@ -82,31 +82,30 @@ TEST(PidParametersTest, InitPidWithAntiwindupTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - ASSERT_NO_THROW( - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, node.get_node_parameters_interface())); + pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rclcpp::Parameter param; - ASSERT_TRUE(node.get_parameter("p", param)); + ASSERT_TRUE(node->get_parameter("p", param)); ASSERT_EQ(param.get_value(), P); - ASSERT_TRUE(node.get_parameter("i", param)); + ASSERT_TRUE(node->get_parameter("i", param)); ASSERT_EQ(param.get_value(), I); - ASSERT_TRUE(node.get_parameter("d", param)); + ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); - ASSERT_TRUE(node.get_parameter("i_clamp_max", param)); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); - ASSERT_TRUE(node.get_parameter("i_clamp_min", param)); + ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); - ASSERT_TRUE(node.get_parameter("antiwindup", param)); + ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); // check gains were set - Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.getGains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -117,9 +116,9 @@ TEST(PidParametersTest, InitPidWithAntiwindupTest) TEST(PidParametersTest, SetParametersTest) { - rclcpp::Node node("pid_parameters_test"); + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - Pid pid; + control_toolbox::PidROS pid(node); const double P = 1.0; const double I = 2.0; @@ -128,34 +127,33 @@ TEST(PidParametersTest, SetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - ASSERT_NO_THROW( - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, node.get_node_parameters_interface())); + pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; // unknown parameter name ASSERT_THROW( - set_result = node.set_parameter(rclcpp::Parameter("unknown", 0.0)), + set_result = node->set_parameter(rclcpp::Parameter("unknown", 0.0)), rclcpp::exceptions::ParameterNotDeclaredException); - ASSERT_NO_THROW(set_result = node.set_parameter(rclcpp::Parameter("p", P))); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("p", P))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node.set_parameter(rclcpp::Parameter("i", I))); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i", I))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node.set_parameter(rclcpp::Parameter("d", D))); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("d", D))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node.set_parameter(rclcpp::Parameter("i_clamp_max", I_MAX))); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_max", I_MAX))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node.set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); ASSERT_TRUE(set_result.successful); - ASSERT_NO_THROW(set_result = node.set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); + ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP))); ASSERT_TRUE(set_result.successful); // process callbacks - rclcpp::spin_some(node.get_node_base_interface()); + rclcpp::spin_some(node->get_node_base_interface()); // check gains were set using the parameters - Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.getGains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -166,9 +164,9 @@ TEST(PidParametersTest, SetParametersTest) TEST(PidParametersTest, GetParametersTest) { - rclcpp::Node node("pid_parameters_test"); + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - Pid pid; + control_toolbox::PidROS pid(node); const double P = 1.0; const double I = 2.0; @@ -177,31 +175,59 @@ TEST(PidParametersTest, GetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - ASSERT_NO_THROW( - pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false, node.get_node_parameters_interface())); - ASSERT_NO_THROW(pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); + pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false); + pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rclcpp::Parameter param; - ASSERT_TRUE(node.get_parameter("p", param)); + ASSERT_TRUE(node->get_parameter("p", param)); ASSERT_EQ(param.get_value(), P); - ASSERT_TRUE(node.get_parameter("i", param)); + ASSERT_TRUE(node->get_parameter("i", param)); ASSERT_EQ(param.get_value(), I); - ASSERT_TRUE(node.get_parameter("d", param)); + ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); - ASSERT_TRUE(node.get_parameter("i_clamp_max", param)); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); - ASSERT_TRUE(node.get_parameter("i_clamp_min", param)); + ASSERT_TRUE(node->get_parameter("i_clamp_min", param)); ASSERT_EQ(param.get_value(), I_MIN); - ASSERT_TRUE(node.get_parameter("antiwindup", param)); + ASSERT_TRUE(node->get_parameter("antiwindup", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP); } +TEST(PidParametersTest, GetParametersFromParams) +{ + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); + + control_toolbox::PidROS pid(node); + + const double P = 1.0; + const double I = 2.0; + const double D = 3.0; + const double I_MAX = 10.0; + const double I_MIN = -10.0; + const bool ANTIWINDUP = true; + + ASSERT_FALSE(pid.initPid()); + + node->declare_parameter("p", P); + node->declare_parameter("i", I); + node->declare_parameter("d", D); + node->declare_parameter("i_clamp_max", I_MAX); + node->declare_parameter("i_clamp_min", I_MIN); + + ASSERT_TRUE(pid.initPid()); + + rclcpp::Parameter param; + + ASSERT_TRUE(node->get_parameter("p", param)); + ASSERT_EQ(param.get_value(), P); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp index 24142aff..17fee3d1 100644 --- a/test/pid_publisher_tests.cpp +++ b/test/pid_publisher_tests.cpp @@ -18,7 +18,7 @@ #include #include -#include "control_toolbox/pid.hpp" +#include "control_toolbox/pidROS.hpp" #include "gtest/gtest.h" @@ -27,35 +27,36 @@ #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" -using control_toolbox::Pid; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; TEST(PidPublihserTest, PublishTest) { - const size_t ATTEMPTS = 10; + const size_t ATTEMPTS = 100; const std::chrono::milliseconds DELAY(250); auto node = std::make_shared("pid_publisher_test"); - Pid pid(1.0, 1.0, 1.0, 5.0, -5.0); + control_toolbox::PidROS pid_ros(node); - ASSERT_NO_THROW(pid.initPublisher(node)); + pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; - PidStateMsg::SharedPtr last_state_msg; - auto state_callback = [&](const PidStateMsg::SharedPtr) + control_msgs::msg::PidState::SharedPtr last_state_msg; + auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) { callback_called = true; }; - auto state_sub = node->create_subscription("pid_state", 10, state_callback); + auto state_sub = node->create_subscription( + "/pid_state", rclcpp::SensorDataQoS(), state_callback); - double command = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); EXPECT_EQ(-1.5, command); // wait for callback for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { + double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); rclcpp::spin_some(node); std::this_thread::sleep_for(DELAY); } diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 62381221..0114d222 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -53,13 +53,13 @@ TEST(ParameterTest, ITermBadIBoundsTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); - pid.getCurrentPIDErrors(&pe, &ie, &de); + cmd = pid.computeCommand(-1.0, 1.0); + pid.getCurrentPIDErrors(pe, ie, de); EXPECT_FALSE(std::isinf(ie)); EXPECT_FALSE(std::isnan(cmd)); - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); - pid.getCurrentPIDErrors(&pe, &ie, &de); + cmd = pid.computeCommand(-1.0, 1.0); + pid.getCurrentPIDErrors(pe, ie, de); EXPECT_FALSE(std::isinf(ie)); EXPECT_FALSE(std::isnan(cmd)); } @@ -76,11 +76,11 @@ TEST(ParameterTest, integrationClampTest) double cmd = 0.0; // Test lower limit - cmd = pid.computeCommand(-10.03, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-10.03, 1.0); EXPECT_EQ(-1.0, cmd); // Test upper limit - cmd = pid.computeCommand(30.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(30.0, 1.0); EXPECT_EQ(1.0, cmd); } @@ -100,13 +100,13 @@ TEST(ParameterTest, integrationClampZeroGainTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); - pid.getCurrentPIDErrors(&pe, &ie, &de); + cmd = pid.computeCommand(-1.0, 1.0); + pid.getCurrentPIDErrors(pe, ie, de); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); @@ -126,16 +126,16 @@ TEST(ParameterTest, integrationAntiwindupTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.5, 1.0); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); } @@ -153,16 +153,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(0.1, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(0.1, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(-0.05, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.05, 1.0); EXPECT_EQ(-0.075, cmd); - cmd = pid.computeCommand(0.1, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.1, 1.0); EXPECT_EQ(-0.2, cmd); } @@ -221,7 +221,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Send update command to populate errors ------------------------------------------------- pid1.setCurrentCmd(10); - pid1.computeCommand(20, rclcpp::Duration(1, 0)); + pid1.computeCommand(20, 1.0); // Test copy constructor ------------------------------------------------- Pid pid2(pid1); @@ -238,7 +238,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test that errors are zero double pe2, ie2, de2; - pid2.getCurrentPIDErrors(&pe2, &ie2, &de2); + pid2.getCurrentPIDErrors(pe2, ie2, de2); EXPECT_EQ(0.0, pe2); EXPECT_EQ(0.0, ie2); EXPECT_EQ(0.0, de2); @@ -259,7 +259,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test that errors are zero double pe3, ie3, de3; - pid3.getCurrentPIDErrors(&pe3, &ie3, &de3); + pid3.getCurrentPIDErrors(pe3, ie3, de3); EXPECT_EQ(0.0, pe3); EXPECT_EQ(0.0, ie3); EXPECT_EQ(0.0, de3); @@ -268,7 +268,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid1.reset(); double pe1, ie1, de1; - pid1.getCurrentPIDErrors(&pe1, &ie1, &de1); + pid1.getCurrentPIDErrors(pe1, ie1, de1); EXPECT_EQ(0.0, pe1); EXPECT_EQ(0.0, ie1); EXPECT_EQ(0.0, de1); @@ -289,22 +289,22 @@ TEST(CommandTest, proportionalOnlyTest) double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect the same as before EXPECT_EQ(-0.5, cmd); // If call again doubling the error - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); // Then expect the command doubled EXPECT_EQ(-1.0, cmd); // If call with positive error - cmd = pid.computeCommand(0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.5, 1.0); // Then expect always command = error EXPECT_EQ(0.5, cmd); } @@ -321,26 +321,26 @@ TEST(CommandTest, integralOnlyTest) double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same arguments - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect the integral part to double the command EXPECT_EQ(-1.0, cmd); // Call again with no error - cmd = pid.computeCommand(0.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.0, 1.0); // Expect the integral part to keep the previous command because it ensures error = 0 EXPECT_EQ(-1.0, cmd); // Double check that the integral contribution keep the previous command - cmd = pid.computeCommand(0.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(0.0, 1.0); EXPECT_EQ(-1.0, cmd); // Finally call again with positive error to see if the command changes in the opposite direction - cmd = pid.computeCommand(1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(1.0, 1.0); // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1 EXPECT_EQ(0.0, cmd); } @@ -357,27 +357,27 @@ TEST(CommandTest, derivativeOnlyTest) double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same error - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = 0 due to no variation on error EXPECT_EQ(0.0, cmd); // If call again with same error and smaller control period - cmd = pid.computeCommand(-0.5, rclcpp::Duration(100 * 10 ^ 6)); + cmd = pid.computeCommand(-0.5, 0.1); // Then expect command = 0 again EXPECT_EQ(0.0, cmd); // If the error increases, with dt = 1 - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); // Then expect the command = change in dt EXPECT_EQ(-0.5, cmd); // If error decreases, with dt = 1 - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } @@ -394,17 +394,17 @@ TEST(CommandTest, completePIDTest) // All contributions are tested, here few tests check that they sum up correctly // If initial error = 0, all gains = 1, dt = 1 - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = 3x error EXPECT_EQ(-1.5, cmd); // If call again with same arguments, no error change, but integration do its part - cmd = pid.computeCommand(-0.5, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = 3x error again EXPECT_EQ(-1.5, cmd); // If call again increasing the error - cmd = pid.computeCommand(-1.0, rclcpp::Duration(1, 0)); + cmd = pid.computeCommand(-1.0, 1.0); // Then expect command equals to p = -1, i = -2.0 (i.e. - 0.5 - 0.5 - 1.0), d = -0.5 EXPECT_EQ(-3.5, cmd); }