Skip to content

Commit

Permalink
Make dt arguments const &
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed Dec 17, 2024
1 parent 6cf88ab commit 49e192f
Show file tree
Hide file tree
Showing 4 changed files with 21 additions and 20 deletions.
17 changes: 9 additions & 8 deletions include/control_toolbox/pid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions include/control_toolbox/pid_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
16 changes: 8 additions & 8 deletions src/pid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<double>(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<double>(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<double>(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<double>(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();
Expand Down
4 changes: 2 additions & 2 deletions src/pid_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -219,15 +219,15 @@ std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> 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);

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);
Expand Down

0 comments on commit 49e192f

Please sign in to comment.