Don't return cmd if called with dt=0 or garbage (backport #326) (#412)

Co-authored-by: Sai Kishor Kothakota <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>

Author: 3 people

control_toolbox/src/pid.cpp

@@ -39,6 +39,7 @@
 #include <algorithm>
 #include <cmath>
 #include <iostream>
+#include <limits>
 #include <stdexcept>
 
 #include "control_toolbox/pid.hpp"
@@ -306,7 +307,19 @@ double Pid::compute_command(double error, const double & dt_s)
 {
   if (is_zero(dt_s))
   {
-    return 0.0;
+    // don't update anything
+    return cmd_;
+  }
+  else if (dt_s < 0.0)
+  {
+    throw std::invalid_argument("Pid is called with negative dt");
+  }
+
+  // don't reset controller but return NaN
+  if (!std::isfinite(error))
+  {
+    std::cout << "Received a non-finite error value\n";
+    return cmd_ = std::numeric_limits<double>::quiet_NaN();
   }
 
   // Calculate the derivative error
@@ -350,7 +363,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
 {
   if (is_zero(dt_s))
   {
-    // Don't update anything
+    // don't update anything
     return cmd_;
   }
   else if (dt_s < 0.0)
@@ -364,11 +377,11 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   p_error_ = error;      // This is error = target - state
   d_error_ = error_dot;  // This is the derivative of error
 
-  // Don't reset controller but return NaN
+  // don't reset controller but return NaN
   if (!std::isfinite(error) || !std::isfinite(error_dot))
   {
     std::cout << "Received a non-finite error/error_dot value\n";
-    return cmd_ = std::numeric_limits<double>::quiet_NaN();
+    return cmd_ = std::numeric_limits<float>::quiet_NaN();
   }
 
   // Calculate proportional contribution to command

control_toolbox/test/pid_tests.cpp

@@ -897,6 +897,29 @@ TEST(CommandTest, timeArgumentTest)
   EXPECT_EQ(cmd1, cmd2);
   EXPECT_EQ(cmd1, cmd3);
   EXPECT_EQ(cmd1, cmd4);
+
+  // call with dt=0, nothing should change
+  double pe, ie1, de, ie2;
+  pid1.get_current_pid_errors(pe, ie1, de);
+  cmd1 = pid1.compute_command(-0.5, 0.0, 0.0);
+  pid1.get_current_pid_errors(pe, ie2, de);
+  EXPECT_EQ(-2.0, cmd1);
+  EXPECT_EQ(ie1, ie2);
+  // should throw if called with negative dt
+  EXPECT_THROW(cmd1 = pid1.compute_command(-0.5, 0.0, -1.0), std::invalid_argument);
+  // call with nan, should return NaN but not reset internal states
+  cmd1 = pid1.compute_command(std::numeric_limits<double>::quiet_NaN(), 0.0, 1.0);
+  cmd3 = pid1.get_current_cmd();
+  EXPECT_FALSE(std::isfinite(cmd1));
+  EXPECT_FALSE(std::isfinite(cmd3));
+  pid1.get_current_pid_errors(pe, ie2, de);
+  EXPECT_EQ(ie1, ie2);
+  cmd2 = pid2.compute_command(-0.5, std::numeric_limits<double>::quiet_NaN(), 1.0);
+  cmd4 = pid2.get_current_cmd();
+  EXPECT_FALSE(std::isfinite(cmd2));
+  EXPECT_FALSE(std::isfinite(cmd4));
+  pid1.get_current_pid_errors(pe, ie2, de);
+  EXPECT_EQ(ie1, ie2);
 }
 
 int main(int argc, char ** argv)