Fix bug in rate_limiter filter and add more tests (#237)

ros-controls · Dec 7, 2024 · 93955a8 · 93955a8
1 parent 3a3e36e
commit 93955a8
Show file tree

Hide file tree

Showing 9 changed files with 320 additions and 28 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -81,6 +81,7 @@ ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPEN
 generate_parameter_library(
   rate_limiter_parameters
   src/control_filters/rate_limiter_parameters.yaml
+  include/control_filters/custom_validators.hpp
 )
 
 add_library(rate_limiter SHARED
@@ -133,6 +134,12 @@ if(BUILD_TESTING)
   target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters)
   ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 
+  add_rostest_with_parameters_gmock(test_rate_limiter test/control_filters/test_rate_limiter.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_rate_limiter_parameters.yaml
+  )
+  target_link_libraries(test_rate_limiter rate_limiter rate_limiter_parameters)
+  ament_target_dependencies(test_rate_limiter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
   ament_add_gmock(test_load_rate_limiter test/control_filters/test_load_rate_limiter.cpp)
   target_link_libraries(test_load_rate_limiter rate_limiter rate_limiter_parameters)
   ament_target_dependencies(test_load_rate_limiter ${CONTROL_FILTERS_INCLUDE_DEPENDS})

diff --git a/include/control_filters/custom_validators.hpp b/include/control_filters/custom_validators.hpp
@@ -0,0 +1,58 @@
+// Copyright 2024 AIT - Austrian Institute of Technology GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_
+#define CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_
+
+#include <fmt/core.h>
+
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rsl/parameter_validators.hpp>
+#include <tl_expected/expected.hpp>
+
+namespace control_filters {
+
+/**
+ * @brief gt_eq, but check only if the value is not NaN
+ */
+template <typename T>
+tl::expected<void, std::string> gt_eq_or_nan(
+    rclcpp::Parameter const& parameter, T expected_value) {
+  auto param_value = parameter.as_double();
+  if (!std::isnan(param_value)) {
+    // check only if the value is not NaN
+    return rsl::gt_eq<T>(parameter, expected_value);
+  }
+  return {};
+}
+
+/**
+ * @brief lt_eq, but check only if the value is not NaN
+ */
+template <typename T>
+tl::expected<void, std::string> lt_eq_or_nan(
+    rclcpp::Parameter const& parameter, T expected_value) {
+  auto param_value = parameter.as_double();
+  if (!std::isnan(param_value)) {
+    // check only if the value is not NaN
+    return rsl::lt_eq<T>(parameter, expected_value);
+  }
+  return {};
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_
diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp
@@ -17,6 +17,7 @@
 
 #include <Eigen/Dense>
 #include <cmath>
+#include <limits>
 #include <memory>
 #include <string>
 
@@ -77,7 +78,7 @@ bool RateLimiter<T>::configure()
   logger_.reset(
     new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_)));
 
-  v0 = v1 = static_cast<T>(0.0);
+  v0 = v1 = std::numeric_limits<T>::quiet_NaN();
 
   // Initialize the parameters once
   if (!parameter_handler_)
@@ -130,9 +131,15 @@ bool RateLimiter<T>::update(const T & data_in, T & data_out)
     );
   }
   T v = data_in;
+  if (std::isnan(v0))
+  {
+    // not initialized yet
+    v1 = v0 = v;
+  }
   limiter->limit(v, v0, v1, static_cast<T>(parameters_.sampling_interval));
+  // shift the values for the next update call
   v1 = v0;
-  v0 = data_in;
+  v0 = v;  // use the limited value
   data_out = v;
   return true;
 }

diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp
@@ -56,14 +56,14 @@ class RateLimiter
    *
    */
   RateLimiter(
-    T min_value = std::numeric_limits<double>::quiet_NaN(),
-    T max_value = std::numeric_limits<double>::quiet_NaN(),
-    T min_first_derivative_neg = std::numeric_limits<double>::quiet_NaN(),
-    T max_first_derivative_pos = std::numeric_limits<double>::quiet_NaN(),
-    T min_first_derivative_pos = std::numeric_limits<double>::quiet_NaN(),
-    T max_first_derivative_neg = std::numeric_limits<double>::quiet_NaN(),
-    T min_second_derivative = std::numeric_limits<double>::quiet_NaN(),
-    T max_second_derivative = std::numeric_limits<double>::quiet_NaN());
+    T min_value = std::numeric_limits<T>::quiet_NaN(),
+    T max_value = std::numeric_limits<T>::quiet_NaN(),
+    T min_first_derivative_neg = std::numeric_limits<T>::quiet_NaN(),
+    T max_first_derivative_pos = std::numeric_limits<T>::quiet_NaN(),
+    T min_first_derivative_pos = std::numeric_limits<T>::quiet_NaN(),
+    T max_first_derivative_neg = std::numeric_limits<T>::quiet_NaN(),
+    T min_second_derivative = std::numeric_limits<T>::quiet_NaN(),
+    T max_second_derivative = std::numeric_limits<T>::quiet_NaN());
 
   /**
    * \brief Limit the value and first_derivative
@@ -123,14 +123,14 @@ class RateLimiter
    * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used
    */
   void set_params(
-    T min_value = std::numeric_limits<double>::quiet_NaN(),
-    T max_value = std::numeric_limits<double>::quiet_NaN(),
-    T min_first_derivative_neg = std::numeric_limits<double>::quiet_NaN(),
-    T max_first_derivative_pos = std::numeric_limits<double>::quiet_NaN(),
-    T min_first_derivative_pos = std::numeric_limits<double>::quiet_NaN(),
-    T max_first_derivative_neg = std::numeric_limits<double>::quiet_NaN(),
-    T min_second_derivative = std::numeric_limits<double>::quiet_NaN(),
-    T max_second_derivative = std::numeric_limits<double>::quiet_NaN());
+    T min_value = std::numeric_limits<T>::quiet_NaN(),
+    T max_value = std::numeric_limits<T>::quiet_NaN(),
+    T min_first_derivative_neg = std::numeric_limits<T>::quiet_NaN(),
+    T max_first_derivative_pos = std::numeric_limits<T>::quiet_NaN(),
+    T min_first_derivative_pos = std::numeric_limits<T>::quiet_NaN(),
+    T max_first_derivative_neg = std::numeric_limits<T>::quiet_NaN(),
+    T min_second_derivative = std::numeric_limits<T>::quiet_NaN(),
+    T max_second_derivative = std::numeric_limits<T>::quiet_NaN());
 
 private:
   // Enable/Disable value/first_derivative/second_derivative limits:
@@ -139,18 +139,18 @@ class RateLimiter
   bool has_second_derivative_limits_ = true;
 
   // value limits:
-  T min_value_ = std::numeric_limits<double>::quiet_NaN();
-  T max_value_ = std::numeric_limits<double>::quiet_NaN();
+  T min_value_ = std::numeric_limits<T>::quiet_NaN();
+  T max_value_ = std::numeric_limits<T>::quiet_NaN();
 
   // first_derivative limits:
-  T min_first_derivative_neg_ = std::numeric_limits<double>::quiet_NaN();
-  T max_first_derivative_pos_ = std::numeric_limits<double>::quiet_NaN();
-  T min_first_derivative_pos_ = std::numeric_limits<double>::quiet_NaN();
-  T max_first_derivative_neg_ = std::numeric_limits<double>::quiet_NaN();
+  T min_first_derivative_neg_ = std::numeric_limits<T>::quiet_NaN();
+  T max_first_derivative_pos_ = std::numeric_limits<T>::quiet_NaN();
+  T min_first_derivative_pos_ = std::numeric_limits<T>::quiet_NaN();
+  T max_first_derivative_neg_ = std::numeric_limits<T>::quiet_NaN();
 
   // second_derivative limits:
-  T min_second_derivative_ = std::numeric_limits<double>::quiet_NaN();
-  T max_second_derivative_ = std::numeric_limits<double>::quiet_NaN();
+  T min_second_derivative_ = std::numeric_limits<T>::quiet_NaN();
+  T max_second_derivative_ = std::numeric_limits<T>::quiet_NaN();
 };
 
 template <typename T>

diff --git a/src/control_filters/rate_limiter_parameters.yaml b/src/control_filters/rate_limiter_parameters.yaml
@@ -3,38 +3,70 @@ rate_limiter:
     type: double,
     description: "Sampling interval in seconds",
     validation: {
-      gt<>: [0.0]
+      gt<>: [0.0],
     },
   }
   max_value: {
     type: double,
     default_value: .NAN,
+    description: "Maximum value, e.g. [m/s]",
+    validation: {
+      "control_filters::gt_eq_or_nan<>": [0.0]
+    },
   }
   min_value: {
     type: double,
     default_value: .NAN,
+    description: "Minimum value, e.g. [m/s]",
+    validation: {
+      "control_filters::lt_eq_or_nan<>": [0.0]
+    },
   }
   max_first_derivative_pos: {
     type: double,
     default_value: .NAN,
+    description: "Maximum value of the first derivative if **value** is positive, e.g. [m/s^2]",
+    validation: {
+      "control_filters::gt_eq_or_nan<>": [0.0]
+    },
   }
   min_first_derivative_pos: {
     type: double,
     default_value: .NAN,
+    description: "Minimum value of the first derivative if **value** is positive, e.g. [m/s^2]",
+    validation: {
+      "control_filters::lt_eq_or_nan<>": [0.0]
+    },
   }
   max_first_derivative_neg: {
     type: double,
     default_value: .NAN,
+    description: "Maximum value of the first derivative if **value** is negative, e.g. [m/s^2]",
+    validation: {
+      "control_filters::gt_eq_or_nan<>": [0.0]
+    },
   }
   min_first_derivative_neg: {
     type: double,
     default_value: .NAN,
+    description: "Minimum value of the first derivative if **value** is negative, e.g. [m/s^2]",
+    validation: {
+      "control_filters::lt_eq_or_nan<>": [0.0]
+    },
   }
   max_second_derivative: {
     type: double,
     default_value: .NAN,
+    description: "Maximum value of the second derivative, e.g. [m/s^3]",
+    validation: {
+      "control_filters::gt_eq_or_nan<>": [0.0]
+    },
   }
   min_second_derivative: {
     type: double,
     default_value: .NAN,
+    description: "Minimum value of the second derivative, e.g. [m/s^3]",
+    validation: {
+      "control_filters::lt_eq_or_nan<>": [0.0]
+    },
   }
diff --git a/test/control_filters/test_load_rate_limiter.cpp b/test/control_filters/test_load_rate_limiter.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright 2024 AIT - Austrian Institute of Technology GmbH
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

diff --git a/test/control_filters/test_rate_limiter.cpp b/test/control_filters/test_rate_limiter.cpp
@@ -0,0 +1,101 @@
+// Copyright 2024 AIT - Austrian Institute of Technology GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_rate_limiter.hpp"
+
+TEST_F(RateLimiterTest, TestRateLimiterAllParameters)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::RateLimiter<double>>();
+
+    // should allow configuration and find parameters in sensor_filter_chain param namespace
+    ASSERT_TRUE(filter_->configure("", "TestRateLimiter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+
+    // change a parameter
+    node_->set_parameter(rclcpp::Parameter("sampling_interval", 0.5));
+    // accept second call to configure with valid parameters to already configured filter
+    ASSERT_TRUE(filter_->configure("", "TestRateLimiter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+}
+
+
+TEST_F(RateLimiterTest, TestRateLimiterMissingParameter)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::RateLimiter<double>>();
+
+    // should deny configuration as sampling_interval is missing
+    ASSERT_FALSE(filter_->configure("", "TestRateLimiter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+}
+
+TEST_F(RateLimiterTest, TestRateLimiterInvalidThenFixedParameter)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::RateLimiter<double>>();
+
+    // should deny configuration as sampling_interval is invalid
+    ASSERT_FALSE(filter_->configure("", "TestRateLimiter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+
+    // fix the param
+    node_->set_parameter(rclcpp::Parameter("sampling_interval", 1.0));
+     // should allow configuration and pass second call to unconfigured filter
+    ASSERT_TRUE(filter_->configure("", "TestRateLimiter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+}
+
+TEST_F(RateLimiterTest, TestRateLimiterThrowsUnconfigured)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::RateLimiter<double>>();
+    double in, out;
+    ASSERT_THROW(filter_->update(in, out), std::runtime_error);
+}
+
+TEST_F(RateLimiterTest, TestRateLimiterCompute)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::RateLimiter<double>>();
+
+    ASSERT_TRUE(filter_->configure("", "TestRateLimiter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+
+    double in = 10.0, out;
+    for (int i = 0; i < 10; i++)
+    {
+        ASSERT_NO_THROW(filter_->update(in, out));
+        // no change
+        EXPECT_THAT(out, ::testing::DoubleEq(in));
+    }
+    in = 0.0;
+    // takes 14 steps to reach 0 (first (10.0/1.0) plus second derivative limits)
+    for (int i = 0; i < 14; i++)
+    {
+        ASSERT_NO_THROW(filter_->update(in, out));
+        EXPECT_THAT(out, ::testing::Not(::testing::DoubleNear(in, 1e-6))) << "i=" << i;
+    }
+    ASSERT_NO_THROW(filter_->update(in, out));
+    EXPECT_THAT(out, ::testing::DoubleNear(in, 1e-6));
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}