ROS 2 twist mux controller #14

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged

rafal-gorecki merged 15 commits into jazzy from ros2-velocity-input-controller

Aug 6, 2025

low_pass_filter/CMakeLists.txt

-Original file line number
+Diff line change
@@ -0,0 +1,51 @@
+    cmake_minimum_required(VERSION 3.11)
+    project(low_pass_filter)
+    if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+      add_compile_options(-Wall -Wextra -Wpedantic)
+    endif()
+    set(PACKAGE_DEPENDENCIES
+        ament_cmake
+        control_toolbox
+        controller_interface
+        generate_parameter_library
+        hardware_interface
+        pluginlib
+        rclcpp
+        rclcpp_lifecycle
+        realtime_tools)
+    foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
+      find_package(${PACKAGE} REQUIRED)
+    endforeach()
+    generate_parameter_library(low_pass_filter_parameters
+                               src/low_pass_filter_parameters.yaml)
+    add_library(low_pass_filter SHARED src/low_pass_filter.cpp)
+    target_include_directories(
+      low_pass_filter PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                             $<INSTALL_INTERFACE:include/low_pass_filter>)
+    target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters)
+    ament_target_dependencies(low_pass_filter PUBLIC ${PACKAGE_DEPENDENCIES})
+    # Causes the visibility macros to use dllexport rather than dllimport, which is
+    # appropriate when building the dll but not consuming it.
+    target_compile_definitions(low_pass_filter
+                               PRIVATE "low_pass_filter_BUILDING_DLL")
+    pluginlib_export_plugin_description_file(controller_interface
+                                             low_pass_filter_plugin.xml)
+    install(DIRECTORY include/ DESTINATION include/low_pass_filter)
+    install(
+      TARGETS ${PROJECT_NAME} low_pass_filter_parameters
+      EXPORT export_${PROJECT_NAME}
+      RUNTIME DESTINATION bin
+      LIBRARY DESTINATION lib
+      ARCHIVE DESTINATION lib)
+    ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+    ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+    ament_package()

low_pass_filter/README.md

-Original file line number
+Diff line change
@@ -0,0 +1,38 @@
+    # Low Pass Filter
+    The `LowPassFilter` is a ROS 2 chainable controller designed to apply a low-pass filter to state interfaces, smoothing noisy signals while maintaining real-time performance.
+    ## Publishers
+    - `interface_values` [*control_msgs::msg::DynamicInterfaceValues*]: Publishes the filtered state interface values.
+    ## Parameters
+    - `state_interface_prefix` [*string*, default: **""**]: Prefix to be prepended to the state interface names.
+    - `state_interface_names` [*string_array*, default: **[]**]: Names of state interfaces to be filtered.
+    - `sampling_frequency` [*double*, default: **100.0**]: Filter sampling frequency in Hz.
+    - `damping_frequency` [*double*, default: **0.1**]: Filter damping frequency in Hz (Is it?).
+    - `damping_intensity` [*double*, default: **0.707**]: Defines how sharp the filter is. 0.707 is the default value for a Butterworth filter.
+    - `publish_interface_values` [*bool*, default: **false**]: If true, the state interface values will be published to a topic.
+    - `zero_threshold` [*double*, default: **0.0025**]: If the filtered value is below this threshold, it will be set to zero.
+    ## Interfaces
+    ### Exported State Interfaces
+    - `{node_name}/{state_interface_prefix}/{state_interface_name}`: Filtered data.
+    ## Example Usage
+    ```yaml
+    low_pass_filter:
+      ros__parameters:
+        state_interface_prefix: <namespace>/imu
+        state_interface_names: [angular_velocity.z]
+        sampling_frequency: 100.0
+        damping_frequency: 0.1
+        damping_intensity: 15.0
+        publish_interface_values: false
+        zero_threshold: 0.0025
+    ```

low_pass_filter/include/low_pass_filter/low_pass_filter.hpp

-Original file line number
+Diff line change
@@ -0,0 +1,75 @@
+    // Copyright 2025 Husarion sp. z o.o.
+    //
+    // 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 LOW_PASS_FILTER_LOW_PASS_FILTER
+    #define LOW_PASS_FILTER_LOW_PASS_FILTER
+    #include <control_toolbox/low_pass_filter.hpp>
+    #include <controller_interface/chainable_controller_interface.hpp>
+    #include <rclcpp/rclcpp.hpp>
+    #include <rclcpp_lifecycle/state.hpp>
+    #include <realtime_tools/realtime_publisher.hpp>
+    #include <control_msgs/msg/dynamic_interface_values.hpp>
+    #include "low_pass_filter/low_pass_filter_parameters.hpp"
+    namespace low_pass_filter
+    {
+    using DynamicInterfaceValuesMsg = control_msgs::msg::DynamicInterfaceValues;
+    class LowPassFilter : public controller_interface::ChainableControllerInterface
+    {
+    public:
+      LowPassFilter();
+      controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+      controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+      // Chainable controller replaces update() with the following two functions
+      controller_interface::return_type update_reference_from_subscribers(
+        const rclcpp::Time & time, const rclcpp::Duration & period) override;
+      controller_interface::return_type update_and_write_commands(
+        const rclcpp::Time & time, const rclcpp::Duration & period) override;
+      controller_interface::CallbackReturn on_init() 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::CallbackReturn on_deactivate(
+        const rclcpp_lifecycle::State & previous_state) override;
+    protected:
+      std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+      std::shared_ptr<ParamListener> param_listener_;
+      Params params_;
+      using StatePublisher = realtime_tools::RealtimePublisher<DynamicInterfaceValuesMsg>;
+      rclcpp::Publisher<DynamicInterfaceValuesMsg>::SharedPtr interface_values_publisher_;
+      std::unique_ptr<StatePublisher> realtime_publisher_;
+      std::shared_ptr<control_toolbox::LowPassFilter<double>> low_pass_filter_;
+    };
+    }  // namespace low_pass_filter
+    #endif  // LOW_PASS_FILTER_LOW_PASS_FILTER

low_pass_filter/low_pass_filter_plugin.xml

-Original file line number
+Diff line change
@@ -0,0 +1,8 @@
+    <library path="low_pass_filter">
+      <class name="low_pass_filter/LowPassFilter" type="low_pass_filter::LowPassFilter"
+        base_class_type="controller_interface::ChainableControllerInterface">
+        <description>
+          Low pass filter.
+        </description>
+      </class>
+    </library>

low_pass_filter/package.xml

-Original file line number
+Diff line change
@@ -0,0 +1,31 @@
+    <?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>low_pass_filter</name>
+      <version>0.0.1</version>
+      <description>Wrapper for low pass filter</description>
+      <maintainer email="[email protected]">Husarion</maintainer>
+      <license>Apache License 2.0</license>
+      <url type="website">https://husarion.com</url>
+      <url type="repository">https://github.com/husarion/husarion_controllers</url>
+      <url type="bugtracker">https://github.com/husarion/husarion_controllers/issues</url>
+      <author email="[email protected]">Dawid Kmak</author>
+      <buildtool_depend>ament_cmake</buildtool_depend>
+      <depend>control_msgs</depend>
+      <depend>control_toolbox</depend>
+      <depend>controller_interface</depend>
+      <depend>generate_parameter_library</depend>
+      <depend>hardware_interface</depend>
+      <depend>pluginlib</depend>
+      <depend>rclcpp</depend>
+      <depend>rclcpp_lifecycle</depend>
+      <depend>realtime_tools</depend>
+      <export>
+        <build_type>ament_cmake</build_type>
+      </export>
+    </package>

low_pass_filter/src/low_pass_filter.cpp

-Original file line number
+Diff line change
@@ -0,0 +1,150 @@
+    // Copyright 2025 Husarion sp. z o.o.
+    //
+    // 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 "low_pass_filter/low_pass_filter.hpp"
+    #include <control_toolbox/low_pass_filter.hpp>
+    #include <controller_interface/chainable_controller_interface.hpp>
+    #include <rclcpp/rclcpp.hpp>
+    #include <rclcpp_lifecycle/state.hpp>
+    namespace low_pass_filter
+    {
+    LowPassFilter::LowPassFilter() : controller_interface::ChainableControllerInterface() {}
+    controller_interface::InterfaceConfiguration LowPassFilter::command_interface_configuration() const
+    {
+      controller_interface::InterfaceConfiguration command_interfaces_config;
+      command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
+      return command_interfaces_config;
+    }
+    controller_interface::InterfaceConfiguration LowPassFilter::state_interface_configuration() const
+    {
+      controller_interface::InterfaceConfiguration state_interfaces_config;
+      state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+      for (const auto & name : params_.state_interface_names) {
+        state_interfaces_config.names.push_back(params_.state_interface_prefix + "/" + name);
+      }
+      return state_interfaces_config;
+    }
+    controller_interface::return_type LowPassFilter::update_reference_from_subscribers(
+      const rclcpp::Time &, const rclcpp::Duration &)
+    {
+      return controller_interface::return_type::OK;
+    }
+    controller_interface::return_type LowPassFilter::update_and_write_commands(
+      const rclcpp::Time & time, const rclcpp::Duration &)
+    {
+      if (!low_pass_filter_ || !low_pass_filter_->is_configured()) {
+        RCLCPP_WARN(get_node()->get_logger(), "Low pass filter is not configured");
+        return controller_interface::return_type::ERROR;
+      }
+      for (size_t i = 0; i < state_interfaces_.size(); ++i) {
+        low_pass_filter_->update(
+          state_interfaces_[i].get_optional<double>().value(), state_interfaces_values_[i]);
+        if (std::abs(state_interfaces_values_[i]) < params_.zero_threshold) {
+          state_interfaces_values_[i] = 0.0;
+        }
+      }
+      if (realtime_publisher_ && realtime_publisher_->trylock()) {
+        realtime_publisher_->msg_.header.stamp = time;
+        realtime_publisher_->msg_.states.interface_names = exported_state_interface_names_;
+        realtime_publisher_->msg_.states.values = state_interfaces_values_;
+        realtime_publisher_->unlockAndPublish();
+      }
+      return controller_interface::return_type::OK;
+    }
+    controller_interface::CallbackReturn LowPassFilter::on_init()
+    {
+      try {
+        param_listener_ = std::make_shared<ParamListener>(get_node());
+        params_ = param_listener_->get_params();
+      } catch (const std::exception & err) {
+        RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to load parameters: %s", err.what());
+        return controller_interface::CallbackReturn::ERROR;
+      }
+      return controller_interface::CallbackReturn::SUCCESS;
+    }
+    controller_interface::CallbackReturn LowPassFilter::on_configure(const rclcpp_lifecycle::State &)
+    {
+      auto logger = get_node()->get_logger();
+      if (params_.publish_interface_values) {
+        interface_values_publisher_ = get_node()->create_publisher<DynamicInterfaceValuesMsg>(
+          "~/interface_values", rclcpp::SystemDefaultsQoS());
+        realtime_publisher_ = std::make_unique<StatePublisher>(interface_values_publisher_);
+      }
+      low_pass_filter_ = std::make_shared<control_toolbox::LowPassFilter<double>>(
+        params_.sampling_frequency, params_.damping_frequency, params_.damping_intensity);
+      low_pass_filter_->configure();
+      return controller_interface::CallbackReturn::SUCCESS;
+    }
+    controller_interface::CallbackReturn LowPassFilter::on_activate(const rclcpp_lifecycle::State &)
+    {
+      // update parameters if they have changed
+      if (param_listener_->is_old(params_)) {
+        params_ = param_listener_->get_params();
+        RCLCPP_INFO(get_node()->get_logger(), "Parameters were updated");
+        low_pass_filter_->set_params(
+          params_.sampling_frequency, params_.damping_frequency, params_.damping_intensity);
+      }
+      return controller_interface::CallbackReturn::SUCCESS;
+    }
+    controller_interface::CallbackReturn LowPassFilter::on_deactivate(const rclcpp_lifecycle::State &)
+    {
+      // Deactivate the controller
+      return controller_interface::CallbackReturn::SUCCESS;
+    }
+    std::vector<hardware_interface::StateInterface> LowPassFilter::on_export_state_interfaces()
+    {
+      std::vector<hardware_interface::StateInterface> state_interfaces;
+      const auto state_interfaces_size = params_.state_interface_names.size();
+      state_interfaces.reserve(state_interfaces_size);
+      state_interfaces_values_.resize(state_interfaces_size, std::numeric_limits<double>::quiet_NaN());
+      std::size_t index = 0;
+      for (const auto & name : params_.state_interface_names) {
+        state_interfaces.push_back(hardware_interface::StateInterface(
+          std::string(this->get_node()->get_name()), name, &state_interfaces_values_[index]));
+        index++;
+      }
+      return state_interfaces;
+    }
+    }  // namespace low_pass_filter
+    #include "class_loader/register_macro.hpp"
+    CLASS_LOADER_REGISTER_CLASS(
+      low_pass_filter::LowPassFilter, controller_interface::ChainableControllerInterface)

low_pass_filter/src/low_pass_filter_parameters.yaml

-Original file line number
+Diff line change
@@ -0,0 +1,39 @@
+    low_pass_filter:
+      state_interface_prefix: {
+        type: string,
+        default_value: "",
+        description: "Prefix to be prepended to the state interface names.",
+      }
+      state_interface_names: {
+        type: string_array,
+        default_value: [],
+        description: "Names of state interfaces to be filtered.",
+        validation: {
+          not_empty<>: []
+        }
+      }
+      sampling_frequency: {
+        type: double,
+        default_value: 100.0,
+        description: "Filter sampling frequency in Hz.",
+      }
+      damping_frequency: {
+        type: double,
+        default_value: 0.1,
+        description: "Filter damping frequency in Hz.",
+      }
+      damping_intensity: {
+        type: double,
+        default_value: 0.707,
+        description: "Defines how sharp the filter is. 0.707 is the default value for a Butterworth filter.",
+      }
+      publish_interface_values: {
+        type: bool,
+        default_value: false,
+        description: "If true, the state interface values will be published to a topic.",
+      }
+      zero_threshold: {
+        type: double,
+        default_value: 0.0,
+        description: "If the filtered value is below this threshold, it will be set to zero.",
+      }

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ROS 2 twist mux controller #14

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Uh oh!

Uh oh!