Move speed limiter from ros2_control repo

CMakeLists.txt

@@ -78,6 +78,26 @@ target_link_libraries(low_pass_filter PUBLIC
 )
 ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 
+generate_parameter_library(
+  rate_limiter_parameters
+  src/control_filters/rate_limiter_parameters.yaml
+)
+
+add_library(rate_limiter SHARED
+  src/control_filters/rate_limiter.cpp
+)
+target_compile_features(rate_limiter PUBLIC cxx_std_17)
+target_include_directories(rate_limiter PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+    $<INSTALL_INTERFACE:include/control_toolbox>
+)
+target_link_libraries(rate_limiter PUBLIC
+  rate_limiter_parameters
+  Eigen3::Eigen
+)
+ament_target_dependencies(rate_limiter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
 # Install pluginlib xml
 pluginlib_export_plugin_description_file(filters control_filters.xml)
 
@@ -92,6 +112,9 @@ if(BUILD_TESTING)
   ament_add_gmock(pid_tests test/pid_tests.cpp)
   target_link_libraries(pid_tests control_toolbox)
 
+  ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp)
+  target_link_libraries(rate_limiter_tests control_toolbox)
+
   ament_add_gtest(pid_parameters_tests test/pid_parameters_tests.cpp)
   target_link_libraries(pid_parameters_tests control_toolbox)
 
@@ -109,14 +132,20 @@ if(BUILD_TESTING)
   ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp)
   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})
+
+  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})
 endif()
 
 # Install
 install(
   DIRECTORY include/
   DESTINATION include/control_toolbox
 )
-install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters
+install(TARGETS control_toolbox
+  low_pass_filter low_pass_filter_parameters
+  rate_limiter rate_limiter_parameters
   EXPORT export_control_toolbox
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

control_filters.xml

@@ -15,4 +15,13 @@
         </description>
       </class>
     </library>
+    <library path="rate_limiter">
+      <class name="control_filters/RateLimiterDouble"
+             type="control_filters::RateLimiter&lt;double&gt;"
+             base_class_type="filters::FilterBase&lt;double&gt;">
+        <description>
+          This is a rate limiter working with a double value.
+        </description>
+      </class>
+    </library>
 </class_libraries>

include/control_filters/rate_limiter.hpp

@@ -0,0 +1,142 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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__RATE_LIMITER_HPP_
+#define CONTROL_FILTERS__RATE_LIMITER_HPP_
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <memory>
+#include <string>
+
+#include "filters/filter_base.hpp"
+
+#include "control_toolbox/rate_limiter.hpp"
+#include "rate_limiter_parameters.hpp"
+
+namespace control_filters
+{
+
+/***************************************************/
+/*! \class RateLimiter
+
+  \section Usage
+
+  The RateLimiter class is meant to be instantiated as a filter in
+  a controller but can also be used elsewhere.
+  For manual instantiation, you should first call configure()
+  (in non-realtime) and then call update() at every update step.
+
+*/
+/***************************************************/
+
+template <typename T>
+class RateLimiter : public filters::FilterBase<T>
+{
+public:
+  /*!
+   * \brief Configure the RateLimiter (access and process params).
+   */
+  bool configure() override;
+
+  /*!
+   * \brief Applies one step of the rate limiter
+   *
+   * \param data_in input to the limiter
+   * \param data_out limited output
+   *
+   * \returns false if filter is not configured, true otherwise
+   */
+  bool update(const T & data_in, T & data_out) override;
+
+private:
+  rclcpp::Clock::SharedPtr clock_;
+  std::shared_ptr<rclcpp::Logger> logger_;
+  std::shared_ptr<rate_limiter::ParamListener> parameter_handler_;
+  rate_limiter::Params parameters_;
+  std::shared_ptr<control_toolbox::RateLimiter<T>> limiter;
+
+  T v1, v0;
+};
+
+template <typename T>
+bool RateLimiter<T>::configure()
+{
+  clock_ = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  logger_.reset(
+    new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_)));
+
+  v0 = v1 = static_cast<T>(0.0);
+
+  // Initialize the parameters once
+  if (!parameter_handler_)
+  {
+    try
+    {
+      parameter_handler_ =
+        std::make_shared<rate_limiter::ParamListener>(this->params_interface_,
+                                                         this->param_prefix_);
+    }
+    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+      RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      return false;
+    }
+    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
+      RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      return false;
+    }
+  }
+  parameters_ = parameter_handler_->get_params();
+  limiter = std::make_shared<control_toolbox::RateLimiter<T>>(
+    parameters_.min_value, parameters_.max_value,
+    parameters_.min_first_derivative_neg, parameters_.max_first_derivative_pos,
+    parameters_.min_first_derivative_pos, parameters_.max_first_derivative_neg,
+    parameters_.min_second_derivative, parameters_.max_second_derivative
+  );
+
+  return true;
+}
+
+template <typename T>
+bool RateLimiter<T>::update(const T & data_in, T & data_out)
+{
+  if (!this->configured_ || !limiter)
+  {
+    throw std::runtime_error("Filter is not configured");
+  }
+
+  // Update internal parameters if required
+  if (parameter_handler_->is_old(parameters_))
+  {
+    parameters_ = parameter_handler_->get_params();
+    limiter->set_params(
+      parameters_.min_value, parameters_.max_value,
+      parameters_.min_first_derivative_neg, parameters_.max_first_derivative_pos,
+      parameters_.min_first_derivative_pos, parameters_.max_first_derivative_neg,
+      parameters_.min_second_derivative, parameters_.max_second_derivative
+    );
+  }
+  T v = data_in;
+  limiter->limit(v, v0, v1, static_cast<T>(parameters_.sampling_interval));
+  v1 = v0;
+  v0 = data_in;
+  data_out = v;
+  return true;
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__RATE_LIMITER_HPP_