Add a generic chained_filter_controller (backport #1634)

chained_filter_controller/CMakeLists.txt

@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 3.10)
+project(chained_filter_controller)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+# Dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  filters
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+# Generate parameters from YAML
+generate_parameter_library(
+  chained_filter_parameters
+  src/chained_filter_parameters.yaml
+)
+
+# Library definition
+add_library(${PROJECT_NAME} SHARED
+  src/chained_filter.cpp
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+
+# Dependencies
+target_link_libraries(${PROJECT_NAME}
+  PUBLIC
+    chained_filter_parameters
+    controller_interface::controller_interface
+    hardware_interface::hardware_interface
+    filters::filter_chain
+    pluginlib::pluginlib
+    rclcpp::rclcpp
+    rclcpp_lifecycle::rclcpp_lifecycle
+)
+
+# Export the plugin description
+pluginlib_export_plugin_description_file(controller_interface plugin_description.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_rostest_with_parameters_gmock(test_chained_filter_controller
+    test/test_chained_filter.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_chained_filter.yaml
+  )
+  target_link_libraries(test_chained_filter_controller
+    ${PROJECT_NAME}
+  )
+
+  add_rostest_with_parameters_gmock(test_multiple_chained_filter_controller
+    test/test_multiple_chained_filter.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_multiple_chained_filter.yaml
+  )
+  target_link_libraries(test_multiple_chained_filter_controller
+    ${PROJECT_NAME}
+  )
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_chained_filter_controller test/test_load_chained_filter_controller.cpp)
+  target_link_libraries(test_load_chained_filter_controller
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  chained_filter_parameters
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Export targets and dependencies
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

chained_filter_controller/doc/userdoc.rst

@@ -0,0 +1,27 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/chained_filter_controller/doc/userdoc.rst
+
+.. _chained_filter_controller_userdoc:
+
+Chained Filter Controller
+--------------------------------
+This controller wraps around ``filter_chain`` library from the `filters <https://index.ros.org/p/filters/#humble>`__ package. It allows to chain multiple filters together, where the output of one filter is the input of the next one. The controller can be used to apply a sequence of filters to a single interface, the same filter chain to multiple interfaces, or different filter chains to different interfaces.
+
+
+Parameters
+^^^^^^^^^^^
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/chained_filter_controller/src/chained_filter_parameters.yaml>`_ contains descriptions for all the parameters used by the controller.
+
+
+Full list of parameters:
+
+.. generate_parameter_library_details:: ../src/chained_filter_parameters.yaml
+
+An example parameter file for this controller can be found in `the test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/chained_filter_controller/test/config/test_chained_filter.yaml>`_ for a single interface:
+
+.. literalinclude:: ../test/config/test_chained_filter.yaml
+   :language: yaml
+
+or for `multiple interfaces <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/chained_filter_controller/test/config/test_multiple_chained_filter.yaml>`_:
+
+.. literalinclude:: ../test/config/test_multiple_chained_filter.yaml
+   :language: yaml

chained_filter_controller/include/chained_filter_controller/chained_filter.hpp

@@ -0,0 +1,63 @@
+// Copyright 2025 ros2_control PMC
+//
+// 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 CHAINED_FILTER_CONTROLLER__CHAINED_FILTER_HPP_
+#define CHAINED_FILTER_CONTROLLER__CHAINED_FILTER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "filters/filter_chain.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+
+#include "chained_filter_controller/chained_filter_parameters.hpp"
+
+namespace chained_filter_controller
+{
+
+class ChainedFilter : public controller_interface::ChainableControllerInterface
+{
+public:
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  std::shared_ptr<chained_filter::ParamListener> param_listener_;
+  chained_filter::Params params_;
+
+  std::vector<std::unique_ptr<filters::FilterChain<double>>> filters_;
+  std::vector<double> output_state_values_;
+};
+}  // namespace chained_filter_controller
+#endif  // CHAINED_FILTER_CONTROLLER__CHAINED_FILTER_HPP_

chained_filter_controller/package.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>chained_filter_controller</name>
+  <version>0.0.0</version>
+  <description>ros2_controller for configuring filter chains</description>
+
+  <maintainer email="[email protected]">Bence Magyar</maintainer>
+  <maintainer email="[email protected]">Denis Štogl</maintainer>
+  <maintainer email="[email protected]">Christoph Froehlich</maintainer>
+  <maintainer email="[email protected]">Sai Kishor Kothakota</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://control.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+
+  <author email="[email protected]">Bence Magyar</author>
+  <author email="[email protected]">Christoph Froehlich</author>
+  <author email="[email protected]">ankur-u24</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>generate_parameter_library</build_depend>
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>controller_interface</depend>
+  <depend>filters</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>hardware_interface</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

chained_filter_controller/plugin_description.xml

@@ -0,0 +1,10 @@
+<library path="chained_filter_controller">
+  <class name="chained_filter_controller/ChainedFilter"
+         type="chained_filter_controller::ChainedFilter"
+         base_class_type="controller_interface::ChainableControllerInterface">
+    <description>
+      A chainable ROS 2 controller that applies a sequence of filters to a state interface using the filters package,
+      and exports the filtered output as a new state interface.
+    </description>
+  </class>
+</library>