Implemented a generic filters plugin

chained_filter_controller/CMakeLists.txt

@@ -0,0 +1,107 @@
+# cmake_minimum_required and project setup
+cmake_minimum_required(VERSION 3.8)
+project(chained_filter_controller)
+
+# Enable warnings
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Dependencies
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(filters REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
+# Generate parameters from YAML
+generate_parameter_library(
+  chained_filter_parameters
+  config/chained_filter_parameters.yaml
+)
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/include)
+
+# Library definition
+add_library(${PROJECT_NAME} SHARED
+  src/chained_filter.cpp
+  ${chained_filter_parameters_INTERFACE_SOURCES}
+)
+
+# Include paths
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${chained_filter_parameters_INCLUDE_DIRS}
+)
+
+# Dependencies
+ament_target_dependencies(${PROJECT_NAME}
+  controller_interface
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  pluginlib
+  filters
+  parameter_traits
+)
+
+# Export the plugin description
+pluginlib_export_plugin_description_file(controller_interface plugin_description.xml)
+
+# Export targets and dependencies
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  controller_interface
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  filters
+  parameter_traits
+)
+
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Installation
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  FILES plugin_description.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Testing support
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_chained_filter_controller
+    test/test_chained_filter.cpp)
+  target_link_libraries(test_chained_filter_controller
+    ${PROJECT_NAME})
+
+  ament_target_dependencies(test_chained_filter_controller
+    controller_interface
+    hardware_interface
+    pluginlib
+    rclcpp)
+endif()
+
+ament_package()

chained_filter_controller/config/chained_filter_parameters.yaml

@@ -0,0 +1,9 @@
+chained_filter:
+  input_interface:
+    type: string
+    default_value: "joint1/position"
+    description: "Name of the input state interface"
+  output_interface:
+    type: string
+    default_value: "filtered_position"
+    description: "Name of the output state interface"

chained_filter_controller/include/chained_filter_controller/chained_filter.hpp

@@ -0,0 +1,51 @@
+#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_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;
+
+      rclcpp::NodeOptions define_custom_node_options() const 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::unique_ptr<filters::FilterChain<double>> filter_;
+
+      double output_state_value_;
+  };
+} // namespace chained_filter_controller
+#endif

chained_filter_controller/package.xml

@@ -0,0 +1,27 @@
+<?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>TODO: Package description</description>
+  <maintainer email="[email protected]">ankur-u24</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>filters</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>hardware_interface</depend>
+  <depend>generate_parameter_library</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <pluginlib plugin_manifest="plugin_description.xml"/>
+  </export>
+</package>

chained_filter_controller/plugin_description.xml

@@ -0,0 +1,10 @@
+<library path="libchained_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>

chained_filter_controller/src/chained_filter.cpp

@@ -0,0 +1,107 @@
+// chained_filter.cpp (migrated from ROSCon 2024 workshop)
+
+#include "chained_filter_controller/chained_filter.hpp"
+
+
+
+#include <limits>
+#include <rclcpp/version.h>
+#include <pluginlib/class_list_macros.hpp>
+
+using namespace chained_filter;
+
+namespace chained_filter_controller
+{
+
+controller_interface::CallbackReturn ChainedFilter::on_init()
+{
+  try
+  {
+    param_listener_ = std::make_shared<chained_filter::ParamListener>(get_node());
+    params_ = param_listener_->get_params();
+    filter_ = std::make_unique<filters::FilterChain<double>>("double");
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration ChainedFilter::command_interface_configuration() const
+{
+  return {controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::InterfaceConfiguration ChainedFilter::state_interface_configuration() const
+{
+  return {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {params_.input_interface}};
+}
+
+controller_interface::CallbackReturn ChainedFilter::on_configure(
+  const rclcpp_lifecycle::State &)
+{
+  params_ = param_listener_->get_params();
+
+  if (!filter_->configure(
+        "filter_chain",
+        get_node()->get_node_logging_interface(),
+        get_node()->get_node_parameters_interface()))
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Failed to configure filter chain. Check the parameters for filters setup.");
+    return controller_interface::CallbackReturn::FAILURE;
+  }
+
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ChainedFilter::on_activate(
+  const rclcpp_lifecycle::State &)
+{
+  output_state_value_ = std::numeric_limits<double>::quiet_NaN();
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type ChainedFilter::update_and_write_commands(
+  const rclcpp::Time &, const rclcpp::Duration &)
+{
+  const auto sensor_value = state_interfaces_[0].get_value();
+
+  if (!std::isnan(sensor_value))
+  {
+    filter_->update(sensor_value, output_state_value_);
+  }
+
+  return controller_interface::return_type::OK;
+}
+
+std::vector<hardware_interface::StateInterface> ChainedFilter::on_export_state_interfaces()
+{
+  return {
+    hardware_interface::StateInterface(
+      get_node()->get_name(), params_.output_interface, &output_state_value_)};
+}
+
+controller_interface::return_type ChainedFilter::update_reference_from_subscribers(
+  const rclcpp::Time &, const rclcpp::Duration &)
+{
+  return controller_interface::return_type::OK;
+}
+
+rclcpp::NodeOptions ChainedFilter::define_custom_node_options() const
+{
+  return rclcpp::NodeOptions()
+    .allow_undeclared_parameters(true)
+    .automatically_declare_parameters_from_overrides(false);
+}
+
+}  // namespace chained_filter_controller
+
+PLUGINLIB_EXPORT_CLASS(
+  chained_filter_controller::ChainedFilter,
+  controller_interface::ChainableControllerInterface)