Skip to content

Commit

Permalink
Rename save_iterm to save_i_term (#285)
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich authored Feb 13, 2025
1 parent 5cbdeae commit 155613f
Show file tree
Hide file tree
Showing 6 changed files with 32 additions and 32 deletions.
4 changes: 2 additions & 2 deletions include/control_toolbox/pid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -240,9 +240,9 @@ class Pid
/*!
* \brief Reset the state of this PID controller
*
* \param save_iterm boolean indicating if integral term is retained on reset()
* \param save_i_term boolean indicating if integral term is retained on reset()
*/
void reset(bool save_iterm);
void reset(bool save_i_term);

/*!
* \brief Clear the saved integrator output of this controller
Expand Down
14 changes: 7 additions & 7 deletions include/control_toolbox/pid_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,12 +130,12 @@ class PidROS
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param antiwindup antiwindup.
* \param save_iterm save integrator output between resets.
* \param save_i_term save integrator output between resets.
*
* \note New gains are not applied if i_min_ > i_max_
*/
void initialize_from_args(
double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm);
double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term);

/*!
* \brief Initialize the PID controller and set the parameters
Expand All @@ -145,12 +145,12 @@ class PidROS
* \param i_max The max integral windup.
* \param i_min The min integral windup.
* \param antiwindup antiwindup.
* \param save_iterm save integrator output between resets.
* \param save_i_term save integrator output between resets.
*
* \note New gains are not applied if i_min_ > i_max_
*/
[[deprecated("Use initialize_from_args() instead")]] void initPid(double p, double i, double d,
double i_max, double i_min, bool antiwindup, bool save_iterm);
double i_max, double i_min, bool antiwindup, bool save_i_term);

/*!
* \brief Initialize the PID controller based on already set parameters
Expand All @@ -167,16 +167,16 @@ class PidROS
/*!
* \brief Reset the state of this PID controller
*
* @note save_iterm parameter is read from ROS parameters
* @note save_i_term parameter is read from ROS parameters
*/
void reset();

/*!
* \brief Reset the state of this PID controller
*
* \param save_iterm boolean indicating if integral term is retained on reset()
* \param save_i_term boolean indicating if integral term is retained on reset()
*/
void reset(bool save_iterm);
void reset(bool save_i_term);

/*!
* \brief Set the PID error and compute the PID command with nonuniform time
Expand Down
4 changes: 2 additions & 2 deletions src/pid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ void Pid::reset()
reset(false);
}

void Pid::reset(bool save_iterm)
void Pid::reset(bool save_i_term)
{
p_error_last_ = 0.0;
p_error_ = 0.0;
Expand All @@ -107,7 +107,7 @@ void Pid::reset(bool save_iterm)
cmd_ = 0.0;

// Check to see if we should reset integral error here
if (!save_iterm) clear_saved_iterm();
if (!save_i_term) clear_saved_iterm();
}

void Pid::clear_saved_iterm()
Expand Down
20 changes: 10 additions & 10 deletions src/pid_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@ bool PidROS::initialize_from_ros_parameters()
all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min);

get_boolean_param(param_prefix_ + "antiwindup", antiwindup);
declare_param(param_prefix_ + "save_iterm", rclcpp::ParameterValue(false));
declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false));

if (all_params_available) {
set_parameter_event_callback();
Expand All @@ -202,7 +202,7 @@ void PidROS::initialize_from_args(double p, double i, double d, double i_max, do
}

void PidROS::initialize_from_args(double p, double i, double d, double i_max, double i_min,
bool antiwindup, bool save_iterm)
bool antiwindup, bool save_i_term)
{
if (i_min > i_max) {
RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains");
Expand All @@ -215,21 +215,21 @@ void PidROS::initialize_from_args(double p, double i, double d, double i_max, do
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));
declare_param(param_prefix_ + "save_iterm", rclcpp::ParameterValue(save_iterm));
declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term));

set_parameter_event_callback();
}
}

void PidROS::reset() {
bool save_iterm = false;
get_boolean_param(param_prefix_ + "save_iterm", save_iterm);
reset(save_iterm);
bool save_i_term = false;
get_boolean_param(param_prefix_ + "save_i_term", save_i_term);
reset(save_i_term);
}

void PidROS::reset(bool save_iterm)
void PidROS::reset(bool save_i_term)
{
pid_.reset(save_iterm);
pid_.reset(save_i_term);
}

std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> PidROS::get_pid_state_publisher()
Expand Down Expand Up @@ -401,8 +401,8 @@ void PidROS::initPid(
}

void PidROS::initPid(double p, double i, double d,
double i_max, double i_min, bool antiwindup, bool save_iterm) {
initialize_from_args(p, i, d, i_max, i_min, antiwindup, save_iterm);
double i_max, double i_min, bool antiwindup, bool save_i_term) {
initialize_from_args(p, i, d, i_max, i_min, antiwindup, save_i_term);
}

bool PidROS::initPid() {
Expand Down
16 changes: 8 additions & 8 deletions test/pid_ros_parameters_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,9 @@ void check_set_parameters(
const double I_MAX = 10.0;
const double I_MIN = -10.0;
const bool ANTIWINDUP = true;
const bool SAVE_ITERM = true;
const bool SAVE_I_TERM = true;

ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM));
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM));

rclcpp::Parameter param;

Expand All @@ -82,8 +82,8 @@ void check_set_parameters(
ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param));
ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);

ASSERT_TRUE(node->get_parameter(prefix + "save_iterm", param));
ASSERT_EQ(param.get_value<bool>(), SAVE_ITERM);
ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param));
ASSERT_EQ(param.get_value<bool>(), SAVE_I_TERM);

// check gains were set
control_toolbox::Pid::Gains gains = pid.get_gains();
Expand Down Expand Up @@ -219,9 +219,9 @@ TEST(PidParametersTest, SetParametersTest)
const double I_MAX = 10.0;
const double I_MIN = -10.0;
const bool ANTIWINDUP = true;
const bool SAVE_ITERM = false;
const bool SAVE_I_TERM = false;

pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM);
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM);

rcl_interfaces::msg::SetParametersResult set_result;

Expand All @@ -242,7 +242,7 @@ TEST(PidParametersTest, SetParametersTest)
ASSERT_TRUE(set_result.successful);
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
ASSERT_TRUE(set_result.successful);
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_iterm", SAVE_ITERM)));
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM)));
ASSERT_TRUE(set_result.successful);

// process callbacks
Expand Down Expand Up @@ -344,7 +344,7 @@ TEST(PidParametersTest, GetParametersTest)
ASSERT_TRUE(node->get_parameter("antiwindup", param));
ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);

ASSERT_TRUE(node->get_parameter("save_iterm", param));
ASSERT_TRUE(node->get_parameter("save_i_term", param));
ASSERT_EQ(param.get_value<bool>(), false);
}

Expand Down
6 changes: 3 additions & 3 deletions test/pid_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -351,7 +351,7 @@ TEST(CommandTest, integralOnlyTest)
cmd = pid.compute_command(-0.5, 1.0);
// Then expect command = error
EXPECT_EQ(-0.5, cmd);
// after reset without argument (save_iterm=false)
// after reset without argument (save_i_term=false)
// we expect the command to be 0 if update is called error = 0
pid.reset();
cmd = pid.compute_command(0.0, 1.0);
Expand All @@ -361,7 +361,7 @@ TEST(CommandTest, integralOnlyTest)
cmd = pid.compute_command(-0.5, 1.0);
// Then expect command = error
EXPECT_EQ(-0.5, cmd);
// after reset with argument (save_iterm=false)
// after reset with argument (save_i_term=false)
// we expect the command to be 0 if update is called error = 0
pid.reset(false);
cmd = pid.compute_command(0.0, 1.0);
Expand All @@ -371,7 +371,7 @@ TEST(CommandTest, integralOnlyTest)
cmd = pid.compute_command(-0.5, 1.0);
// Then expect command = error
EXPECT_EQ(-0.5, cmd);
// after reset with save_iterm=true
// after reset with save_i_term=true
// we expect still the same command if update is called error = 0
pid.reset(true);
cmd = pid.compute_command(0.0, 1.0);
Expand Down

0 comments on commit 155613f

Please sign in to comment.