Add filtering capability to ft_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
@@ -69,7 +71,33 @@ if(BUILD_TESTING)
   target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include)
   target_link_libraries(test_force_torque_sensor_broadcaster
     force_torque_sensor_broadcaster
+    force_torque_sensor_broadcaster_parameters
+  )
+
+  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/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,39 @@ 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 (https://github.com/ros/filters/issues/89).
+  has_filter_chain_ = filter_chain_->configure(
+    "sensor_filter_chain", get_node()->get_node_logging_interface(),
+    get_node()->get_node_parameters_interface());
+
+  RCLCPP_INFO_EXPRESSION(
+    get_node()->get_logger(), has_filter_chain_, "Filter chain is successfully configured!");
+
   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 +132,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 +186,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 +253,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