diff --git a/chained_filter_controller/CMakeLists.txt b/chained_filter_controller/CMakeLists.txt new file mode 100644 index 0000000000..d42178f079 --- /dev/null +++ b/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 + $ + $ + ${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() diff --git a/chained_filter_controller/config/chained_filter_parameters.yaml b/chained_filter_controller/config/chained_filter_parameters.yaml new file mode 100644 index 0000000000..f8bd984fca --- /dev/null +++ b/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" \ No newline at end of file diff --git a/chained_filter_controller/include/chained_filter_controller/chained_filter.hpp b/chained_filter_controller/include/chained_filter_controller/chained_filter.hpp new file mode 100644 index 0000000000..0987258e31 --- /dev/null +++ b/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 +#include +#include + +#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 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 param_listener_; + chained_filter::Params params_; + std::unique_ptr> filter_; + + double output_state_value_; + }; +} // namespace chained_filter_controller +#endif \ No newline at end of file diff --git a/chained_filter_controller/package.xml b/chained_filter_controller/package.xml new file mode 100644 index 0000000000..058fa4d391 --- /dev/null +++ b/chained_filter_controller/package.xml @@ -0,0 +1,27 @@ + + + + chained_filter_controller + 0.0.0 + TODO: Package description + ankur-u24 + TODO: License declaration + + ament_cmake + + controller_interface + filters + pluginlib + rclcpp + rclcpp_lifecycle + hardware_interface + generate_parameter_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/chained_filter_controller/plugin_description.xml b/chained_filter_controller/plugin_description.xml new file mode 100644 index 0000000000..b2c239b95a --- /dev/null +++ b/chained_filter_controller/plugin_description.xml @@ -0,0 +1,10 @@ + + + + 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. + + + \ No newline at end of file diff --git a/chained_filter_controller/src/chained_filter.cpp b/chained_filter_controller/src/chained_filter.cpp new file mode 100644 index 0000000000..bb98e8c569 --- /dev/null +++ b/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 +#include +#include + +using namespace chained_filter; + +namespace chained_filter_controller +{ + +controller_interface::CallbackReturn ChainedFilter::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + filter_ = std::make_unique>("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::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 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) diff --git a/chained_filter_controller/test/test_chained_filter.cpp b/chained_filter_controller/test/test_chained_filter.cpp new file mode 100644 index 0000000000..cb5aa06c8a --- /dev/null +++ b/chained_filter_controller/test/test_chained_filter.cpp @@ -0,0 +1,93 @@ +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "controller_interface/controller_interface.hpp" +#include "chained_filter_controller/chained_filter.hpp" + +using chained_filter_controller::ChainedFilter; +using controller_interface::CallbackReturn; + +class ChainedFilterControllerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + controller_ = std::make_shared(); + } + + std::shared_ptr controller_; +}; + +TEST_F(ChainedFilterControllerTest, InitReturnsSuccess) +{ + auto result = controller_->init("chained_filter", "test_ns", 3, "", rclcpp::NodeOptions()); + EXPECT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(ChainedFilterControllerTest, ConfigureReturnsFailureWithNoParams) +{ + // Initialize the controller (should set up internal node) + ASSERT_EQ( + controller_->init( + "chained_filter", // controller name + "test_ns", // namespace + 0, // intra-process (0 = disabled) + "test_node", // RCL node name + rclcpp::NodeOptions()), // node options + controller_interface::return_type::OK); + + + // We do NOT declare any filter parameters — this mimics a misconfigured user setup + // The expectation is that configuration fails gracefully (not segfault) + + auto result = controller_->on_configure(rclcpp_lifecycle::State{}); + EXPECT_EQ(result, controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(ChainedFilterControllerTest, ActivateReturnsSuccessWithoutError) +{ + auto init_result = controller_->init("chained_filter", "test_ns", 3, "", rclcpp::NodeOptions()); + ASSERT_EQ(init_result, controller_interface::return_type::OK); + + auto configure_result = controller_->on_configure(rclcpp_lifecycle::State()); + EXPECT_EQ(configure_result, CallbackReturn::SUCCESS); // Expected because no params loaded + + auto activate_result = controller_->on_activate(rclcpp_lifecycle::State()); + EXPECT_EQ(activate_result, CallbackReturn::SUCCESS); +} + +TEST_F(ChainedFilterControllerTest, DeactivateDoesNotCrash) +{ + EXPECT_NO_THROW({ + controller_->on_deactivate(rclcpp_lifecycle::State()); + }); +} + +TEST_F(ChainedFilterControllerTest, CleanupDoesNotCrash) +{ + EXPECT_NO_THROW({ + controller_->on_cleanup(rclcpp_lifecycle::State()); + }); +} + +TEST_F(ChainedFilterControllerTest, ShutdownDoesNotCrash) +{ + EXPECT_NO_THROW({ + controller_->on_shutdown(rclcpp_lifecycle::State()); + }); +} + + +int main(int argc, char **argv) +{ + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Run all the tests + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + + // Shutdown ROS 2 + rclcpp::shutdown(); + return result; +} \ No newline at end of file