Skip to content

Commit

Permalink
init_pid --> initialize
Browse files Browse the repository at this point in the history
Co-authored-by: Sai Kishor Kothakota <[email protected]>
  • Loading branch information
christophfroehlich and saikishor authored Dec 13, 2024
1 parent 589a555 commit e18bc66
Show file tree
Hide file tree
Showing 6 changed files with 23 additions and 23 deletions.
8 changes: 4 additions & 4 deletions include/control_toolbox/pid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand All @@ -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);
}

/*!
Expand Down
8 changes: 4 additions & 4 deletions include/control_toolbox/pid_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
}

/*!
Expand All @@ -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();
}

/*!
Expand Down
2 changes: 1 addition & 1 deletion src/pid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
8 changes: 4 additions & 4 deletions src/pid_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::quiet_NaN();
Expand All @@ -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;
}
Expand All @@ -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));
Expand Down
16 changes: 8 additions & 8 deletions test/pid_parameters_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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));
Expand Down
4 changes: 2 additions & 2 deletions test/pid_publisher_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit e18bc66

Please sign in to comment.