From d24c7b6d758e05b9c3d046de4ad1eadb578b0359 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Dec 2024 17:41:24 +0000 Subject: [PATCH 01/21] Add computeCommand with double dt --- include/control_toolbox/pid.hpp | 29 ++++++++++++++-- src/pid.cpp | 34 +++++++++++++++---- test/pid_tests.cpp | 60 ++++++++++++++++----------------- 3 files changed, 84 insertions(+), 39 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 8f3272a7..b00321ca 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -251,6 +251,18 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ void setGains(const Gains & gains); + /*! + * \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 + */ + [[nodiscard]] double computeCommand(double error, double dt_s); + /*! * \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 @@ -261,7 +273,20 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, uint64_t dt); + [[nodiscard]] double computeCommand(double error, uint64_t dt_ns); + + /*! + * \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. + * + * \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 in seconds + * + * \returns PID command + */ + [[nodiscard]] double computeCommand(double error, double error_dot, double dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -274,7 +299,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt); + [[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt_ns); /*! * \brief Set current command for this PID controller diff --git a/src/pid.cpp b/src/pid.cpp index c1f23a12..10d94abc 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -121,22 +121,42 @@ void Pid::setGains(const Gains & gains) } } -double Pid::computeCommand(double error, uint64_t dt) +double Pid::computeCommand(double error, uint64_t dt_ns) { - if (dt == 0 || std::isnan(error) || std::isinf(error)) { + if (dt_ns == 0 || std::isnan(error) || std::isinf(error)) { return 0.0; } error_dot_ = d_error_; // Calculate the derivative error - error_dot_ = (error - p_error_last_) / (static_cast(dt) / 1e9); + error_dot_ = (error - p_error_last_) / (static_cast(dt_ns) / 1e9); p_error_last_ = error; - return computeCommand(error, error_dot_, dt); + return computeCommand(error, error_dot_, dt_ns); } -double Pid::computeCommand(double error, double error_dot, uint64_t dt) +double Pid::computeCommand(double error, double dt_s) +{ + if (dt_s <= 0.0 || std::isnan(error) || std::isinf(error)) { + return 0.0; + } + + error_dot_ = d_error_; + + // Calculate the derivative error + error_dot_ = (error - p_error_last_) / dt_s; + p_error_last_ = error; + + return computeCommand(error, error_dot_, dt_s); +} + +double Pid::computeCommand(double error, double error_dot, uint64_t dt_ns) +{ + return computeCommand(error, error_dot, static_cast(dt_ns) / 1.e9); +} + +double Pid::computeCommand(double error, double error_dot, double dt_s) { // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); @@ -146,7 +166,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt) d_error_ = error_dot; if ( - dt == 0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || + dt_s <= 0.0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot)) { return 0.0; } @@ -155,7 +175,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt) p_term = gains.p_gain_ * p_error_; // Calculate the integral of the position error - i_error_ += (static_cast(dt) / 1e9) * p_error_; + i_error_ += dt_s * p_error_; if (gains.antiwindup_ && gains.i_gain_ != 0) { // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_ diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 31498fda..f8b71465 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -80,11 +80,11 @@ TEST(ParameterTest, integrationClampTest) double cmd = 0.0; // Test lower limit - cmd = pid.computeCommand(-10.03, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-10.03, 1.0); EXPECT_EQ(-1.0, cmd); // Test upper limit - cmd = pid.computeCommand(30.0, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(30.0, 1.0); EXPECT_EQ(1.0, cmd); } @@ -104,13 +104,13 @@ TEST(ParameterTest, integrationClampZeroGainTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); @@ -129,16 +129,16 @@ TEST(ParameterTest, integrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(0.5, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(0.5, 1.0); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); } @@ -155,16 +155,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(0.1, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(0.1, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(-0.05, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-0.05, 1.0); EXPECT_EQ(-0.075, cmd); - cmd = pid.computeCommand(0.1, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(0.1, 1.0); EXPECT_EQ(-0.2, cmd); } @@ -223,7 +223,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Send update command to populate errors ------------------------------------------------- pid1.setCurrentCmd(10); - (void) pid1.computeCommand(20, static_cast(1.0 * 1e9)); + (void) pid1.computeCommand(20, 1.0); // Test copy constructor ------------------------------------------------- Pid pid2(pid1); @@ -291,22 +291,22 @@ TEST(CommandTest, proportionalOnlyTest) double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again - cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(0.5, 1.0); // Then expect always command = error EXPECT_EQ(0.5, cmd); } @@ -323,26 +323,26 @@ TEST(CommandTest, integralOnlyTest) double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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); } @@ -359,27 +359,27 @@ TEST(CommandTest, derivativeOnlyTest) double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(0.1 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + cmd = pid.computeCommand(-0.5, 1.0); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } @@ -396,17 +396,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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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, static_cast(1.0 * 1e9)); + 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); } From d351720f5d38e3929cc7a1d9dec441e246e889a5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Dec 2024 17:41:24 +0000 Subject: [PATCH 02/21] Calc dt only once --- src/pid.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pid.cpp b/src/pid.cpp index 10d94abc..f6b78248 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -130,10 +130,11 @@ double Pid::computeCommand(double error, uint64_t dt_ns) error_dot_ = d_error_; // Calculate the derivative error - error_dot_ = (error - p_error_last_) / (static_cast(dt_ns) / 1e9); + double dt_s = (static_cast(dt_ns) / 1e9); + error_dot_ = (error - p_error_last_) / dt_s; p_error_last_ = error; - return computeCommand(error, error_dot_, dt_ns); + return computeCommand(error, error_dot_, dt_s); } double Pid::computeCommand(double error, double dt_s) From 59b65c65707d261da709d2472f121f7ca9e55469 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Dec 2024 17:41:24 +0000 Subject: [PATCH 03/21] Add specializations for duration types --- include/control_toolbox/pid.hpp | 59 +++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index b00321ca..6cfcd0f3 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -38,6 +38,7 @@ #include #include +#include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.h" #include "control_toolbox/visibility_control.hpp" @@ -275,6 +276,34 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ [[nodiscard]] double computeCommand(double error, uint64_t dt_ns); + /*! + * \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 + * + * \returns PID command + */ + [[nodiscard]] double computeCommand(double error, rcl_duration_value_t dt) { + return computeCommand(error, static_cast(dt)); + } + + /*! + * \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 + * + * \returns PID command + */ + [[nodiscard]] double computeCommand(double error, std::chrono::nanoseconds dt_ns) { + return computeCommand(error, static_cast(dt_ns.count())); + } + /*! * \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 @@ -301,6 +330,36 @@ class CONTROL_TOOLBOX_PUBLIC Pid */ [[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt_ns); + /*! + * \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. + * + * \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 + * + * \returns PID command + */ + [[nodiscard]] double computeCommand(double error, double error_dot, rcl_duration_value_t dt) { + return computeCommand(error, error_dot, static_cast(dt)); + } + + /*! + * \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. + * + * \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 + * + * \returns PID command + */ + [[nodiscard]] double computeCommand(double error, double error_dot, std::chrono::nanoseconds dt_ns) { + return computeCommand(error, error_dot, static_cast(dt_ns.count())); + } + /*! * \brief Set current command for this PID controller */ From d883ab36d6784e87f4cdeff401c606df1a787d05 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Dec 2024 17:41:24 +0000 Subject: [PATCH 04/21] Format code --- include/control_toolbox/pid.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 6cfcd0f3..251767f7 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -356,7 +356,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, std::chrono::nanoseconds dt_ns) { + [[nodiscard]] double computeCommand( + double error, double error_dot, std::chrono::nanoseconds dt_ns) { return computeCommand(error, error_dot, static_cast(dt_ns.count())); } From a7773d50790002ced7ccefdec6e0018b4ca94438 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 13 Dec 2024 14:49:12 +0000 Subject: [PATCH 05/21] Deprecate camelCase methods and add new overloads --- include/control_toolbox/pid.hpp | 194 ++++++++++++++++++++++++---- include/control_toolbox/pid_ros.hpp | 175 ++++++++++++++++++++++--- src/pid.cpp | 55 +++----- src/pid_ros.cpp | 102 +++++++-------- test/pid_parameters_tests.cpp | 26 ++-- test/pid_publisher_tests.cpp | 14 +- test/pid_tests.cpp | 86 ++++++------ 7 files changed, 458 insertions(+), 194 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 251767f7..43cdb825 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -89,13 +89,13 @@ namespace control_toolbox \verbatim control_toolbox::Pid pid; - pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3); + pid.init_pid(6.0, 1.0, 2.0, 0.3, -0.3); double position_desi_ = 0.5; ... rclcpp::Time last_time = get_clock()->now(); while (true) { rclcpp::Time time = get_clock()->now(); - double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time).nanoseconds()); + double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time)); last_time = time; } \endverbatim @@ -197,7 +197,25 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void init_pid(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 + * 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 i_max The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + * + * \note New gains are not applied if i_min_ > i_max_ + */ + [[deprecated("Use init_pid() instead")]] void initPid( + double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { + init_pid(p, i, d, i_max, i_min, antiwindup); + } /*! * \brief Reset the state of this PID controller @@ -212,7 +230,21 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \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 get_gains(double & p, double & i, double & d, double & i_max, double & i_min); + + /*! + * \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. + */ + [[deprecated("Use get_gains() instead")]] void getGains( + double & p, double & i, double & d, double & i_max, double & i_min) { + get_gains(p, i, d, i_max, i_min); + } + /*! * \brief Get PID gains for the controller. * \param p The proportional gain. @@ -222,14 +254,49 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param i_min The min integral windup. * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced */ - void getGains( + void get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); + /*! + * \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. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + */ + [[deprecated("Use get_gains() instead")]] void getGains( + double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) { + get_gains(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Get PID gains for the controller. + * \return gains A struct of the PID gain values + */ + Gains get_gains(); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values */ - Gains getGains(); + [[deprecated("Use get_gains() instead")]] Gains getGains() { + return get_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. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + * + * \note New gains are not applied if i_min > i_max + */ + void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. @@ -242,7 +309,18 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + [[deprecated("Use set_gains() instead")]] void setGains( + double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { + set_gains(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Set PID gains for the controller. + * \param gains A struct of the PID gain values + * + * \note New gains are not applied if gains.i_min_ > gains.i_max_ + */ + void set_gains(const Gains & gains); /*! * \brief Set PID gains for the controller. @@ -250,7 +328,9 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void setGains(const Gains & gains); + [[deprecated("Use set_gains() instead")]] void setGains(const Gains & gains) { + set_gains(gains); + } /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -262,7 +342,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double dt_s); + [[nodiscard]] double compute_command(double error, double dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -274,7 +354,24 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, uint64_t dt_ns); + [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand( + double error, uint64_t dt_ns) { + return compute_command(error, static_cast(dt_ns) / 1.e9); + } + + /*! + * \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, measured in nanoseconds. + * + * \returns PID command + */ + [[nodiscard]] double compute_command(double error, rcl_duration_value_t dt_ns) { + return compute_command(error, static_cast(dt_ns)/1.e9); + } /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -286,8 +383,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, rcl_duration_value_t dt) { - return computeCommand(error, static_cast(dt)); + [[nodiscard]] double compute_command(double error, rclcpp::Duration dt) { + return compute_command(error, dt.seconds()); } /*! @@ -301,7 +398,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \returns PID command */ [[nodiscard]] double computeCommand(double error, std::chrono::nanoseconds dt_ns) { - return computeCommand(error, static_cast(dt_ns.count())); + return compute_command(error, static_cast(dt_ns.count())/1.e9); } /*! @@ -315,7 +412,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, double dt_s); + [[nodiscard]] double compute_command(double error, double error_dot, double dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -328,7 +425,10 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt_ns); + [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand( + double error, double error_dot, uint64_t dt_ns) { + return compute_command(error, error_dot, static_cast(dt_ns) / 1.e9); + } /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -337,12 +437,12 @@ class CONTROL_TOOLBOX_PUBLIC 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, measured in nanoseconds. * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, rcl_duration_value_t dt) { - return computeCommand(error, error_dot, static_cast(dt)); + [[nodiscard]] double compute_command(double error, double error_dot, rcl_duration_value_t dt_ns) { + return compute_command(error, error_dot, static_cast(dt_ns)/1.e9); } /*! @@ -356,25 +456,68 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand( + [[nodiscard]] double compute_command(double error, double error_dot, rclcpp::Duration dt) { + return compute_command(error, error_dot, dt.seconds()); + } + + /*! + * \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. + * + * \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, measured in nanoseconds. + * + * \returns PID command + */ + [[nodiscard]] double compute_command( double error, double error_dot, std::chrono::nanoseconds dt_ns) { - return computeCommand(error, error_dot, static_cast(dt_ns.count())); + return compute_command(error, error_dot, static_cast(dt_ns.count())/1.e9); } /*! * \brief Set current command for this PID controller */ - void setCurrentCmd(double cmd); + void set_current_cmd(double cmd); + + /*! + * \brief Set current command for this PID controller + */ + [[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd) { + set_current_cmd(cmd); + } + + /*! + * \brief Return current command for this PID controller + */ + double get_current_cmd(); /*! * \brief Return current command for this PID controller */ - double getCurrentCmd(); + [[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd() { + return get_current_cmd(); + } /*! * \brief Return derivative error */ - double getDerivativeError(); + double get_derivative_error(); + + /*! + * \brief Return derivative error + */ + [[deprecated("Use get_derivative_error() instead")]] + double getDerivativeError() { return get_derivative_error(); } + + /*! + * \brief Return PID error terms for the controller. + * \param pe The proportional error. + * \param ie The integral error. + * \param de The derivative error. + */ + void get_current_pid_errors(double & pe, double & ie, double & de); /*! * \brief Return PID error terms for the controller. @@ -382,7 +525,10 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param ie The integral error. * \param de The derivative error. */ - void getCurrentPIDErrors(double & pe, double & ie, double & de); + [[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors( + double & pe, double & ie, double & de) { + get_current_pid_errors(pe, ie, de); + } /*! * @brief Custom assignment operator diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index cf55eb3c..e58ad470 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -101,13 +101,37 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + void init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + + /*! + * \brief Initialize the PID controller and set the parameters + * \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. + * \param antiwindup antiwindup. + * + * \note New gains are not applied if i_min_ > i_max_ + */ + [[deprecated("Use init_pid() instead")]] void initPid( + double p, double i, double d, double i_max, double i_min, bool antiwindup) { + init_pid(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Initialize the PID controller based on already set parameters + * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise + */ + bool init_pid(); /*! * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ - bool initPid(); + [[deprecated("Use init_pid() instead")]] bool initPid() { + return init_pid(); + } /*! * \brief Reset the state of this PID controller @@ -124,7 +148,22 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \returns PID command */ - double computeCommand(double error, rclcpp::Duration dt); + double compute_command(double error, rclcpp::Duration dt); + + /*! + * \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 + */ + [[deprecated("Use compute_command() instead")]] double computeCommand( + double error, rclcpp::Duration dt) { + return compute_command(error, dt); + } /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -137,13 +176,48 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \returns PID command */ - double computeCommand(double error, double error_dot, rclcpp::Duration dt); + double compute_command(double error, double error_dot, rclcpp::Duration dt); + + /*! + * \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. + * + * \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 in seconds + * + * \returns PID command + */ + [[deprecated("Use compute_command() instead")]] double computeCommand( + double error, double error_dot, rclcpp::Duration dt) { + return compute_command(error, error_dot, dt); + } + + /*! + * \brief Get PID gains for the controller. + * \return gains A struct of the PID gain values + */ + Pid::Gains get_gains(); /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values */ - Pid::Gains getGains(); + [[deprecated("Use get_gains() instead")]] Pid::Gains getGains() { return get_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. + * \param antiwindup antiwindup. + * + * \note New gains are not applied if i_min > i_max + */ + void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. @@ -156,7 +230,18 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + [[deprecated("Use set_gains() instead")]] void setGains( + double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { + set_gains(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Set PID gains for the controller. + * \param gains A struct of the PID gain values + * + * \note New gains are not applied if gains.i_min_ > gains.i_max_ + */ + void set_gains(const Pid::Gains & gains); /*! * \brief Set PID gains for the controller. @@ -164,25 +249,60 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void setGains(const Pid::Gains & gains); + [[deprecated("Use set_gains() instead")]] void setGains(const Pid::Gains & gains) { + set_gains(gains); + } /*! * \brief Set current command for this PID controller * \param cmd command to set */ - void setCurrentCmd(double cmd); + void set_current_cmd(double cmd); + + /*! + * \brief Set current command for this PID controller + * \param cmd command to set + */ + [[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd) { + set_current_cmd(cmd); + } + + /*! + * \brief Return current command for this PID controller + * \return current cmd + */ + double get_current_cmd(); /*! * \brief Return current command for this PID controller * \return current cmd */ - double getCurrentCmd(); + [[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd() { + return get_current_cmd(); + } /*! * \brief Return PID state publisher * \return shared_ptr to the PID state publisher */ - std::shared_ptr> getPidStatePublisher(); + std::shared_ptr> get_pid_state_publisher(); + + /*! + * \brief Return PID state publisher + * \return shared_ptr to the PID state publisher + */ + [[deprecated("Use get_pid_state_publisher() instead")]] + std::shared_ptr> getPidStatePublisher() { + return get_pid_state_publisher(); + } + + /*! + * \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 get_current_pid_errors(double & pe, double & ie, double & de); /*! * \brief Return PID error terms for the controller. @@ -190,37 +310,56 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \param ie[out] The integral error. * \param de[out] The derivative error. */ - void getCurrentPIDErrors(double & pe, double & ie, double & de); + [[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors( + double & pe, double & ie, double & de) { + get_current_pid_errors(pe, ie, de); + } /*! * \brief Print to console the current parameters */ - void printValues(); + void print_values(); + + /*! + * \brief Print to console the current parameters + */ + [[deprecated("Use print_values() instead")]] void printValues() { print_values(); } /*! * \brief Return PID parameters callback handle * \return shared_ptr to the PID parameters callback handle */ inline rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr - getParametersCallbackHandle() + get_parameters_callback_handle() { return parameter_callback_; } + /*! + * \brief Return PID parameters callback handle + * \return shared_ptr to the PID parameters callback handle + */ + [[deprecated("Use get_parameters_callback_handle() instead")]] + inline rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + getParametersCallbackHandle() + { + return get_parameters_callback_handle(); + } + protected: std::string topic_prefix_; std::string param_prefix_; private: - void setParameterEventCallback(); + void set_parameter_event_callback(); - void publishPIDState(double cmd, double error, rclcpp::Duration dt); + void publish_pid_state(double cmd, double error, rclcpp::Duration dt); - void declareParam(const std::string & param_name, rclcpp::ParameterValue param_value); + void declare_param(const std::string & param_name, rclcpp::ParameterValue param_value); - bool getDoubleParam(const std::string & param_name, double & value); + bool get_double_param(const std::string & param_name, double & value); - bool getBooleanParam(const std::string & param_name, bool & value); + bool get_boolean_param(const std::string & param_name, bool & value); /*! * \param topic_prefix prefix to add to the pid parameters. diff --git a/src/pid.cpp b/src/pid.cpp index f6b78248..fb1ee9b4 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -52,7 +52,7 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } - setGains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, antiwindup); reset(); } @@ -68,9 +68,9 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - setGains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, antiwindup); reset(); } @@ -84,13 +84,13 @@ void Pid::reset() cmd_ = 0.0; } -void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min) +void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) { bool antiwindup; - getGains(p, i, d, i_max, i_min, antiwindup); + get_gains(p, i, d, i_max, i_min, antiwindup); } -void Pid::getGains( +void Pid::get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) { Gains gains = *gains_buffer_.readFromRT(); @@ -103,16 +103,16 @@ void Pid::getGains( antiwindup = gains.antiwindup_; } -Pid::Gains Pid::getGains() { return *gains_buffer_.readFromRT(); } +Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } -void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { Gains gains(p, i, d, i_max, i_min, antiwindup); - setGains(gains); + set_gains(gains); } -void Pid::setGains(const Gains & gains) +void Pid::set_gains(const Gains & gains) { if (gains.i_min_ > gains.i_max_) { std::cout << "received i_min > i_max, skip new gains\n"; @@ -121,23 +121,7 @@ void Pid::setGains(const Gains & gains) } } -double Pid::computeCommand(double error, uint64_t dt_ns) -{ - if (dt_ns == 0 || std::isnan(error) || std::isinf(error)) { - return 0.0; - } - - error_dot_ = d_error_; - - // Calculate the derivative error - double dt_s = (static_cast(dt_ns) / 1e9); - error_dot_ = (error - p_error_last_) / dt_s; - p_error_last_ = error; - - return computeCommand(error, error_dot_, dt_s); -} - -double Pid::computeCommand(double error, double dt_s) +double Pid::compute_command(double error, double dt_s) { if (dt_s <= 0.0 || std::isnan(error) || std::isinf(error)) { return 0.0; @@ -149,15 +133,10 @@ double Pid::computeCommand(double error, double dt_s) error_dot_ = (error - p_error_last_) / dt_s; p_error_last_ = error; - return computeCommand(error, error_dot_, dt_s); -} - -double Pid::computeCommand(double error, double error_dot, uint64_t dt_ns) -{ - return computeCommand(error, error_dot, static_cast(dt_ns) / 1.e9); + return compute_command(error, error_dot_, dt_s); } -double Pid::computeCommand(double error, double error_dot, double dt_s) +double Pid::compute_command(double error, double error_dot, double dt_s) { // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); @@ -202,13 +181,13 @@ double Pid::computeCommand(double error, double error_dot, double dt_s) return cmd_; } -void Pid::setCurrentCmd(double cmd) { cmd_ = cmd; } +void Pid::set_current_cmd(double cmd) { cmd_ = cmd; } -double Pid::getDerivativeError() { return error_dot_; } +double Pid::get_derivative_error() { return error_dot_; } -double Pid::getCurrentCmd() { return cmd_; } +double Pid::get_current_cmd() { return cmd_; } -void Pid::getCurrentPIDErrors(double & pe, double & ie, double & de) +void Pid::get_current_pid_errors(double & pe, double & ie, double & de) { pe = p_error_; ie = i_error_; diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 36e18d28..20488ba7 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -119,9 +119,9 @@ void PidROS::initialize(std::string topic_prefix) } } -bool PidROS::getBooleanParam(const std::string & param_name, bool & value) +bool PidROS::get_boolean_param(const std::string & param_name, bool & value) { - declareParam(param_name, rclcpp::ParameterValue(value)); + declare_param(param_name, rclcpp::ParameterValue(value)); rclcpp::Parameter param; if (node_params_->has_parameter(param_name)) { node_params_->get_parameter(param_name, param); @@ -138,9 +138,9 @@ bool PidROS::getBooleanParam(const std::string & param_name, bool & value) } // TODO(anyone): to-be-removed once this functionality becomes supported by the param API directly -bool PidROS::getDoubleParam(const std::string & param_name, double & value) +bool PidROS::get_double_param(const std::string & param_name, double & value) { - declareParam(param_name, rclcpp::ParameterValue(value)); + declare_param(param_name, rclcpp::ParameterValue(value)); rclcpp::Parameter param; if (node_params_->has_parameter(param_name)) { node_params_->get_parameter(param_name, param); @@ -164,80 +164,80 @@ bool PidROS::getDoubleParam(const std::string & param_name, double & value) } } -bool PidROS::initPid() +bool PidROS::init_pid() { double p, i, d, i_min, i_max; p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); bool antiwindup = false; bool all_params_available = true; - all_params_available &= getDoubleParam(param_prefix_ + "p", p); - all_params_available &= getDoubleParam(param_prefix_ + "i", i); - all_params_available &= getDoubleParam(param_prefix_ + "d", d); - all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_max", i_max); - all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min); + all_params_available &= get_double_param(param_prefix_ + "p", p); + all_params_available &= get_double_param(param_prefix_ + "i", i); + all_params_available &= get_double_param(param_prefix_ + "d", d); + all_params_available &= get_double_param(param_prefix_ + "i_clamp_max", i_max); + all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); - getBooleanParam(param_prefix_ + "antiwindup", antiwindup); + get_boolean_param(param_prefix_ + "antiwindup", antiwindup); if (all_params_available) { - setParameterEventCallback(); + set_parameter_event_callback(); } - pid_.initPid(p, i, d, i_max, i_min, antiwindup); + pid_.init_pid(p, i, d, i_max, i_min, antiwindup); return all_params_available; } -void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value) +void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValue param_value) { if (!node_params_->has_parameter(param_name)) { node_params_->declare_parameter(param_name, param_value); } } -void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.initPid(p, i, d, i_max, i_min, antiwindup); + pid_.init_pid(p, i, d, i_max, i_min, antiwindup); - declareParam(param_prefix_ + "p", rclcpp::ParameterValue(p)); - declareParam(param_prefix_ + "i", rclcpp::ParameterValue(i)); - declareParam(param_prefix_ + "d", rclcpp::ParameterValue(d)); - declareParam(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); - declareParam(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); - declareParam(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); + declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); + declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); + declare_param(param_prefix_ + "d", rclcpp::ParameterValue(d)); + declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); + declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); + declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); - setParameterEventCallback(); + set_parameter_event_callback(); } } void PidROS::reset() { pid_.reset(); } -std::shared_ptr> PidROS::getPidStatePublisher() +std::shared_ptr> PidROS::get_pid_state_publisher() { return state_pub_; } -double PidROS::computeCommand(double error, rclcpp::Duration dt) +double PidROS::compute_command(double error, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, static_cast(dt.nanoseconds())); - publishPIDState(cmd_, error, dt); + double cmd_ = pid_.compute_command(error, dt); + publish_pid_state(cmd_, error, dt); return cmd_; } -double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration dt) +double PidROS::compute_command(double error, double error_dot, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, error_dot, static_cast(dt.nanoseconds())); - publishPIDState(cmd_, error, dt); + double cmd_ = pid_.compute_command(error, error_dot, dt); + publish_pid_state(cmd_, error, dt); return cmd_; } -Pid::Gains PidROS::getGains() { return pid_.getGains(); } +Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } -void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); @@ -249,16 +249,16 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min, rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); - pid_.setGains(p, i, d, i_max, i_min, antiwindup); + pid_.set_gains(p, i, d, i_max, i_min, antiwindup); } } -void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) +void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) { - Pid::Gains gains = pid_.getGains(); + Pid::Gains gains = pid_.get_gains(); double p_error_, i_error_, d_error_; - getCurrentPIDErrors(p_error_, i_error_, d_error_); + get_current_pid_errors(p_error_, i_error_, d_error_); // Publish controller state if configured if (rt_state_pub_) { @@ -266,7 +266,7 @@ void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) 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 = pid_.getDerivativeError(); + rt_state_pub_->msg_.error_dot = pid_.get_derivative_error(); rt_state_pub_->msg_.p_error = p_error_; rt_state_pub_->msg_.i_error = i_error_; rt_state_pub_->msg_.d_error = d_error_; @@ -281,25 +281,25 @@ void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) } } -void PidROS::setCurrentCmd(double cmd) { pid_.setCurrentCmd(cmd); } +void PidROS::set_current_cmd(double cmd) { pid_.set_current_cmd(cmd); } -double PidROS::getCurrentCmd() { return pid_.getCurrentCmd(); } +double PidROS::get_current_cmd() { return pid_.get_current_cmd(); } -void PidROS::getCurrentPIDErrors(double & pe, double & ie, double & de) +void PidROS::get_current_pid_errors(double & pe, double & ie, double & de) { double _pe, _ie, _de; - pid_.getCurrentPIDErrors(_pe, _ie, _de); + pid_.get_current_pid_errors(_pe, _ie, _de); pe = _pe; ie = _ie; de = _de; } -void PidROS::printValues() +void PidROS::print_values() { - Pid::Gains gains = pid_.getGains(); + Pid::Gains gains = pid_.get_gains(); double p_error_, i_error_, d_error_; - getCurrentPIDErrors(p_error_, i_error_, d_error_); + get_current_pid_errors(p_error_, i_error_, d_error_); RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n" << " P Gain: " << gains.p_gain_ << "\n" @@ -312,26 +312,26 @@ void PidROS::printValues() << " P_Error: " << p_error_ << "\n" << " I_Error: " << i_error_ << "\n" << " D_Error: " << d_error_ << "\n" - << " Command: " << getCurrentCmd();); + << " Command: " << get_current_cmd();); } -void PidROS::setGains(const Pid::Gains & gains) +void PidROS::set_gains(const Pid::Gains & gains) { if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.setGains(gains); + pid_.set_gains(gains); } } -void PidROS::setParameterEventCallback() +void PidROS::set_parameter_event_callback() { 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(); + Pid::Gains gains = pid_.get_gains(); bool changed = false; for (auto & parameter : parameters) { @@ -362,11 +362,11 @@ void PidROS::setParameterEventCallback() } if (changed) { - /// @note don't call setGains() from inside a callback + /// @note don't call set_gains() from inside a callback if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.setGains(gains); + pid_.set_gains(gains); } } diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 9a26d659..d54dfce0 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -58,7 +58,7 @@ void check_set_parameters( const double I_MIN = -10.0; const bool ANTIWINDUP = true; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); + ASSERT_NO_THROW(pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); rclcpp::Parameter param; @@ -82,7 +82,7 @@ void check_set_parameters( ASSERT_EQ(param.get_value(), ANTIWINDUP); // check gains were set - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -115,7 +115,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); + ASSERT_NO_THROW(pid.init_pid(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); rclcpp::Parameter param; @@ -128,7 +128,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("antiwindup", param)); // check gains were NOT set - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, 0.0); ASSERT_EQ(gains.i_gain_, 0.0); ASSERT_EQ(gains.d_gain_, 0.0); @@ -216,7 +216,7 @@ TEST(PidParametersTest, SetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; @@ -242,7 +242,7 @@ TEST(PidParametersTest, SetParametersTest) rclcpp::spin_some(node->get_node_base_interface()); // check gains were set using the parameters - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -266,7 +266,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double I_MIN_BAD = 20.0; const bool ANTIWINDUP = true; - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; @@ -292,7 +292,7 @@ TEST(PidParametersTest, SetBadParametersTest) rclcpp::spin_some(node->get_node_base_interface()); // check gains were NOT set using the parameters - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -314,8 +314,8 @@ TEST(PidParametersTest, GetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false); - pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.init_pid(0.0, 0.0, 0.0, 0.0, 0.0, false); + pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rclcpp::Parameter param; @@ -344,7 +344,7 @@ TEST(PidParametersTest, GetParametersFromParams) TestablePidROS pid(node); - ASSERT_TRUE(pid.initPid()); + ASSERT_TRUE(pid.init_pid()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); @@ -380,8 +380,8 @@ TEST(PidParametersTest, MultiplePidInstances) const double I_MAX = 10.0; const double I_MIN = -10.0; - ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false)); - ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true)); + ASSERT_NO_THROW(pid_1.init_pid(P, I, D, I_MAX, I_MIN, false)); + ASSERT_NO_THROW(pid_2.init_pid(P, I, D, I_MAX, I_MIN, true)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp index 1c3de5f1..f215b6a0 100644 --- a/test/pid_publisher_tests.cpp +++ b/test/pid_publisher_tests.cpp @@ -39,7 +39,7 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.init_pid(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -50,12 +50,12 @@ TEST(PidPublisherTest, PublishTest) auto state_sub = node->create_subscription( "/pid_state", rclcpp::SensorDataQoS(), state_callback); - double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); EXPECT_EQ(-1.5, command); // wait for callback for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { - pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); rclcpp::spin_some(node); std::this_thread::sleep_for(DELAY); } @@ -74,10 +74,10 @@ TEST(PidPublisherTest, PublishTestLifecycle) auto state_pub_lifecycle_ = std::dynamic_pointer_cast>( - pid_ros.getPidStatePublisher()); + pid_ros.get_pid_state_publisher()); // state_pub_lifecycle_->on_activate(); - pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.init_pid(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -88,12 +88,12 @@ TEST(PidPublisherTest, PublishTestLifecycle) auto state_sub = node->create_subscription( "/pid_state", rclcpp::SensorDataQoS(), state_callback); - double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); EXPECT_EQ(-1.5, command); // wait for callback for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { - pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); rclcpp::spin_some(node->get_node_base_interface()); std::this_thread::sleep_for(DELAY); } diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index f8b71465..15e66462 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -58,11 +58,11 @@ TEST(ParameterTest, ITermBadIBoundsTest) "i_min > i_max)."); Pid pid(1.0, 1.0, 1.0, 1.0, -1.0); - auto gains = pid.getGains(); + auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); // Try to set bad i-bounds, i.e. i_min > i_max - EXPECT_NO_THROW(pid.setGains(1.0, 1.0, 1.0, -2.0, 2.0)); + EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -2.0, 2.0)); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); @@ -80,11 +80,11 @@ TEST(ParameterTest, integrationClampTest) double cmd = 0.0; // Test lower limit - cmd = pid.computeCommand(-10.03, 1.0); + cmd = pid.compute_command(-10.03, 1.0); EXPECT_EQ(-1.0, cmd); // Test upper limit - cmd = pid.computeCommand(30.0, 1.0); + cmd = pid.compute_command(30.0, 1.0); EXPECT_EQ(1.0, cmd); } @@ -104,13 +104,13 @@ TEST(ParameterTest, integrationClampZeroGainTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, 1.0); - pid.getCurrentPIDErrors(pe, ie, de); + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); @@ -129,16 +129,16 @@ TEST(ParameterTest, integrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(0.5, 1.0); + cmd = pid.compute_command(0.5, 1.0); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); } @@ -155,16 +155,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.compute_command(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.compute_command(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(-0.05, 1.0); + cmd = pid.compute_command(-0.05, 1.0); EXPECT_EQ(-0.075, cmd); - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.compute_command(0.1, 1.0); EXPECT_EQ(-0.2, cmd); } @@ -189,7 +189,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; bool antiwindup_return; - pid1.getGains( + pid1.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); @@ -207,9 +207,9 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); - Pid::Gains g1 = pid1.getGains(); + Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); EXPECT_EQ(i_gain, g1.i_gain_); EXPECT_EQ(d_gain, g1.d_gain_); @@ -222,13 +222,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- // Send update command to populate errors ------------------------------------------------- - pid1.setCurrentCmd(10); - (void) pid1.computeCommand(20, 1.0); + pid1.set_current_cmd(10); + (void) pid1.compute_command(20, 1.0); // Test copy constructor ------------------------------------------------- Pid pid2(pid1); - pid2.getGains( + pid2.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); @@ -240,7 +240,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test that errors are zero double pe2, ie2, de2; - pid2.getCurrentPIDErrors(pe2, ie2, de2); + pid2.get_current_pid_errors(pe2, ie2, de2); EXPECT_EQ(0.0, pe2); EXPECT_EQ(0.0, ie2); EXPECT_EQ(0.0, de2); @@ -249,7 +249,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid3; pid3 = pid1; - pid3.getGains( + pid3.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); @@ -261,7 +261,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test that errors are zero double pe3, ie3, de3; - pid3.getCurrentPIDErrors(pe3, ie3, de3); + pid3.get_current_pid_errors(pe3, ie3, de3); EXPECT_EQ(0.0, pe3); EXPECT_EQ(0.0, ie3); EXPECT_EQ(0.0, de3); @@ -270,12 +270,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid1.reset(); double pe1, ie1, de1; - pid1.getCurrentPIDErrors(pe1, ie1, de1); + pid1.get_current_pid_errors(pe1, ie1, de1); EXPECT_EQ(0.0, pe1); EXPECT_EQ(0.0, ie1); EXPECT_EQ(0.0, de1); - double cmd1 = pid1.getCurrentCmd(); + double cmd1 = pid1.get_current_cmd(); EXPECT_EQ(0.0, cmd1); } @@ -291,22 +291,22 @@ TEST(CommandTest, proportionalOnlyTest) double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-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, 1.0); + cmd = pid.compute_command(-1.0, 1.0); // Then expect the command doubled EXPECT_EQ(-1.0, cmd); // If call with positive error - cmd = pid.computeCommand(0.5, 1.0); + cmd = pid.compute_command(0.5, 1.0); // Then expect always command = error EXPECT_EQ(0.5, cmd); } @@ -323,26 +323,26 @@ TEST(CommandTest, integralOnlyTest) double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same arguments - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-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, 1.0); + cmd = pid.compute_command(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, 1.0); + cmd = pid.compute_command(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, 1.0); + cmd = pid.compute_command(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); } @@ -359,27 +359,27 @@ TEST(CommandTest, derivativeOnlyTest) double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same error - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-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, 0.1); + cmd = pid.compute_command(-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, 1.0); + cmd = pid.compute_command(-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, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } @@ -396,17 +396,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, 1.0); + cmd = pid.compute_command(-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, 1.0); + cmd = pid.compute_command(-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, 1.0); + cmd = pid.compute_command(-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); } From e18bc66bf7e09df3d823ab44074ccc26dd7e3651 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 13 Dec 2024 20:34:55 +0100 Subject: [PATCH 06/21] init_pid --> initialize Co-authored-by: Sai Kishor Kothakota --- include/control_toolbox/pid.hpp | 8 ++++---- include/control_toolbox/pid_ros.hpp | 8 ++++---- src/pid.cpp | 2 +- src/pid_ros.cpp | 8 ++++---- test/pid_parameters_tests.cpp | 16 ++++++++-------- test/pid_publisher_tests.cpp | 4 ++-- 6 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 43cdb825..69998e4b 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -89,7 +89,7 @@ namespace control_toolbox \verbatim control_toolbox::Pid pid; - pid.init_pid(6.0, 1.0, 2.0, 0.3, -0.3); + pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3); double position_desi_ = 0.5; ... rclcpp::Time last_time = get_clock()->now(); @@ -197,7 +197,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min_ > i_max_ */ - void init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void initialize(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 @@ -212,9 +212,9 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use init_pid() instead")]] void initPid( + [[deprecated("Use initialize() instead")]] void initPid( double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { - init_pid(p, i, d, i_max, i_min, antiwindup); + initialize(p, i, d, i_max, i_min, antiwindup); } /*! diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index e58ad470..71f55e6e 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -101,7 +101,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if i_min_ > i_max_ */ - void init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + void initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! * \brief Initialize the PID controller and set the parameters @@ -114,9 +114,9 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if i_min_ > i_max_ */ - [[deprecated("Use init_pid() instead")]] void initPid( + [[deprecated("Use initialize() instead")]] void initPid( double p, double i, double d, double i_max, double i_min, bool antiwindup) { - init_pid(p, i, d, i_max, i_min, antiwindup); + initialize(p, i, d, i_max, i_min, antiwindup); } /*! @@ -130,7 +130,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ [[deprecated("Use init_pid() instead")]] bool initPid() { - return init_pid(); + return initialize(); } /*! diff --git a/src/pid.cpp b/src/pid.cpp index fb1ee9b4..6a376d5d 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -68,7 +68,7 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { set_gains(p, i, d, i_max, i_min, antiwindup); diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 20488ba7..d1458c58 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -164,7 +164,7 @@ bool PidROS::get_double_param(const std::string & param_name, double & value) } } -bool PidROS::init_pid() +bool PidROS::initialize() { double p, i, d, i_min, i_max; p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); @@ -182,7 +182,7 @@ bool PidROS::init_pid() set_parameter_event_callback(); } - pid_.init_pid(p, i, d, i_max, i_min, antiwindup); + pid_.initialize(p, i, d, i_max, i_min, antiwindup); return all_params_available; } @@ -194,12 +194,12 @@ void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValu } } -void PidROS::init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.init_pid(p, i, d, i_max, i_min, antiwindup); + pid_.initialize(p, i, d, i_max, i_min, antiwindup); declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index d54dfce0..63affa72 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -58,7 +58,7 @@ void check_set_parameters( const double I_MIN = -10.0; const bool ANTIWINDUP = true; - ASSERT_NO_THROW(pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); + ASSERT_NO_THROW(pid.initialize(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); rclcpp::Parameter param; @@ -115,7 +115,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; - ASSERT_NO_THROW(pid.init_pid(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); + ASSERT_NO_THROW(pid.initialize(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); rclcpp::Parameter param; @@ -216,7 +216,7 @@ TEST(PidParametersTest, SetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; @@ -266,7 +266,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double I_MIN_BAD = 20.0; const bool ANTIWINDUP = true; - pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.initialize(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; @@ -314,7 +314,7 @@ TEST(PidParametersTest, GetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - pid.init_pid(0.0, 0.0, 0.0, 0.0, 0.0, false); + pid.initialize(0.0, 0.0, 0.0, 0.0, 0.0, false); pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rclcpp::Parameter param; @@ -344,7 +344,7 @@ TEST(PidParametersTest, GetParametersFromParams) TestablePidROS pid(node); - ASSERT_TRUE(pid.init_pid()); + ASSERT_TRUE(pid.initialize()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); @@ -380,8 +380,8 @@ TEST(PidParametersTest, MultiplePidInstances) const double I_MAX = 10.0; const double I_MIN = -10.0; - ASSERT_NO_THROW(pid_1.init_pid(P, I, D, I_MAX, I_MIN, false)); - ASSERT_NO_THROW(pid_2.init_pid(P, I, D, I_MAX, I_MIN, true)); + ASSERT_NO_THROW(pid_1.initialize(P, I, D, I_MAX, I_MIN, false)); + ASSERT_NO_THROW(pid_2.initialize(P, I, D, I_MAX, I_MIN, true)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp index f215b6a0..746cae9c 100644 --- a/test/pid_publisher_tests.cpp +++ b/test/pid_publisher_tests.cpp @@ -39,7 +39,7 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.init_pid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.initialize(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -77,7 +77,7 @@ TEST(PidPublisherTest, PublishTestLifecycle) pid_ros.get_pid_state_publisher()); // state_pub_lifecycle_->on_activate(); - pid_ros.init_pid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.initialize(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; From 0539f8721672df066283c8a8a93af140b1728121 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 13 Dec 2024 19:35:43 +0000 Subject: [PATCH 07/21] initialize --- include/control_toolbox/pid_ros.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 71f55e6e..96e14af6 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -123,13 +123,13 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ - bool init_pid(); + bool initialize(); /*! * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ - [[deprecated("Use init_pid() instead")]] bool initPid() { + [[deprecated("Use initialize() instead")]] bool initPid() { return initialize(); } From c96b8dfaa6a372164928377e4fe29e950e959d50 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 13 Dec 2024 19:37:20 +0000 Subject: [PATCH 08/21] Satisfy pre-commit --- include/control_toolbox/pid.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 69998e4b..6fc74f95 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -197,7 +197,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min_ > i_max_ */ - void initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void initialize( + 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 From 8954660a82d601723d36d775219b483ef7edbd44 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 13 Dec 2024 20:52:45 +0000 Subject: [PATCH 09/21] Move function definitions to cpp file --- include/control_toolbox/pid.hpp | 39 +++++++++++---------------------- src/pid.cpp | 35 ++++++++++++++++++++++++----- 2 files changed, 42 insertions(+), 32 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 6fc74f95..95e7420e 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -370,9 +370,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, rcl_duration_value_t dt_ns) { - return compute_command(error, static_cast(dt_ns)/1.e9); - } + [[nodiscard]] double compute_command(double error, rcl_duration_value_t dt_ns); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -384,9 +382,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, rclcpp::Duration dt) { - return compute_command(error, dt.seconds()); - } + [[nodiscard]] double compute_command(double error, rclcpp::Duration dt); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -398,9 +394,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, std::chrono::nanoseconds dt_ns) { - return compute_command(error, static_cast(dt_ns.count())/1.e9); - } + [[nodiscard]] double compute_command(double error, std::chrono::nanoseconds dt_ns); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -442,9 +436,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, double error_dot, rcl_duration_value_t dt_ns) { - return compute_command(error, error_dot, static_cast(dt_ns)/1.e9); - } + [[nodiscard]] double compute_command(double error, double error_dot, rcl_duration_value_t dt_ns); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -457,9 +449,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, double error_dot, rclcpp::Duration dt) { - return compute_command(error, error_dot, dt.seconds()); - } + [[nodiscard]] double compute_command(double error, double error_dot, rclcpp::Duration dt); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -473,9 +463,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \returns PID command */ [[nodiscard]] double compute_command( - double error, double error_dot, std::chrono::nanoseconds dt_ns) { - return compute_command(error, error_dot, static_cast(dt_ns.count())/1.e9); - } + double error, double error_dot, std::chrono::nanoseconds dt_ns); /*! * \brief Set current command for this PID controller @@ -504,13 +492,12 @@ class CONTROL_TOOLBOX_PUBLIC Pid /*! * \brief Return derivative error */ - double get_derivative_error(); - - /*! - * \brief Return derivative error - */ - [[deprecated("Use get_derivative_error() instead")]] - double getDerivativeError() { return get_derivative_error(); } + [[deprecated("Use get_current_pid_errors() instead")]] + double getDerivativeError() { + double pe, ie, de; + get_current_pid_errors(pe, ie, de); + return de; + } /*! * \brief Return PID error terms for the controller. @@ -560,7 +547,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid double i_error_; /**< Integral of position error. */ double d_error_; /**< Derivative of position error. */ double cmd_; /**< Command to send. */ - double error_dot_; /**< Derivative error */ + [[deprecated("Use d_error_")]] double error_dot_; /**< Derivative error */ }; } // namespace control_toolbox diff --git a/src/pid.cpp b/src/pid.cpp index 6a376d5d..21bde27f 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -81,6 +81,7 @@ void Pid::reset() p_error_ = 0.0; i_error_ = 0.0; d_error_ = 0.0; + error_dot_ = 0.0; // deprecated cmd_ = 0.0; } @@ -127,13 +128,36 @@ double Pid::compute_command(double error, double dt_s) return 0.0; } - error_dot_ = d_error_; - // Calculate the derivative error - error_dot_ = (error - p_error_last_) / dt_s; + d_error_ = (error - p_error_last_) / dt_s; p_error_last_ = error; - return compute_command(error, error_dot_, dt_s); + return compute_command(error, d_error_, dt_s); +} + +double Pid::compute_command(double error, rcl_duration_value_t dt_ns) { + return compute_command(error, static_cast(dt_ns)/1.e9); +} + +double Pid::compute_command(double error, rclcpp::Duration dt) { + return compute_command(error, dt.seconds()); +} + +double Pid::compute_command(double error, std::chrono::nanoseconds dt_ns) { + return compute_command(error, static_cast(dt_ns.count())/1.e9); +} + +double Pid::compute_command(double error, double error_dot, rcl_duration_value_t dt_ns) { + return compute_command(error, error_dot, static_cast(dt_ns)/1.e9); +} + +double Pid::compute_command(double error, double error_dot, rclcpp::Duration dt) { + return compute_command(error, error_dot, dt.seconds()); +} + +double Pid::compute_command( + double error, double error_dot, std::chrono::nanoseconds dt_ns) { + return compute_command(error, error_dot, static_cast(dt_ns.count())/1.e9); } double Pid::compute_command(double error, double error_dot, double dt_s) @@ -144,6 +168,7 @@ double Pid::compute_command(double error, double error_dot, double dt_s) double p_term, d_term, i_term; p_error_ = error; // this is error = target - state d_error_ = error_dot; + error_dot_ = error_dot; // deprecated if ( dt_s <= 0.0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || @@ -183,8 +208,6 @@ double Pid::compute_command(double error, double error_dot, double dt_s) void Pid::set_current_cmd(double cmd) { cmd_ = cmd; } -double Pid::get_derivative_error() { return error_dot_; } - double Pid::get_current_cmd() { return cmd_; } void Pid::get_current_pid_errors(double & pe, double & ie, double & de) From eb12319ce8ed9645cc67c302fd2312d168051dde Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 13 Dec 2024 21:01:10 +0000 Subject: [PATCH 10/21] Rename variables --- src/pid_ros.cpp | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index d1458c58..8c9f0d98 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -221,18 +221,18 @@ std::shared_ptr> PidROS::get_pid_ double PidROS::compute_command(double error, rclcpp::Duration dt) { - double cmd_ = pid_.compute_command(error, dt); - publish_pid_state(cmd_, error, dt); + double cmd = pid_.compute_command(error, dt); + publish_pid_state(cmd, error, dt); - return cmd_; + return cmd; } double PidROS::compute_command(double error, double error_dot, rclcpp::Duration dt) { - double cmd_ = pid_.compute_command(error, error_dot, dt); - publish_pid_state(cmd_, error, dt); + double cmd = pid_.compute_command(error, error_dot, dt); + publish_pid_state(cmd, error, dt); - return cmd_; + return cmd; } Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } @@ -257,8 +257,8 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) { Pid::Gains gains = pid_.get_gains(); - double p_error_, i_error_, d_error_; - get_current_pid_errors(p_error_, i_error_, d_error_); + double p_error, i_error, d_error; + get_current_pid_errors(p_error, i_error, d_error); // Publish controller state if configured if (rt_state_pub_) { @@ -266,10 +266,10 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) 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 = pid_.get_derivative_error(); - 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_.error_dot = d_error; + 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_; @@ -298,8 +298,8 @@ void PidROS::print_values() { Pid::Gains gains = pid_.get_gains(); - double p_error_, i_error_, d_error_; - get_current_pid_errors(p_error_, i_error_, d_error_); + double p_error, i_error, d_error; + get_current_pid_errors(p_error, i_error, d_error); RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n" << " P Gain: " << gains.p_gain_ << "\n" @@ -309,9 +309,9 @@ void PidROS::print_values() << " 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" + << " P_Error: " << p_error << "\n" + << " I_Error: " << i_error << "\n" + << " D_Error: " << d_error << "\n" << " Command: " << get_current_cmd();); } From 542b68a128c0da7190eaf089c61004e874858e77 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 13 Dec 2024 21:20:48 +0000 Subject: [PATCH 11/21] Add tests for overloaded methods --- test/pid_tests.cpp | 75 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 15e66462..47ee53f6 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -38,6 +38,7 @@ #include "gtest/gtest.h" using control_toolbox::Pid; +using namespace std::chrono_literals; TEST(ParameterTest, ITermBadIBoundsTestConstructor) { @@ -411,6 +412,80 @@ TEST(CommandTest, completePIDTest) EXPECT_EQ(-3.5, cmd); } +TEST(CommandTest, compatibilityTest) +{ + RecordProperty( + "description", + "Tests deprecated methods."); + + // Disable deprecated warnings + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + + Pid pid; + Pid pid_old; + pid.initialize(1.0, 1.0, 1.0, 5.0, -5.0); + pid_old.initPid(1.0, 1.0, 1.0, 5.0, -5.0); + + auto cmd1 = pid.compute_command(-0.5, 1.0); + auto cmd2 = pid_old.computeCommand(-0.5, static_cast(1.0 * 1e9)); + EXPECT_EQ(cmd1, cmd2); + + pid.set_gains(2.0, 1.0, 1.0, 5.0, -5.0); + pid_old.setGains(2.0, 1.0, 1.0, 5.0, -5.0); + + cmd1 = pid.compute_command(-0.5, 1.0); + cmd2 = pid_old.computeCommand(-0.5, static_cast(1.0 * 1e9)); + EXPECT_EQ(cmd1, cmd2); + + // Re-enable deprecated warnings + #pragma GCC diagnostic pop +} + +TEST(CommandTest, timeArgumentTest) +{ + RecordProperty( + "description", + "Tests different dt argument type methods."); + + Pid pid1(1.0, 1.0, 1.0, 5.0, -5.0); + Pid pid2(1.0, 1.0, 1.0, 5.0, -5.0); + Pid pid3(1.0, 1.0, 1.0, 5.0, -5.0); + Pid pid4(1.0, 1.0, 1.0, 5.0, -5.0); + + // call without error_dt, dt is used to calculate error_dt + auto cmd1 = pid1.compute_command(-0.5, 1.0); + auto dt = rclcpp::Duration::from_seconds(1.0); + auto cmd2 = pid2.compute_command(-0.5, dt); + auto cmd3 = pid3.compute_command(-0.5, dt.nanoseconds()); + auto cmd4 = pid4.compute_command(-0.5, std::chrono::nanoseconds(1s)); + + // If initial error = 0, all gains = 1, dt = 1 + // Then expect command = 3x error + EXPECT_EQ(-1.5, cmd1); + EXPECT_EQ(cmd1, cmd2); + EXPECT_EQ(cmd1, cmd3); + EXPECT_EQ(cmd1, cmd4); + + // call with error_dt -> integral part is updated with dt + cmd1 = pid1.compute_command(-0.5, 0.0, 1.0); + cmd2 = pid2.compute_command(-0.5, 0.0, dt); + cmd3 = pid3.compute_command(-0.5, 0.0, dt.nanoseconds()); + cmd4 = pid4.compute_command(-0.5, 0.0, std::chrono::nanoseconds(1s)); + EXPECT_EQ(-1.5, cmd1); + EXPECT_EQ(cmd1, cmd2); + EXPECT_EQ(cmd1, cmd3); + EXPECT_EQ(cmd1, cmd4); + cmd1 = pid1.compute_command(-0.5, 0.0, 1.0); + cmd2 = pid2.compute_command(-0.5, 0.0, dt); + cmd3 = pid3.compute_command(-0.5, 0.0, dt.nanoseconds()); + cmd4 = pid4.compute_command(-0.5, 0.0, std::chrono::nanoseconds(1s)); + EXPECT_EQ(-2.0, cmd1); + EXPECT_EQ(cmd1, cmd2); + EXPECT_EQ(cmd1, cmd3); + EXPECT_EQ(cmd1, cmd4); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 6cf88ab2a551383507b1290fc04c056c8d096acc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Dec 2024 20:46:09 +0100 Subject: [PATCH 12/21] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- src/pid.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/pid.cpp b/src/pid.cpp index 21bde27f..1beaafbe 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -124,7 +124,7 @@ void Pid::set_gains(const Gains & gains) double Pid::compute_command(double error, double dt_s) { - if (dt_s <= 0.0 || std::isnan(error) || std::isinf(error)) { + if (dt_s <= 0.0 || !std::isfinite(error)) { return 0.0; } @@ -171,8 +171,7 @@ double Pid::compute_command(double error, double error_dot, double dt_s) error_dot_ = error_dot; // deprecated if ( - dt_s <= 0.0 || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || - std::isinf(error_dot)) { + dt_s <= 0.0 || !std::isfinite(error) || !std::isfinite(error_dot)) { return 0.0; } From 49e192f4b334a55265dfbe88b4ffd0cab692e070 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 17 Dec 2024 19:59:00 +0000 Subject: [PATCH 13/21] Make dt arguments const & --- include/control_toolbox/pid.hpp | 17 +++++++++-------- include/control_toolbox/pid_ros.hpp | 4 ++-- src/pid.cpp | 16 ++++++++-------- src/pid_ros.cpp | 4 ++-- 4 files changed, 21 insertions(+), 20 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 86156ba6..e08b9014 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -343,7 +343,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, double dt_s); + [[nodiscard]] double compute_command(double error, const double & dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -370,7 +370,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, rcl_duration_value_t dt_ns); + [[nodiscard]] double compute_command(double error, const rcl_duration_value_t & dt_ns); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -382,7 +382,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, rclcpp::Duration dt); + [[nodiscard]] double compute_command(double error, const rclcpp::Duration & dt); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -394,7 +394,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, std::chrono::nanoseconds dt_ns); + [[nodiscard]] double compute_command(double error, const std::chrono::nanoseconds & dt_ns); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -407,7 +407,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, double error_dot, double dt_s); + [[nodiscard]] double compute_command(double error, double error_dot, const double & dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -436,7 +436,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, double error_dot, rcl_duration_value_t dt_ns); + [[nodiscard]] double compute_command( + double error, double error_dot, const rcl_duration_value_t & dt_ns); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -449,7 +450,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double compute_command(double error, double error_dot, rclcpp::Duration dt); + [[nodiscard]] double compute_command(double error, double error_dot, const rclcpp::Duration & dt); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -463,7 +464,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \returns PID command */ [[nodiscard]] double compute_command( - double error, double error_dot, std::chrono::nanoseconds dt_ns); + double error, double error_dot, const std::chrono::nanoseconds & dt_ns); /*! * \brief Set current command for this PID controller diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index 7978ba5d..b4536015 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -148,7 +148,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \returns PID command */ - double compute_command(double error, rclcpp::Duration dt); + double compute_command(double error, const rclcpp::Duration & dt); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -176,7 +176,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \returns PID command */ - double compute_command(double error, double error_dot, rclcpp::Duration dt); + double compute_command(double error, double error_dot, const rclcpp::Duration & dt); /*! * \brief Set the PID error and compute the PID command with nonuniform diff --git a/src/pid.cpp b/src/pid.cpp index 1beaafbe..732387a0 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -122,7 +122,7 @@ void Pid::set_gains(const Gains & gains) } } -double Pid::compute_command(double error, double dt_s) +double Pid::compute_command(double error, const double & dt_s) { if (dt_s <= 0.0 || !std::isfinite(error)) { return 0.0; @@ -135,32 +135,32 @@ double Pid::compute_command(double error, double dt_s) return compute_command(error, d_error_, dt_s); } -double Pid::compute_command(double error, rcl_duration_value_t dt_ns) { +double Pid::compute_command(double error, const rcl_duration_value_t & dt_ns) { return compute_command(error, static_cast(dt_ns)/1.e9); } -double Pid::compute_command(double error, rclcpp::Duration dt) { +double Pid::compute_command(double error, const rclcpp::Duration & dt) { return compute_command(error, dt.seconds()); } -double Pid::compute_command(double error, std::chrono::nanoseconds dt_ns) { +double Pid::compute_command(double error, const std::chrono::nanoseconds & dt_ns) { return compute_command(error, static_cast(dt_ns.count())/1.e9); } -double Pid::compute_command(double error, double error_dot, rcl_duration_value_t dt_ns) { +double Pid::compute_command(double error, double error_dot, const rcl_duration_value_t & dt_ns) { return compute_command(error, error_dot, static_cast(dt_ns)/1.e9); } -double Pid::compute_command(double error, double error_dot, rclcpp::Duration dt) { +double Pid::compute_command(double error, double error_dot, const rclcpp::Duration & dt) { return compute_command(error, error_dot, dt.seconds()); } double Pid::compute_command( - double error, double error_dot, std::chrono::nanoseconds dt_ns) { + double error, double error_dot, const std::chrono::nanoseconds & dt_ns) { return compute_command(error, error_dot, static_cast(dt_ns.count())/1.e9); } -double Pid::compute_command(double error, double error_dot, double dt_s) +double Pid::compute_command(double error, double error_dot, const double & dt_s) { // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 8c9f0d98..2d7b035c 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -219,7 +219,7 @@ std::shared_ptr> PidROS::get_pid_ return state_pub_; } -double PidROS::compute_command(double error, rclcpp::Duration dt) +double PidROS::compute_command(double error, const rclcpp::Duration & dt) { double cmd = pid_.compute_command(error, dt); publish_pid_state(cmd, error, dt); @@ -227,7 +227,7 @@ double PidROS::compute_command(double error, rclcpp::Duration dt) return cmd; } -double PidROS::compute_command(double error, double error_dot, rclcpp::Duration dt) +double PidROS::compute_command(double error, double error_dot, const rclcpp::Duration & dt) { double cmd = pid_.compute_command(error, error_dot, dt); publish_pid_state(cmd, error, dt); From f694d57313a3b9c57864931173ed86dcc5e85211 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 12:15:06 +0000 Subject: [PATCH 14/21] Rename initialize methods --- include/control_toolbox/pid_ros.hpp | 11 ++++++----- src/pid_ros.cpp | 6 +++--- test/pid_parameters_tests.cpp | 2 +- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index b4536015..c5c7802e 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -123,14 +123,14 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ - bool initialize(); + bool initialize_from_ros_parameters(); /*! * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ - [[deprecated("Use initialize() instead")]] bool initPid() { - return initialize(); + [[deprecated("Use initialize_from_ros_parameters() instead")]] bool initPid() { + return initialize_from_ros_parameters(); } /*! @@ -362,12 +362,13 @@ class CONTROL_TOOLBOX_PUBLIC PidROS bool get_boolean_param(const std::string & param_name, bool & value); /*! - * \param topic_prefix prefix to add to the pid parameters. + * \brief Set prefix for topic and parameter names + * \param[in] topic_prefix prefix to add to the pid parameters. * Per default is prefix interpreted as prefix for topics. * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., * "/" will be added in front of topic prefix */ - void initialize(std::string topic_prefix); + void set_prefixes(std::string topic_prefix); rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 2d7b035c..6628a8d2 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -83,7 +83,7 @@ PidROS::PidROS( } else { - initialize(prefix); + set_prefixes(prefix); } state_pub_ = rclcpp::create_publisher( @@ -92,7 +92,7 @@ PidROS::PidROS( new realtime_tools::RealtimePublisher(state_pub_)); } -void PidROS::initialize(std::string topic_prefix) +void PidROS::set_prefixes(std::string topic_prefix) { param_prefix_ = topic_prefix; // If it starts with a "~", remove it @@ -164,7 +164,7 @@ bool PidROS::get_double_param(const std::string & param_name, double & value) } } -bool PidROS::initialize() +bool PidROS::initialize_from_ros_parameters() { double p, i, d, i_min, i_max; p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 63affa72..29045866 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -344,7 +344,7 @@ TEST(PidParametersTest, GetParametersFromParams) TestablePidROS pid(node); - ASSERT_TRUE(pid.initialize()); + ASSERT_TRUE(pid.initialize_from_ros_parameters()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); From 46e89a48e44b8aca750b308d2ca02e7aa3e2cd56 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 12:18:53 +0000 Subject: [PATCH 15/21] Disable deprecation warning of deprecated class member --- include/control_toolbox/pid.hpp | 1 + src/pid.cpp | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index e08b9014..c213706e 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -548,6 +548,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid double i_error_; /**< Integral of position error. */ double d_error_; /**< Derivative of position error. */ double cmd_; /**< Command to send. */ + // TODO(christophfroehlich) remove this -> breaks ABI [[deprecated("Use d_error_")]] double error_dot_; /**< Derivative error */ }; diff --git a/src/pid.cpp b/src/pid.cpp index 732387a0..8aed6a38 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -44,6 +44,9 @@ #include "control_toolbox/pid.hpp" +// Disable deprecated warnings +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" namespace control_toolbox { Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) @@ -66,6 +69,9 @@ Pid::Pid(const Pid & source) reset(); } +// Enable deprecated warnings again +#pragma GCC diagnostic pop + Pid::~Pid() {} void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) @@ -81,7 +87,13 @@ void Pid::reset() p_error_ = 0.0; i_error_ = 0.0; d_error_ = 0.0; + + // Disable deprecated warnings + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" error_dot_ = 0.0; // deprecated + #pragma GCC diagnostic pop + cmd_ = 0.0; } @@ -168,7 +180,11 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) double p_term, d_term, i_term; p_error_ = error; // this is error = target - state d_error_ = error_dot; + + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" error_dot_ = error_dot; // deprecated + #pragma GCC diagnostic pop if ( dt_s <= 0.0 || !std::isfinite(error) || !std::isfinite(error_dot)) { From fc065936d3376463ba131cddb512bd451edcb6fb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 18 Dec 2024 12:20:05 +0000 Subject: [PATCH 16/21] Update comments --- include/control_toolbox/pid.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index c213706e..2fd305d4 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -543,13 +543,13 @@ class CONTROL_TOOLBOX_PUBLIC Pid // blocking the realtime update loop realtime_tools::RealtimeBuffer gains_buffer_; - 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. */ + double p_error_last_; /** Save state for derivative state calculation. */ + double p_error_; /** Error. */ + double i_error_; /** Integral of error. */ + double d_error_; /** Derivative of error. */ + double cmd_; /** Command to send. */ // TODO(christophfroehlich) remove this -> breaks ABI - [[deprecated("Use d_error_")]] double error_dot_; /**< Derivative error */ + [[deprecated("Use d_error_")]] double error_dot_; /** Derivative error */ }; } // namespace control_toolbox From 44a8734126db39bec4a3fe833619c120dfeeb13c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 18 Dec 2024 20:04:45 +0100 Subject: [PATCH 17/21] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- include/control_toolbox/pid_ros.hpp | 2 +- src/pid_ros.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index c5c7802e..f3481dfa 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -368,7 +368,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., * "/" will be added in front of topic prefix */ - void set_prefixes(std::string topic_prefix); + void set_prefixes(const std::string &topic_prefix); rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 6628a8d2..28b477b8 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -92,7 +92,7 @@ PidROS::PidROS( new realtime_tools::RealtimePublisher(state_pub_)); } -void PidROS::set_prefixes(std::string topic_prefix) +void PidROS::set_prefixes(const std::string &topic_prefix) { param_prefix_ = topic_prefix; // If it starts with a "~", remove it From 3d3bb503fab3255797a5d69e367c1181ce790245 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 18 Dec 2024 20:05:38 +0100 Subject: [PATCH 18/21] Update docstring --- include/control_toolbox/pid.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 2fd305d4..8fe251c0 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -90,12 +90,12 @@ namespace control_toolbox \verbatim control_toolbox::Pid pid; pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3); - double position_desi_ = 0.5; + double position_desi = 0.5; ... rclcpp::Time last_time = get_clock()->now(); while (true) { rclcpp::Time time = get_clock()->now(); - double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time)); + double effort = pid.compute_command(position_desi - currentPosition(), time - last_time); last_time = time; } \endverbatim From 876907dadd6cb92577275b135c1b797cc434a7a2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 1 Jan 2025 21:56:20 +0000 Subject: [PATCH 19/21] Cleanup includes --- include/control_toolbox/pid.hpp | 5 +---- src/pid.cpp | 4 ++-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 8fe251c0..a71bae3b 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -33,10 +33,7 @@ #ifndef CONTROL_TOOLBOX__PID_HPP_ #define CONTROL_TOOLBOX__PID_HPP_ -#include -#include -#include -#include +#include #include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" diff --git a/src/pid.cpp b/src/pid.cpp index 8aed6a38..26b0915b 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -38,9 +38,9 @@ #include #include -#include +#include +#include #include -#include #include "control_toolbox/pid.hpp" From 23a86a4f79c271cb2cd95e9d9dccc1baac089458 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 1 Jan 2025 22:07:53 +0000 Subject: [PATCH 20/21] Update comments and remove default constructor --- include/control_toolbox/pid.hpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index a71bae3b..db9dcf74 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -117,7 +117,6 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param i_max The max integral windup. * \param i_min The min integral windup. * - * \throws An std::invalid_argument exception is thrown if i_min > i_max */ 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), antiwindup_(true) @@ -134,16 +133,14 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param i_min The min integral windup. * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced * - * \throws An std::invalid_argument exception is thrown if i_min > i_max */ - Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) + Gains( + double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = 0.0, + bool antiwindup = false) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup) { } - // Default constructor - Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false) - { - } + double p_gain_; /**< Proportional gain. */ double i_gain_; /**< Integral gain. */ double d_gain_; /**< Derivative gain. */ From 15e132ade4853142e2ee77d9db3bcc3f993bb4fa Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 2 Jan 2025 10:08:10 +0000 Subject: [PATCH 21/21] Update docstring --- include/control_toolbox/pid.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index d5040c7b..b1f9929e 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -85,12 +85,12 @@ namespace control_toolbox \verbatim control_toolbox::Pid pid; pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3); - double position_desi = 0.5; + double position_desired = 0.5; ... rclcpp::Time last_time = get_clock()->now(); while (true) { rclcpp::Time time = get_clock()->now(); - double effort = pid.compute_command(position_desi - currentPosition(), time - last_time); + double effort = pid.compute_command(position_desired - currentPosition(), time - last_time); last_time = time; } \endverbatim