[jazzy] Port to ROS2 by DLu · Pull Request #32 · peci1/robot_body_filter

DLu · 2026-04-02T21:26:56Z

Ok, here's my ROS2 port. I've broken it up into commits to be easier to parse, since its currently sitting at 1,349 additions / 2,108 deletions. It's 20 commits, but many of them should be easy to skim.

Rote work

This stuff is mostly by-the-books

Interface syntax - changing the includes for ROS interfaces, and adding a heck of a lot of ::msg
Various Headers - Changing roscpp headers to rclcpp, changing .h to .hpp and adding a few basic library #includes that we'll need eventually.
Logging interface and Clock Pointer - Add access to logging interface/clock for other classes
Time and Duration - Changing ros::Duration and ros::Time to their rclcpp counterparts, also including
- changing toSec to seconds
- removing WallTime/Duration
- making sure initializations either use two ints or the ::from_seconds method.
- using rclcpp::sleep_for
Logging - change all of the logging macros
Pub/Sub/ok - update the publishers/subscribers/service and calls to ros::ok
TF Fixes
boost -> std locks
Rate change Rate to Rate pointer since you can't copy by value.
Update message definitions

New implementations / updates

Parameter work -
- Remove references to xmlrpc
- Remove const from FilterBase parameter methods (this changed in the filters ROS 2 port)
- New implementation of getParamDuration because you can't construct Duration naturally.
- New implementation of getParamVerboseSet
- Independently declare the robot_description parameter (separate from filter_utils logic)
Update package data - Update package.xml, Cmake, et al.
Test infrastructure - get the tests to compile and somewhat run
Dynamic parameters - Use ROS 2 dynamic parameters
Service callback
Update example - Get the example to launch properly

Other notes

The tests aren't passing yet
Reloading the model via service call or parameter crashes the node.
The tests don't pass in a Logging interface or clock, and instead use nullptr, and those probably should be mocked out.

peci1 · 2026-04-04T23:20:08Z

Thanks a lot! I stared going through the PR. I'm a bit limited over Easter, so I have to test with Kilted. I got a few build failures I needed to fix, but none of them seems to be related to Kilted. Here's the patch that got me going.

Subject: [PATCH] fix build
---
Index: CMakeLists.txt
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/CMakeLists.txt b/CMakeLists.txt
--- a/CMakeLists.txt	(revision a2d902d0648dcf0f15c88f0e58f7f06f3b8d7518)
+++ b/CMakeLists.txt	(date 1775343266027)
@@ -60,6 +60,7 @@
   $<INSTALL_INTERFACE:include>
 )
 ament_target_dependencies(${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS})
+target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES})
 
 add_library(tf2_sensor_msgs_rbf src/utils/tf2_sensor_msgs.cpp)
 target_link_libraries(tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils)
Index: test/test_tf_frames_watchdog.cpp
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/test/test_tf_frames_watchdog.cpp b/test/test_tf_frames_watchdog.cpp
--- a/test/test_tf_frames_watchdog.cpp	(revision a2d902d0648dcf0f15c88f0e58f7f06f3b8d7518)
+++ b/test/test_tf_frames_watchdog.cpp	(date 1775205289125)
@@ -148,9 +148,9 @@
 
   watchdog.unpause(); // searchForReachableFrames checks this->paused
 
-  rclcpp::Time start = clock->now();
+  rclcpp::Time start = clock_ptr->now();
   watchdog.searchForReachableFrames();
-  rclcpp::Time end = clock->now();
+  rclcpp::Time end = clock_ptr->now();
 
   EXPECT_FALSE(watchdog.isReachable("left_track"));
   EXPECT_FALSE(watchdog.isReachable("front_left_flipper"));
@@ -165,13 +165,13 @@
   tf.transform.rotation.w = 1.0;
   for (double d = -5.0; d < 5.0; d += 0.1)
   {
-    tf.header.stamp = clock->now() + rclcpp::Duration::from_seconds(d);
+    tf.header.stamp = clock_ptr->now() + rclcpp::Duration::from_seconds(d);
     tfBuffer->setTransform(tf, "test");
   }
 
-  start = clock->now();
+  start = clock_ptr->now();
   watchdog.searchForReachableFrames();
-  end = clock->now();
+  end = clock_ptr->now();
 
   EXPECT_TRUE(watchdog.isReachable("left_track"));
   EXPECT_FALSE(watchdog.isReachable("front_left_flipper"));
@@ -185,7 +185,7 @@
   tf.transform.rotation.w = 1.0;
   for (double d = -5.0; d < 5.0; d += 0.1)
   {
-    tf.header.stamp = clock->now() + rclcpp::Duration::from_seconds(d);
+    tf.header.stamp = clock_ptr->now() + rclcpp::Duration::from_seconds(d);
     tfBuffer->setTransform(tf, "test");
   }
 
@@ -205,19 +205,19 @@
   TestWatchdog watchdog("base_link", {"left_track", "front_left_flipper"}, tfBuffer,
                         rclcpp::Duration::from_seconds(0.1), std::make_shared<rclcpp::Rate>(1.0));
 
-  EXPECT_THROW(watchdog.lookupTransform("left_track", clock->now(), rclcpp::Duration::from_seconds(1)), std::runtime_error);
+  EXPECT_THROW(watchdog.lookupTransform("left_track", clock_ptr->now(), rclcpp::Duration::from_seconds(1)), std::runtime_error);
 
   watchdog.started = true; // fake the running thread
 
   EXPECT_FALSE(watchdog.isReachable("left_track"));
-  auto resTf = watchdog.lookupTransform("left_track", clock->now(), rclcpp::Duration::from_seconds(1));
+  auto resTf = watchdog.lookupTransform("left_track", clock_ptr->now(), rclcpp::Duration::from_seconds(1));
   EXPECT_FALSE((bool) resTf);
 
   // if the frame is marked reachable and canTransform fails, it is marked unreachable
   watchdog.markReachable("left_track");
-  rclcpp::Time start = clock->now();
-  resTf = watchdog.lookupTransform("left_track", clock->now(), rclcpp::Duration::from_seconds(1));
-  rclcpp::Time end = clock->now();
+  rclcpp::Time start = clock_ptr->now();
+  resTf = watchdog.lookupTransform("left_track", clock_ptr->now(), rclcpp::Duration::from_seconds(1));
+  rclcpp::Time end = clock_ptr->now();
   EXPECT_FALSE(watchdog.isReachable("left_track"));
   EXPECT_FALSE((bool) resTf);
   // check that the lookup took at least the amount of time specified by timeout, but not much more
@@ -230,20 +230,20 @@
   tf.transform.rotation.w = 1.0;
   for (double d = -5.0; d < 5.0; d += 0.1)
   {
-    tf.header.stamp = clock->now() + rclcpp::Duration::from_seconds(d);
+    tf.header.stamp = clock_ptr->now() + rclcpp::Duration::from_seconds(d);
     tfBuffer->setTransform(tf, "test");
   }
 
   // if the transform is there but the frame is marked unreachable, lookupTransform should fail
 
-  resTf = watchdog.lookupTransform("left_track", clock->now(), rclcpp::Duration::from_seconds(1));
+  resTf = watchdog.lookupTransform("left_track", clock_ptr->now(), rclcpp::Duration::from_seconds(1));
   EXPECT_FALSE(watchdog.isReachable("left_track"));
   EXPECT_FALSE((bool) resTf);
 
   // if looking up an unmonitored frame, the first lookup fails, but sets the frame as monitored
   EXPECT_FALSE(watchdog.isMonitored("rear_left_flipper"));
   EXPECT_FALSE(watchdog.isReachable("rear_left_flipper"));
-  resTf = watchdog.lookupTransform("rear_left_flipper", clock->now(), rclcpp::Duration::from_seconds(1));
+  resTf = watchdog.lookupTransform("rear_left_flipper", clock_ptr->now(), rclcpp::Duration::from_seconds(1));
   EXPECT_TRUE(watchdog.isMonitored("rear_left_flipper"));
   EXPECT_FALSE(watchdog.isReachable("rear_left_flipper"));
   EXPECT_FALSE((bool) resTf);
@@ -257,14 +257,14 @@
   tf.transform.rotation.w = 1.0;
   for (double d = -5.0; d < 5.0; d += 0.1)
   {
-    tf.header.stamp = clock->now() + rclcpp::Duration::from_seconds(d);
+    tf.header.stamp = clock_ptr->now() + rclcpp::Duration::from_seconds(d);
     tfBuffer->setTransform(tf, "test");
   }
 
   EXPECT_TRUE(watchdog.isMonitored("rear_left_flipper"));
   EXPECT_FALSE(watchdog.isReachable("rear_left_flipper"));
   watchdog.markReachable("rear_left_flipper");
-  rclcpp::Time time = clock->now();
+  rclcpp::Time time = clock_ptr->now();
   resTf = watchdog.lookupTransform("rear_left_flipper", time, rclcpp::Duration::from_seconds(1));
   EXPECT_TRUE(watchdog.isMonitored("rear_left_flipper"));
   EXPECT_TRUE(watchdog.isReachable("rear_left_flipper"));

peci1 · 2026-04-06T11:08:47Z

I've added ROS2 CI to the ros2 branch. Please update your PR so that it is run.

Also, would you please allow maintainers to edit this PR? I think that could speed up the review rounds.

peci1 · 2026-04-08T03:37:52Z

I also removed Humble CI because cras_cpp_common is not released for Humble.

It's probably not appropriate when I work on this project in my job :)

peci1 · 2026-04-12T02:05:40Z

I've pushed some more commits that remove obsolete stuff or move some utils to cras_cpp_common. Make sure to update cras_cpp_common again to ros2 branch.

Reference and inspiration taken from https://github.com/moveit/moveit2/blob/main/moveit_ros/tests .

DLu · 2026-04-13T03:10:09Z

  <license file="LICENSE">BSD</license>

-  <url type="website">https://github.com/peci1/robot_body_filter</url>
+  <url type="repository">https://github.com/ctu-vras/robot_body_filter</url>


This link doesn't work for me yet?

Oi, I haven't done the move yet... I thought it happened years ago :-D

DLu · 2026-04-13T03:15:05Z

+                "test_filter_utils",
+            ]
+        ),
+        parameters=[os.path.join(os.path.dirname(__file__), 'test_robot_body_filter.yaml')],


Any reason to use PathJoinSubstitution above and os.path.join here?

Or really, just use pathlib

I don't think the indirection via substition is needed here. I just want to pass an absolute value. Pathlib would definitely be possible but I haven't used it much yet...

peci1 · 2026-04-13T05:57:16Z

All tests are now actually passing, but they suffer from weird errors probably somehow related to launch_testing (or my usage of it).

https://github.com/peci1/robot_body_filter/actions/runs/24327748016/job/71026552922?pr=32#step:4:602

5: Ran 1 test in 3.477sProcesses under test stopped before tests completed

I don't know what to do with these. But if you look at the gtests inside them, they're all passing!

One bigger change I did is that I changed the distribution method of the robot model from parameter to topic, because that is how it should be done in ROS 2 IMO.

However, I wouldn't be against adding parameter support in the future, it might make some scenarios easier.

DLu · 2026-04-13T21:19:43Z

One other question I had: what are we supposed to be seeing in the test example?

peci1 · 2026-04-14T21:49:24Z

One other question I had: what are we supposed to be seeing in the test example?

No idea off the top of my head. I'm now on vacation until 24th, so I can't help until then...

DLu added 20 commits April 2, 2026 16:48

Interface syntax

b4a50db

Various Headers

48c8ea6

Logging interface and Clock Pointer

da8d4c0

Time and Duration

3e5daef

Logging

9d85b8c

Pub/Sub/ok

810d0a1

TF Fixes

c3d66e5

boost -> std locks

988018e

Rate

6bf0038

Parameter work

d2cf7f6

Update message definitions

e199190

Update package data

8ae0a79

Test infrastructure

e15affc

Dynamic parameters

ecc75aa

Service callback

3190660

Update example

fbe27c5

Remove has_value

00bd17e

Compiler warnings

4979086

Callback inequality

da2a25b

Figure out time travel

a2d902d

This was referenced Apr 2, 2026

Humble #27

Draft

ROS2 plans #22

Open

peci1 self-requested a review April 3, 2026 07:01

peci1 self-assigned this Apr 4, 2026

peci1 marked this pull request as draft April 6, 2026 11:06

peci1 marked this pull request as ready for review April 6, 2026 11:06

peci1 closed this Apr 6, 2026

peci1 reopened this Apr 6, 2026

Initialize rclcpp in tests

de64809

DLu commented Apr 8, 2026

View reviewed changes

Comment thread CMakeLists.txt Outdated

peci1 added 11 commits April 12, 2026 00:14

Removed utils/optional.hpp.

baaa466

Switched headers to using #pragma once.

2b8cdc4

Removed funding file.

013366e

It's probably not appropriate when I work on this project in my job :)

Switched to SPDX license headers, added Reuse compliance check.

546cea4

Fixed links in package.xml

47514e3

utils/bodies: simplified

c72a44d

utils/cloud, utils/tf2_sensor_msgs: moved to cras_cpp_common.

aaed570

utils/crop_box: removed, no longer needed

aa74b24

utils/set_utils: removed, no longer needed

073dacd

utils/string_utils: removed, no longer needed

9dc8b1c

utils/time_utils: removed, no longer needed

fbe03a2

peci1 added 6 commits April 12, 2026 18:44

test/test_filter_utils: Fixed the test.

d1cf49d

Reference and inspiration taken from https://github.com/moveit/moveit2/blob/main/moveit_ros/tests .

test/test_bodies: Fixed the test.

50ff9c3

test/test_shapes: Fixed the test.

214283f

test/test_ray_casting_shape_mask: Fixed the test.

56ea4bc

test/test_ray_casting_shape_mask: Cleaned up test.

33b319d

test/test_tf_frames_watchdog: Fixed test.

a6078cc

DLu commented Apr 13, 2026

View reviewed changes

peci1 added 2 commits April 13, 2026 07:39

test/test_robot_body_filter: Fixed test.

4371e32

test/test_robot_body_filter: Fixed test.

6e408a5

peci1 added 2 commits April 24, 2026 01:45

examples: Fixed and improved the examples.

93b0405

examples: Fixed configs.

0584c52

Uh oh!

Conversation

DLu commented Apr 2, 2026

Rote work

New implementations / updates

More problematic / opinionated stuff

Other notes

Uh oh!

peci1 commented Apr 4, 2026

Uh oh!

peci1 commented Apr 6, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

peci1 commented Apr 8, 2026

Uh oh!

Uh oh!

peci1 commented Apr 12, 2026

Uh oh!

DLu Apr 13, 2026

Choose a reason for hiding this comment

Uh oh!

peci1 Apr 13, 2026

Choose a reason for hiding this comment

Uh oh!

DLu Apr 13, 2026

Choose a reason for hiding this comment

Uh oh!

DLu Apr 13, 2026

Choose a reason for hiding this comment

Uh oh!

peci1 Apr 13, 2026

Choose a reason for hiding this comment

Uh oh!

peci1 commented Apr 13, 2026

Uh oh!

DLu commented Apr 13, 2026

Uh oh!

peci1 commented Apr 14, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

peci1 commented Apr 6, 2026 •

edited

Loading