Add filtering capability and publishers for additional frames to fts broadcaster #559

guihomework · 2023-04-03T11:08:11Z

Is a rebase of #269, using ParameterListener, with additional tests and some fixes.
Also already includes revised #268
depends on ros-controls/control_toolbox#152 and ros-controls/control_toolbox#153

guihomework · 2023-04-03T17:03:21Z

I think the build fail due to a change in the controller_interface (ros-controls/ros2_control@fea2d8a) not yet accessible by the CI ?

mergify · 2024-02-05T08:40:01Z

This pull request is in conflict. Could you fix it @guihomework?

github-actions · 2025-04-11T12:41:23Z

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

codecov · 2025-04-11T12:42:00Z

Codecov Report

❌ Patch coverage is 83.87097% with 30 lines in your changes missing coverage. Please review.
✅ Project coverage is 86.37%. Comparing base (9e89020) to head (63dcce2).
⚠️ Report is 16 commits behind head on master.

Files with missing lines	Patch %	Lines
...roadcaster/src/force_torque_sensor_broadcaster.cpp	68.83%	21 Missing and 3 partials ⚠️
...ster/test/test_force_torque_sensor_broadcaster.cpp	85.71%	1 Missing and 2 partials ⚠️
...t_force_torque_sensor_broadcaster_with_filters.cpp	96.59%	0 Missing and 3 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #559      +/-   ##
==========================================
- Coverage   86.42%   86.37%   -0.05%     
==========================================
  Files         123      124       +1     
  Lines       12231    12400     +169     
  Branches     1021     1031      +10     
==========================================
+ Hits        10571    10711     +140     
- Misses       1344     1366      +22     
- Partials      316      323       +7

Flag	Coverage Δ
unittests	`86.37% <83.87%> (-0.05%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ster/test/test_force_torque_sensor_broadcaster.cpp	`96.73% <85.71%> (-1.43%)`	⬇️
...t_force_torque_sensor_broadcaster_with_filters.cpp	`96.59% <96.59%> (ø)`
...roadcaster/src/force_torque_sensor_broadcaster.cpp	`81.76% <68.83%> (-10.74%)`	⬇️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

mergify · 2025-04-11T12:42:02Z

This pull request is in conflict. Could you fix it @guihomework?

…r-rolling

…ility-to-fts-broadcaster-rolling

needs ros/filters@34f154b

.github/workflows/rolling-compatibility-humble-binary-build.yml

…r-rolling

ros2_controllers-not-released.humble.repos

…ility-to-fts-broadcaster-rolling

OscarMrZ · 2025-06-18T09:13:48Z

Hello @christophfroehlich,

I'm interested in finishing this PR. I did some preliminary testing (in Humble) and it seems to be working fine. Some

Could you provide me with some suggestions on how to do this? Namely:

Should I open a new PR with a fork with the needed changes?
I see a lot of CI checks are failing, any input for that?

Thanks a lot!

P.D. @guihomework are you interested in finishing this?

christophfroehlich · 2025-06-18T10:48:32Z

I can fix the latest conflicts in the next days. This PR was ready to merge, only reviews were missing.

Raivias

Functionally looks good to me. Just some readability/maintainability comments.

force_torque_sensor_broadcaster/CMakeLists.txt

force_torque_sensor_broadcaster/package.xml

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

Raivias · 2025-07-01T17:25:58Z

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

  // TODO(juliaj): remove the logging after resolving
  // https://github.com/ros-controls/ros2_controllers/issues/1574
-  RCLCPP_INFO(get_node()->get_logger(), "Locking realtime publisher");
-  realtime_publisher_->lock();
-  RCLCPP_INFO(get_node()->get_logger(), "Locked realtime publisher");
-
-  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
-
-  RCLCPP_INFO(get_node()->get_logger(), "Unlocking realtime publisher");
-  realtime_publisher_->unlock();
-  RCLCPP_INFO(get_node()->get_logger(), "Unlocked realtime publisher");
+  RCLCPP_INFO(get_node()->get_logger(), "Locking realtime_raw_publisher");
+  realtime_raw_publisher_->lock();
+  RCLCPP_INFO(get_node()->get_logger(), "Locked realtime_raw_publisher");
+  realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id;
+  RCLCPP_INFO(get_node()->get_logger(), "Unlocking realtime_raw_publisher");
+  realtime_raw_publisher_->unlock();
+  RCLCPP_INFO(get_node()->get_logger(), "Unlocked realtime_raw_publisher");
+
+  RCLCPP_INFO(get_node()->get_logger(), "Locking realtime_filtered_publisher");
+  realtime_filtered_publisher_->lock();
+  RCLCPP_INFO(get_node()->get_logger(), "Locked realtime_filtered_publisher");
+  realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id;
+  RCLCPP_INFO(get_node()->get_logger(), "Unlocking realtime_filtered_publisher");
+  realtime_filtered_publisher_->unlock();
+  RCLCPP_INFO(get_node()->get_logger(), "Unlocked realtime_filtered_publisher");


Seems like the logging can be pulled out. The linked issue is closed, and it doesn't seem to add much valuable information. If we do want to keep it, maybe set it to debug rather than info.

I cleaned it up: the respective logging was reverted on the master branch.

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml

ros2_controllers-not-released.humble.repos

mergify · 2025-07-01T17:33:32Z

This pull request is in conflict. Could you fix it @guihomework?

…r-rolling

Co-authored-by: Xavier Guay <[email protected]>

christophfroehlich · 2025-07-01T20:16:32Z

@OscarMrZ could you leave your review here please? I fixed the merge conflicts, and failing binary tests are due to upstream changes.

saikishor · 2025-07-01T20:32:52Z

...nsor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

+  std::string sensor_name_;
+  std::array<std::string, 6> interface_names_;


Do we need these?, don't we have the same information in the GPL params?

saikishor · 2025-07-01T20:36:54Z

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

+    return CallbackReturn::ERROR;
+  }
+  if (!filter_chain_->configure(
+        "sensor_filter_chain", get_node()->get_node_logging_interface(),


Can you add some documentation of the sensor_filter_chain namespace and how it can be used to configure filters?

What happens currently if there is no filter configuration present in the params? THis would fail and the broadcaster won't activate right?. I believe this shouldn't be the case

saikishor · 2025-07-01T20:42:39Z

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

+  // Add additional frames to publish if any exits
+  if (!params_.additional_frames_to_publish.empty())
+  {
+    auto nr_frames = params_.additional_frames_to_publish.size();
+    wrench_additional_frames_pubs_.reserve(nr_frames);
+    wrench_additional_frames_publishers_.reserve(nr_frames);
+    for (const auto & frame : params_.additional_frames_to_publish)
+    {
+      StatePublisher pub = get_node()->create_publisher<WrenchMsgType>(
+        "~/wrench_filtered_" + frame, rclcpp::SystemDefaultsQoS());
+      wrench_additional_frames_pubs_.emplace_back(pub);
+      wrench_additional_frames_publishers_.emplace_back(std::make_unique<StateRTPublisher>(pub));
+
+      wrench_additional_frames_publishers_.back()->lock();
+      wrench_additional_frames_publishers_.back()->msg_.header.frame_id = frame;
+      wrench_additional_frames_publishers_.back()->unlock();
+    }


I don't know if we should do this here. I think every individual should be responsible for transforming to their desired one. I don't think it make sense to transform and do it and have publisher for each of those.

saikishor · 2025-07-01T20:43:43Z

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

+  if (!filtered)
+  {
+    wrench_filtered_.wrench.force.x = std::numeric_limits<double>::quiet_NaN();
+    wrench_filtered_.wrench.force.y = std::numeric_limits<double>::quiet_NaN();
+    wrench_filtered_.wrench.force.z = std::numeric_limits<double>::quiet_NaN();
+    wrench_filtered_.wrench.torque.x = std::numeric_limits<double>::quiet_NaN();
+    wrench_filtered_.wrench.torque.y = std::numeric_limits<double>::quiet_NaN();
+    wrench_filtered_.wrench.torque.z = std::numeric_limits<double>::quiet_NaN();
+  }
+
+  if (realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock())


Suggested change

if (!filtered)

{

wrench_filtered_.wrench.force.x = std::numeric_limits<double>::quiet_NaN();

wrench_filtered_.wrench.force.y = std::numeric_limits<double>::quiet_NaN();

wrench_filtered_.wrench.force.z = std::numeric_limits<double>::quiet_NaN();

wrench_filtered_.wrench.torque.x = std::numeric_limits<double>::quiet_NaN();

wrench_filtered_.wrench.torque.y = std::numeric_limits<double>::quiet_NaN();

wrench_filtered_.wrench.torque.z = std::numeric_limits<double>::quiet_NaN();

}

if (realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock())

if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock())

If the filtering is failing, just don't publish it. Publishing NaNs might mess up other's code right?

saikishor · 2025-07-01T20:45:42Z

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

+        publisher->msg_.header.frame_id.c_str(), e.what());
+      publisher->msg_.wrench.force.x = std::numeric_limits<double>::quiet_NaN();
+      publisher->msg_.wrench.force.y = std::numeric_limits<double>::quiet_NaN();
+      publisher->msg_.wrench.force.z = std::numeric_limits<double>::quiet_NaN();
+      publisher->msg_.wrench.torque.x = std::numeric_limits<double>::quiet_NaN();
+      publisher->msg_.wrench.torque.y = std::numeric_limits<double>::quiet_NaN();
+      publisher->msg_.wrench.torque.z = std::numeric_limits<double>::quiet_NaN();


If it fails, just don't publish it. Better skip that iteration

saikishor · 2025-07-01T20:46:11Z

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

+        auto transform = tf_buffer_->lookupTransform(
+          publisher->msg_.header.frame_id, params_.frame_id, tf2::TimePointZero);
+        tf2::doTransform(wrench_filtered_, publisher->msg_, transform);
+      }
+      else
+      {
+        throw tf2::TransformException("cannot transform, filtered message is invalid");
+      }


I'm not a big fan of having tf inside the RT loops. If this is the case, I would ask you to time it and share the results on how much time it consumes 1. static transforms 2. dynamic transforms

…r-rolling

destogl · 2025-07-30T17:35:03Z

We can close this in favor of creating a new node that transforms the input Wrench topic to output Wrench topic.

guihomework mentioned this pull request Apr 3, 2023

Add filtering capability and publishers for additional frames to fts broadcaster destogl/ros2_controllers#44

Closed

destogl and others added 6 commits April 13, 2023 14:32

Add support for publishing additional topics to the FTS Broadcaster

fe36000

Added filtering capabilities

ef7ad6a

Added more tests with filters

a47926a

Removed helper that only shows declared params

a413217

Fixed filter handling and unitialized frame_id

3652ab1

Added test for additional_frames_to_publish

8163413

destogl force-pushed the add-filtering-capability-to-fts-broadcaster-rolling branch from ebaa3d6 to 8163413 Compare April 13, 2023 12:32

github-actions bot added the stale label Apr 11, 2025

christophfroehlich added 2 commits April 12, 2025 07:48

Merge branch 'master' into add-filtering-capability-to-fts-broadcaste…

a58bd31

…r-rolling

Fix test parameters

31e79e2

christophfroehlich marked this pull request as ready for review April 12, 2025 08:43

github-actions bot requested review from ARK3r, bmagyar, mcbed, olivier-stasse, Raivias, saikishor and VX792 April 12, 2025 08:43

github-actions bot removed the stale label Apr 14, 2025

This was referenced Apr 15, 2025

[FTS Broadcaster] Add support for publishing forces in other frames on additional topics #268

Closed

Add support for filtering of wrenches in the broadcaster. #269

Closed

Merge remote-tracking branch 'origin/master' into add-filtering-capab…

41853ab

…ility-to-fts-broadcaster-rolling

christophfroehlich mentioned this pull request Apr 19, 2025

Add multiplier support to ForceTorqueSensorBroadcaster #1647

Merged

compile filters from source on humble

c9b52f1

needs ros/filters@34f154b

christophfroehlich reviewed Apr 21, 2025

View reviewed changes

.github/workflows/rolling-compatibility-humble-binary-build.yml Outdated Show resolved Hide resolved

bmagyar and others added 2 commits May 3, 2025 10:08

Merge branch 'master' into add-filtering-capability-to-fts-broadcaste…

206da96

…r-rolling

Use filters from binary

0759030

christophfroehlich reviewed May 3, 2025

View reviewed changes

ros2_controllers-not-released.humble.repos Outdated Show resolved Hide resolved

Update ros2_controllers-not-released.humble.repos

aa12321

christophfroehlich reviewed May 3, 2025

View reviewed changes

ros2_controllers-not-released.humble.repos Outdated Show resolved Hide resolved

christophfroehlich and others added 3 commits May 3, 2025 18:43

Fix pre-commit

3d15fa5

Merge remote-tracking branch 'origin/master' into add-filtering-capab…

b431110

…ility-to-fts-broadcaster-rolling

Fix the tests

a78dab0

OscarMrZ mentioned this pull request Jun 11, 2025

Unable to configure filters ros-controls/control_toolbox#399

Closed

Raivias approved these changes Jul 1, 2025

View reviewed changes

christophfroehlich and others added 3 commits July 1, 2025 20:05

Merge branch 'master' into add-filtering-capability-to-fts-broadcaste…

714400d

…r-rolling

Apply suggestions from code review

834e1ad

Co-authored-by: Xavier Guay <[email protected]>

Apply suggestions from code review

98fac42

Co-authored-by: Xavier Guay <[email protected]>

christophfroehlich previously approved these changes Jul 1, 2025

View reviewed changes

Use correct hpp file

3a6978c

christophfroehlich dismissed their stale review via 3a6978c July 1, 2025 20:18

saikishor requested changes Jul 1, 2025

View reviewed changes

christophfroehlich added 2 commits July 3, 2025 13:36

Merge branch 'master' into add-filtering-capability-to-fts-broadcaste…

204e5d3

…r-rolling

Fix format

63dcce2

OscarMrZ mentioned this pull request Jul 11, 2025

Add filtering capability to ft_broadcaster #1814

Open

destogl closed this Jul 30, 2025

destogl mentioned this pull request Jul 30, 2025

Create a utility node that transforms input Wrench to output Wrenches for a given list of frames #1842

Open

		std::string sensor_name_;
		std::array<std::string, 6> interface_names_;

Add filtering capability and publishers for additional frames to fts broadcaster #559

Add filtering capability and publishers for additional frames to fts broadcaster #559

Uh oh!

Conversation

guihomework commented Apr 3, 2023

Uh oh!

guihomework commented Apr 3, 2023

Uh oh!

mergify bot commented Feb 5, 2024

Uh oh!

github-actions bot commented Apr 11, 2025

Uh oh!

codecov bot commented Apr 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mergify bot commented Apr 11, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

OscarMrZ commented Jun 18, 2025

Uh oh!

christophfroehlich commented Jun 18, 2025

Uh oh!

Raivias left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jul 1, 2025

Uh oh!

christophfroehlich commented Jul 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

destogl commented Jul 30, 2025

Uh oh!

Uh oh!

codecov bot commented Apr 11, 2025 •

edited

Loading

christophfroehlich commented Jul 1, 2025 •

edited

Loading