From bb52b23ce8056ed76285e518731a29781e1cffca Mon Sep 17 00:00:00 2001 From: BenBean18 Date: Fri, 10 Mar 2023 17:42:26 -0500 Subject: [PATCH] Fix bug with NaN setpoint If the setpoint was set to NaN, the PID node would refuse to send a command even after setting a valid setpoint (and the node would have to be restarted). Now, if the setpoint is NaN, it is ignored and PID is disabled. --- src/pid.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/pid.cpp b/src/pid.cpp index f35e18d..40010cf 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -86,9 +86,14 @@ PidObject::PidObject() : error_(3, 0), filtered_error_(3, 0), error_deriv_(3, 0) void PidObject::setpointCallback(const std_msgs::Float64& setpoint_msg) { - setpoint_ = setpoint_msg.data; - last_setpoint_msg_time_ = ros::Time::now(); - new_state_or_setpt_ = true; + if (!isnan(setpoint_msg.data)) { + setpoint_ = setpoint_msg.data; + last_setpoint_msg_time_ = ros::Time::now(); + new_state_or_setpt_ = true; + } else { + ROS_ERROR_STREAM("PID node: received NaN as a setpoint, ignoring and disabling!"); + pid_enabled_ = false; + } } void PidObject::plantStateCallback(const std_msgs::Float64& state_msg)