feat(bridge_ros2): align REP-105 TF publishing with Nav2 conventions#132
Conversation
This brings the localization /tf publisher in line with the convention
used by AMCL, slam_toolbox, RTAB-Map and Cartographer, addressing three
issues observed when MOLA is consumed by Nav2-style downstream nodes:
1. REP-105 composition bug (fix). The inner factor of
map -> odom = (map -> base)(t_scan) * (base -> odom)(t)
was sampled at "latest" via waitForTransform(), which only supports
tf2::TimePoint{}. The two factors must be sampled at the same
instant or the published correction is biased by the odom-frame
motion accumulated during the localizer's processing latency. Switch
to tf_buffer_->lookupTransform(..., scan_tp). On lookup failure the
publish is skipped (an empty default-constructed TransformStamped
would inject TF_NO_FRAME_ID into every consumer's tf2 buffer).
2. transform_tolerance (new param, default 0.1s). The published stamp
is now node->now() + transform_tolerance so consumers can do
lookupTransform(map, base_link, now()) without
ExtrapolationException. Mirrors AMCL's transform_tolerance and
RTAB-Map's tf_tolerance. Uses the ROS clock so use_sim_time is
honored automatically.
3. transform_publish_period (new param, default 0.05s = 20 Hz). A
wall timer re-broadcasts the cached map -> odom independent of
localization update rate, keeping the TF buffer continuously fresh
even when the localizer runs slower than the consumer query rate.
Set to 0 to disable. Mirrors slam_toolbox's transform_publish_period
and RTAB-Map's tf_delay.
Backwards compat: to exactly preserve the prior (post-bugfix) stamping
behavior set transform_tolerance: 0 and transform_publish_period: 0.
publish_in_sim_time retains its meaning for all other publishers
(sensor TFs, odom messages, etc.); only the localization /tf stamp
source changed.
📝 WalkthroughWalkthroughThe changes introduce a caching and periodic rebroadcast mechanism for localization transforms in the ROS2 bridge. A configurable wall timer republishes the most recently computed transform with an updated timestamp offset, while the TF lookup for REP-105 correction now uses the localization scan time instead of the latest available time. Changes
Sequence DiagramsequenceDiagram
participant Timer as Wall Timer<br/>(periodic)
participant Node as ROS Node
participant Cache as Transform Cache<br/>(mutex-protected)
participant TFBroadcaster as TF Broadcaster
Timer->>Node: Check if broadcastCachedLocalizationTf()<br/>should execute
Note over Node: Enters broadcastCachedLocalizationTf()
Node->>Cache: Acquire lock & read<br/>cachedLocalizationTf_
alt Cache is populated
Cache-->>Node: Return cached Transform
Node->>Node: Update stamp to<br/>node->now() +<br/>transform_tolerance
Node->>TFBroadcaster: sendTransform(updated_tf)
TFBroadcaster-->>Node: Transform broadcasted
else Cache is empty
Cache-->>Node: Return (no-op)
end
Node->>Cache: Release lock
Estimated Code Review Effort🎯 4 (Complex) | ⏱️ ~50 minutes Possibly Related PRs
Poem
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 1
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
mola_bridge_ros2/src/BridgeROS2.cpp (1)
1649-1661:⚠️ Potential issue | 🟡 MinorCI blocker:
clang-formatstyle check failing on the stamp assignment.The pipeline (
CI clang-format) reports that lines 1650–1651 need reformatting. Collapse the multi-line assignment onto a single line per the project.clang-formatso CI passes:🎨 Proposed formatting fix
- tf.header.stamp = - node->now() + rclcpp::Duration::from_seconds(params_.transform_tolerance); + tf.header.stamp = node->now() + rclcpp::Duration::from_seconds(params_.transform_tolerance);Alternatively, run
/usr/bin/clang-format-14 --style=file -i mola_bridge_ros2/src/BridgeROS2.cpplocally.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@mola_bridge_ros2/src/BridgeROS2.cpp` around lines 1649 - 1661, The assignment to tf.header.stamp should be collapsed to a single line to satisfy clang-format: replace the current multi-line assignment in the block that calls rosNode() (around rosNode(), tf.header.stamp, params_.transform_tolerance and rclcpp::Duration::from_seconds) with a single-line expression (e.g. tf.header.stamp = node->now() + rclcpp::Duration::from_seconds(params_.transform_tolerance);) and then run clang-format (or rely on CI) to verify formatting.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@mola_bridge_ros2/src/BridgeROS2.cpp`:
- Around line 1595-1614: The code must guard against an epoch/zero scan
timestamp so tf_buffer_->lookupTransform isn't called with a zero time (which
tf2 treats as "latest"); before creating scan_tp or calling
tf_buffer_->lookupTransform check l.timestamp (or the result
scan_stamp.nanoseconds()) for zero and if zero either return/skip publishing
(matching existing behavior) or MRPT_LOG_THROTTLE_WARN_STREAM and return; also
include the actual scan timestamp (e.g., scan_stamp.seconds() or nanoseconds())
in the MRPT_LOG_THROTTLE_ERROR_STREAM message that references params_.odom_frame
and l.child_frame so timing issues are actionable (adjust the catch message near
MRPT_LOG_THROTTLE_ERROR_STREAM accordingly).
---
Outside diff comments:
In `@mola_bridge_ros2/src/BridgeROS2.cpp`:
- Around line 1649-1661: The assignment to tf.header.stamp should be collapsed
to a single line to satisfy clang-format: replace the current multi-line
assignment in the block that calls rosNode() (around rosNode(), tf.header.stamp,
params_.transform_tolerance and rclcpp::Duration::from_seconds) with a
single-line expression (e.g. tf.header.stamp = node->now() +
rclcpp::Duration::from_seconds(params_.transform_tolerance);) and then run
clang-format (or rely on CI) to verify formatting.
🪄 Autofix (Beta)
❌ Autofix failed (check again to retry)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: defaults
Review profile: CHILL
Plan: Pro
Run ID: f8489769-bc24-4089-8f7e-586de1b592da
📒 Files selected for processing (2)
mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.hmola_bridge_ros2/src/BridgeROS2.cpp
| tf2::Transform odomOnBase_tf; | ||
| try | ||
| { | ||
| const rclcpp::Time scan_stamp = mrpt::ros2bridge::toROS(l.timestamp); | ||
| const tf2::TimePoint scan_tp{std::chrono::nanoseconds(scan_stamp.nanoseconds())}; | ||
| const auto ref_to_trgFrame = | ||
| tf_buffer_->lookupTransform(l.child_frame, params_.odom_frame, scan_tp); | ||
| tf2::fromMsg(ref_to_trgFrame.transform, odomOnBase_tf); | ||
| } | ||
| catch (const tf2::TransformException& ex) | ||
| { | ||
| // Skip publishing entirely: broadcasting a default-constructed | ||
| // TransformStamped here would inject empty frame_ids into every | ||
| // subscriber's tf2 buffer (TF_NO_FRAME_ID / TF_SELF_TRANSFORM spam). | ||
| MRPT_LOG_THROTTLE_ERROR_STREAM( | ||
| 5.0, "publish_localization_following_rep105=true but could not resolve tf '" | ||
| << params_.odom_frame << "' -> '" << l.child_frame << "'; skipping TF publish."); | ||
| << params_.odom_frame << "' -> '" << l.child_frame << "' at scan stamp (" | ||
| << ex.what() << "); skipping TF publish."); | ||
| return; | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
tf2 BufferCore lookupTransform behavior when time argument is zero (TimePointZero) — does it return the latest available transform?
💡 Result:
Yes, in tf2 BufferCore, passing TimePointZero (time == 0) to lookupTransform returns the latest available transform. This is explicitly documented in the official tf2 API reference across multiple ROS 2 versions (e.g., latest, rolling, humble, foxy). For the single-time overload lookupTransform(target_frame, source_frame, time), the documentation states: "time – The time at which the value of the transform is desired. (0 will get the latest)". The same applies to target_time and source_time in the multi-time overload: "(0 will get the latest)". This behavior is consistent from ROS 1 documentation as well, where time=0 or rospy.Time indicates the latest common time.
Citations:
- 1: https://docs.ros2.org/latest/api/tf2/classtf2_1_1BufferCore.html
- 2: https://docs.ros.org/en/rolling/p/tf2/generated/doxygen/html/classtf2_1_1BufferCore.html
- 3: https://docs.ros.org/en/humble/p/tf2/generated/classtf2_1_1BufferCore.html
- 4: https://docs.ros2.org/foxy/api/tf2/classtf2_1_1BufferCore.html
Add guard against zero/epoch timestamp to prevent silent "latest" TF fallback.
scan_stamp = mrpt::ros2bridge::toROS(l.timestamp) produces nanoseconds() == 0 if LocalizationUpdate is published with a default-constructed timestamp. In tf2 BufferCore::lookupTransform, a zero timestamp is explicitly interpreted as "latest available", which silently re-introduces the timing bias you are removing in this PR — without warning.
Guard against a zero/epoch stamp by either skipping the publish (matching current skip-on-failure behavior) or logging a throttled warning:
Proposed guard
try
{
const rclcpp::Time scan_stamp = mrpt::ros2bridge::toROS(l.timestamp);
+ if (scan_stamp.nanoseconds() == 0)
+ {
+ MRPT_LOG_THROTTLE_WARN_STREAM(
+ 5.0,
+ "publish_localization_following_rep105=true but LocalizationUpdate has a "
+ "default/epoch timestamp; skipping REP-105 composition to avoid a silent "
+ "'latest' TF lookup.");
+ return;
+ }
const tf2::TimePoint scan_tp{std::chrono::nanoseconds(scan_stamp.nanoseconds())};
const auto ref_to_trgFrame =
tf_buffer_->lookupTransform(l.child_frame, params_.odom_frame, scan_tp);
tf2::fromMsg(ref_to_trgFrame.transform, odomOnBase_tf);
}Minor: the error message at lines 1610–1612 prints frame transform direction but omits scan_tp timestamp detail (e.g., scan_stamp.seconds()), making it less actionable when debugging TF buffer timing issues.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@mola_bridge_ros2/src/BridgeROS2.cpp` around lines 1595 - 1614, The code must
guard against an epoch/zero scan timestamp so tf_buffer_->lookupTransform isn't
called with a zero time (which tf2 treats as "latest"); before creating scan_tp
or calling tf_buffer_->lookupTransform check l.timestamp (or the result
scan_stamp.nanoseconds()) for zero and if zero either return/skip publishing
(matching existing behavior) or MRPT_LOG_THROTTLE_WARN_STREAM and return; also
include the actual scan timestamp (e.g., scan_stamp.seconds() or nanoseconds())
in the MRPT_LOG_THROTTLE_ERROR_STREAM message that references params_.odom_frame
and l.child_frame so timing issues are actionable (adjust the catch message near
MRPT_LOG_THROTTLE_ERROR_STREAM accordingly).
|
Note Autofix is a beta feature. Expect some limitations and changes as we gather feedback and continue to improve it. ❌ Failed to clone repository into sandbox. Please try again. |
|
@robin-zealrobotics Thanks! I guess you have already tested it with a real robot? Let me study this one carefully to make sure it works with both, REP105 and non-REP105 modes... |
|
Tested it in simulation, not a real robot yet. |
|
Superseded by #135 |
2 participants
Brings the localization
/tfpublisher in line with what AMCL / slam_toolbox / RTAB-Map do, so MOLA is a drop-in REP-105 citizen for Nav2 consumers. Three things in one commit:REP-105 composition fix.
(map -> base)(t_scan) * (base -> odom)(t)was usingwaitForTransform()which only does "latest" — biases the correction by the localizer's processing latency. Now usestf_buffer_->lookupTransform(..., scan_tp)so both factors sharel.timestamp. (Skip-on-failure behavior from fix(bridge_ros2): skip TF publish when REP-105 odom lookup fails #130 preserved.)transform_tolerance(default0.1s). Stamp =node->now() + toleranceso consumers canlookupTransform(map, base, now())withoutExtrapolationException. Same convention as AMCLtransform_tolerance/ RTAB-Maptf_tolerance. Usesrclcpp::Node::now()souse_sim_timeworks automatically.transform_publish_period(default0.05s = 20 Hz,0disables). Wall timer re-broadcasts the cachedmap -> odomindependent of localizer rate. Same as slam_toolboxtransform_publish_period/ RTAB-Maptf_delay.Backwards-compat: set both new params to
0to recover prior stamping behavior (modulo the t_scan fix).publish_in_sim_timestill controls all other publishers; only the localization/tfstamp source moved toNode::now().Companion PR for
mola_lidar_odometry(env-var plumbing) follows.Summary by CodeRabbit
transform_tolerance(timestamp offset in seconds) andtransform_publish_period(rebroadcast interval, disabled when set to 0)