Add filtering capability to ft_broadcaster (#1814)

Co-authored-by: Denis Štogl <[email protected]>
Co-authored-by: Guillaume Walck <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Xavier Guay <[email protected]>
Co-authored-by: Aarav Gupta <[email protected]>
Co-authored-by: Oscar Lima <[email protected]>
Co-authored-by: Hilary Luo <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>

Author: 10 people

doc/release_notes.rst

@@ -11,6 +11,7 @@ chained_filter_controller
 force_torque_sensor_broadcaster
 *******************************
 * Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 <https://github.com/ros-controls/ros2_controllers/pull/1647/files>`__).
+* Added support for filter chains, allowing users to configure a sequence of filter plugins with their parameters. The force/torque sensor readings are filtered sequentially and published on a separate topic.
 
 imu_sensor_broadcaster
 *******************************

force_torque_sensor_broadcaster/CMakeLists.txt

@@ -7,6 +7,7 @@ export_windows_symbols()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
+  filters
   generate_parameter_library
   geometry_msgs
   hardware_interface
@@ -37,6 +38,7 @@ target_include_directories(force_torque_sensor_broadcaster PUBLIC
 target_link_libraries(force_torque_sensor_broadcaster PUBLIC
                       force_torque_sensor_broadcaster_parameters
                       controller_interface::controller_interface
+                      filters::filter_chain
                       hardware_interface::hardware_interface
                       pluginlib::pluginlib
                       rclcpp::rclcpp
@@ -70,6 +72,31 @@ if(BUILD_TESTING)
   target_link_libraries(test_force_torque_sensor_broadcaster
     force_torque_sensor_broadcaster
   )
+
+  add_library(dummy_filter SHARED
+    test/dummy_filter.cpp
+  )
+  target_compile_features(dummy_filter PUBLIC cxx_std_17)
+  target_include_directories(dummy_filter PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include/force_torque_sensor_broadcaster>
+  )
+  target_link_libraries(dummy_filter PUBLIC
+    filters::increment
+    ${geometry_msgs_TARGETS}
+    pluginlib::pluginlib
+    rclcpp::rclcpp
+  )
+  install(
+    TARGETS
+      dummy_filter
+    EXPORT export_force_torque_sensor_broadcaster
+    RUNTIME DESTINATION bin
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+  )
+
+  pluginlib_export_plugin_description_file(filters "test/dummy_filter_plugin.xml")
 endif()
 
 install(

force_torque_sensor_broadcaster/doc/userdoc.rst

@@ -7,8 +7,11 @@ Force Torque Sensor Broadcaster
 Broadcaster of messages from force/torque state interfaces of a robot or sensor.
 The published message type is ``geometry_msgs/msg/WrenchStamped``.
 
-The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see ``controller_interface`` package).
+The broadcaster also supports filtering the force/torque readings using a filter chain, enabling the sequential application of multiple filters. In this case, an additional topic with the
+``_filtered`` suffix will be published containing the final result. For more details on filters, refer to the `filters package repository <https://github.com/ros/filters>`_.
+See the parameter section for instructions on configuring a filter chain with an arbitrary number of filters.
 
+The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see ``controller_interface`` package).
 
 Parameters
 ^^^^^^^^^^^
@@ -17,6 +20,12 @@ This controller uses the `generate_parameter_library <https://github.com/PickNik
 The interfaces can be defined in two ways, using the ``sensor_name`` or the ``interface_names`` parameter:
 Those two parameters cannot be defined at the same time.
 
+The filter chain is configured as a sequential list of filters under the ``sensor_filter_chain`` parameter, where each filter is identified by the key ``filterN``, with ``N`` representing its position in the chain (e.g., ``filter1``, ``filter2``, etc.).
+Each filter entry must specify the ``name`` and ``type`` of the plugin, along with any additional parameters required by that specific filter plugin. The chain will use the `pluginlib <https://github.com/ros/pluginlib>`_ library to load each filter at runtime,
+passing the specified parameters.
+
+The chain processes the data sequentially, passing the output of one filter as the input to the next.
+
 Full list of parameters:
 
 .. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.yaml

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

@@ -20,18 +20,23 @@
 #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "controller_interface/chainable_controller_interface.hpp"
-// auto-generated by generate_parameter_library
-#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp"
+#include "filters/filter_chain.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 
+// auto-generated by generate_parameter_library
+#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp"
+
 namespace force_torque_sensor_broadcaster
 {
+using WrenchMsgType = geometry_msgs::msg::WrenchStamped;
+
 class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface
 {
 public:
@@ -67,10 +72,18 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr
   Params params_;
 
   std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
-
-  using StatePublisher = realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>;
-  rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr sensor_state_publisher_;
-  std::unique_ptr<StatePublisher> realtime_publisher_;
+  bool has_filter_chain_ = false;
+
+  WrenchMsgType wrench_raw_;
+  WrenchMsgType wrench_filtered_;
+  std::unique_ptr<filters::FilterChain<WrenchMsgType>> filter_chain_;
+
+  using StatePublisher = rclcpp::Publisher<WrenchMsgType>::SharedPtr;
+  using StateRTPublisher = realtime_tools::RealtimePublisher<WrenchMsgType>;
+  StatePublisher sensor_raw_state_publisher_;
+  StatePublisher sensor_filtered_state_publisher_;
+  std::unique_ptr<StateRTPublisher> realtime_raw_publisher_;
+  std::unique_ptr<StateRTPublisher> realtime_filtered_publisher_;
 };
 
 }  // namespace force_torque_sensor_broadcaster

force_torque_sensor_broadcaster/package.xml

@@ -25,6 +25,7 @@
 
   <depend>backward_ros</depend>
   <depend>controller_interface</depend>
+  <depend>filters</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -18,9 +18,12 @@
 
 #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
 
+#include <limits>
 #include <memory>
 #include <string>
 
+#include "rclcpp/logging.hpp"
+
 namespace force_torque_sensor_broadcaster
 {
 ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster()
@@ -87,12 +90,43 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
         torque_names.z));
   }
 
+  try
+  {
+    filter_chain_ =
+      std::make_unique<filters::FilterChain<WrenchMsgType>>("geometry_msgs::msg::WrenchStamped");
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during filter chain creation with message : %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+
+  // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is
+  // present. Even if the sensor_filter_chain parameter is not specified, the filter chain will be
+  // correctly configured with an empty list of filters.
+  bool filter_chain_configured = filter_chain_->configure(
+    "sensor_filter_chain", get_node()->get_node_logging_interface(),
+    get_node()->get_node_parameters_interface());
+
+  // Even on successful configure, if empty, the chain won't be used
+  has_filter_chain_ = filter_chain_configured && filter_chain_->get_length() > 0;
+
+  RCLCPP_INFO_EXPRESSION(
+    get_node()->get_logger(), has_filter_chain_, "Filter active with %zu filters!",
+    filter_chain_->get_length());
+
   try
   {
     // register ft sensor data publisher
-    sensor_state_publisher_ = get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
+    sensor_raw_state_publisher_ = get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
       "~/wrench", rclcpp::SystemDefaultsQoS());
-    realtime_publisher_ = std::make_unique<StatePublisher>(sensor_state_publisher_);
+    realtime_raw_publisher_ = std::make_unique<StateRTPublisher>(sensor_raw_state_publisher_);
+
+    sensor_filtered_state_publisher_ =
+      get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
+        "~/wrench_filtered", rclcpp::SystemDefaultsQoS());
+    realtime_filtered_publisher_ =
+      std::make_unique<StateRTPublisher>(sensor_filtered_state_publisher_);
   }
   catch (const std::exception & e)
   {
@@ -102,9 +136,19 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  realtime_publisher_->lock();
-  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
-  realtime_publisher_->unlock();
+  wrench_raw_.header.frame_id = params_.frame_id;
+  wrench_filtered_.header.frame_id = params_.frame_id;
+
+  realtime_raw_publisher_->lock();
+  realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id;
+  realtime_raw_publisher_->unlock();
+
+  if (has_filter_chain_)
+  {
+    realtime_filtered_publisher_->lock();
+    realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id;
+    realtime_filtered_publisher_->unlock();
+  }
 
   RCLCPP_INFO(get_node()->get_logger(), "configure successful");
   return controller_interface::CallbackReturn::SUCCESS;
@@ -146,13 +190,28 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write
 {
   param_listener_->try_get_params(params_);
 
-  if (realtime_publisher_ && realtime_publisher_->trylock())
+  wrench_raw_.header.stamp = time;
+  force_torque_sensor_->get_values_as_message(wrench_raw_.wrench);
+  this->apply_sensor_offset(params_, wrench_raw_);
+  this->apply_sensor_multiplier(params_, wrench_raw_);
+
+  if (realtime_raw_publisher_ && realtime_raw_publisher_->trylock())
+  {
+    realtime_raw_publisher_->msg_.header.stamp = time;
+    realtime_raw_publisher_->msg_.wrench = wrench_raw_.wrench;
+    realtime_raw_publisher_->unlockAndPublish();
+  }
+
+  if (has_filter_chain_)
   {
-    realtime_publisher_->msg_.header.stamp = time;
-    force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench);
-    this->apply_sensor_offset(params_, realtime_publisher_->msg_);
-    this->apply_sensor_multiplier(params_, realtime_publisher_->msg_);
-    realtime_publisher_->unlockAndPublish();
+    // Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_
+    auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_);
+    if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock())
+    {
+      realtime_filtered_publisher_->msg_.header.stamp = time;
+      realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench;
+      realtime_filtered_publisher_->unlockAndPublish();
+    }
   }
 
   return controller_interface::return_type::OK;
@@ -198,38 +257,32 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces()
   if (!force_names[0].empty())
   {
     exported_state_interfaces.emplace_back(
-      hardware_interface::StateInterface(
-        export_prefix, force_names[0], &realtime_publisher_->msg_.wrench.force.x));
+      export_prefix, force_names[0], &realtime_raw_publisher_->msg_.wrench.force.x);
   }
   if (!force_names[1].empty())
   {
     exported_state_interfaces.emplace_back(
-      hardware_interface::StateInterface(
-        export_prefix, force_names[1], &realtime_publisher_->msg_.wrench.force.y));
+      export_prefix, force_names[1], &realtime_raw_publisher_->msg_.wrench.force.y);
   }
   if (!force_names[2].empty())
   {
     exported_state_interfaces.emplace_back(
-      hardware_interface::StateInterface(
-        export_prefix, force_names[2], &realtime_publisher_->msg_.wrench.force.z));
+      export_prefix, force_names[2], &realtime_raw_publisher_->msg_.wrench.force.z);
   }
   if (!torque_names[0].empty())
   {
     exported_state_interfaces.emplace_back(
-      hardware_interface::StateInterface(
-        export_prefix, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x));
+      export_prefix, torque_names[0], &realtime_raw_publisher_->msg_.wrench.torque.x);
   }
   if (!torque_names[1].empty())
   {
     exported_state_interfaces.emplace_back(
-      hardware_interface::StateInterface(
-        export_prefix, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y));
+      export_prefix, torque_names[1], &realtime_raw_publisher_->msg_.wrench.torque.y);
   }
   if (!torque_names[2].empty())
   {
     exported_state_interfaces.emplace_back(
-      hardware_interface::StateInterface(
-        export_prefix, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z));
+      export_prefix, torque_names[2], &realtime_raw_publisher_->msg_.wrench.torque.z);
   }
   return exported_state_interfaces;
 }

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml

@@ -112,3 +112,10 @@ force_torque_sensor_broadcaster:
         default_value: 1.0,
         description: "The multiplier of torque value around 'z' axis.",
       }
+  sensor_filter_chain: {
+    type: none,
+    description: "Map of parameters that defines a filter chain, containing filterN as key. The fields for each filter are:
+    type: The filter plugin to be loaded
+    name: Actual name semantically describing the filter, e.g., low_pass_filter
+    params: And underlying map of parameters needed for a specific filter, refer to the specific filter documentation."
+  }

force_torque_sensor_broadcaster/test/dummy_filter.cpp

@@ -0,0 +1,56 @@
+
+// Copyright (c) 2025, PAL Robotics
+//
+// 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.
+
+/*
+ * Authors: Oscar Martinez
+ */
+
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+
+#include "filters/increment.hpp"
+
+namespace filters
+{
+
+template <>
+bool IncrementFilter<geometry_msgs::msg::WrenchStamped>::update(
+  const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out)
+{
+  if (!this->configured_)
+  {
+    throw std::runtime_error("Filter is not configured");
+  }
+
+  // Just increment every value
+  data_out.wrench.force.x = data_in.wrench.force.x + 1;
+  data_out.wrench.force.y = data_in.wrench.force.y + 1;
+  data_out.wrench.force.z = data_in.wrench.force.z + 1;
+  data_out.wrench.torque.x = data_in.wrench.torque.x + 1;
+  data_out.wrench.torque.y = data_in.wrench.torque.y + 1;
+  data_out.wrench.torque.z = data_in.wrench.torque.z + 1;
+
+  return true;
+}
+
+}  // namespace filters
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  filters::IncrementFilter<geometry_msgs::msg::WrenchStamped>,
+  filters::FilterBase<geometry_msgs::msg::WrenchStamped>)

force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml

@@ -0,0 +1,9 @@
+<class_libraries>
+    <library path="dummy_filter">
+        <class name="filters/IncrementFilterWrench" type="filters::IncrementFilter&lt;geometry_msgs::msg::WrenchStamped&gt;" base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
+            <description>
+                This is a increment filter which works on a wrench message.
+            </description>
+        </class>
+    </library>
+</class_libraries>

force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml

@@ -1,4 +1,11 @@
 test_force_torque_sensor_broadcaster:
   ros__parameters:
-
     frame_id:  "fts_sensor_frame"
+test_force_torque_sensor_broadcaster_with_chain:
+  ros__parameters:
+    frame_id:  "fts_sensor_frame"
+    sensor_name: "fts_sensor"
+    sensor_filter_chain:
+      filter1:
+        name: dummy
+        type: filters/IncrementFilterWrench