Add param 'odometry_as_robot_pose_observation' to switch OdometryMsg … - #122
Merged
Conversation
…mapping to MRPT types
|
Note Reviews pausedIt looks like this branch is under active development. To avoid overwhelming you with review comments due to an influx of new commits, CodeRabbit has automatically paused this review. You can configure this behavior by changing the Use the following commands to manage reviews:
Use the checkboxes below for quick actions:
📝 WalkthroughWalkthroughAdds a runtime parameter to toggle ROS Odometry import between SE(3) Changes
Sequence Diagram(s)sequenceDiagram
participant ROS as ROS topic
participant Bridge as BridgeROS2
participant Frontend as MRPT Front-ends
ROS->>Bridge: nav_msgs/Odometry message
alt params_.odometry_as_robot_pose_observation == true
Bridge->>Bridge: convert to CObservationRobotPose (SE(3), 6x6 cov)
Bridge->>Frontend: publish CObservationRobotPose
else params_.odometry_as_robot_pose_observation == false
Bridge->>Bridge: convert to CObservationOdometry (2D + velocities)
Bridge->>Frontend: publish CObservationOdometry
end
Estimated code review effort🎯 3 (Moderate) | ⏱️ ~30 minutes Poem
🚥 Pre-merge checks | ✅ 1 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (1 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: 2
🤖 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/include/mola_bridge_ros2/BridgeROS2.h`:
- Around line 214-219: The field odometry_as_robot_pose_observation in
BridgeROS2.h is currently true by default causing Odometry to be converted to
CObservationRobotPose; change its default to false so the legacy
CObservationOdometry mapping remains the default (make the 3D pose path opt-in).
Update the declaration of the bool odometry_as_robot_pose_observation to
initialize to false, and adjust any related documentation/comments in the
BridgeROS2 class (and any config parsing or constructors that might override
defaults) to reflect the new opt-in behavior.
In `@mola_bridge_ros2/src/BridgeROS2.cpp`:
- Around line 513-550: The odometry-handling branch (guarded by
params_.odometry_as_robot_pose_observation) emits a CObservationRobotPose built
from o.pose without validating or transforming between o.header.frame_id and
o.child_frame_id; this can forward a local odom->base_link pose as if in
params_.reference_frame. Fix: before creating/sending the CObservationRobotPose,
use the same TF check/transform pattern as other handlers (e.g., call
waitForTransform() / lookup/transform to ensure o.header.frame_id ->
params_.reference_frame or to params_.base_link_frame), reject the message if
the frame pair is missing/mismatched, or transform the pose into
params_.reference_frame (and set sensorPose/sensorLabel accordingly), then call
sendObservationsToFrontEnds(obs).
🪄 Autofix (Beta)
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: 3297dde7-bcdd-4c5e-8890-f1489cc4a632
📒 Files selected for processing (2)
mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.hmola_bridge_ros2/src/BridgeROS2.cpp
There was a problem hiding this comment.
Actionable comments posted: 6
🧹 Nitpick comments (1)
mola_demos/demos/fake_imu_publisher.py (1)
21-25: Consider extractingyaw_to_quaternion()into a shared demo utility.This helper is duplicated across the three new demo publishers; a shared module would reduce drift and maintenance overhead.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@mola_demos/demos/fake_imu_publisher.py` around lines 21 - 25, Extract the duplicated yaw_to_quaternion(yaw: float) -> Quaternion helper into a shared demo utility module (e.g., demo_utils or imu_utils) and have each demo publisher import it instead of redefining it; update the three demo publisher files to import the function and any required types (Quaternion) from the shared module, remove the local definitions, and run tests/lint to ensure correct imports and no name collisions.
🤖 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_demos/demos/fake_imu_publisher.py`:
- Around line 41-50: Validate the rate_hz parameter before computing dt to avoid
division by zero or negative timer periods: after reading rate via
self.get_parameter('rate_hz').value, check that it is > 0 and handle invalid
values (e.g. log an error with self.get_logger().error and set a safe default
rate or raise rclpy.exceptions.RCLError), then compute self.dt = 1.0 / rate only
when valid; update any related logic that creates timers or relies on self.dt
(e.g., the publisher setup and timer creation in the FakeImuPublisher
initializer) to use the validated rate.
- Around line 59-60: The IMU message sets st.header.frame_id to the hardcoded
string 'base_link', which breaks TF lookups when BridgeROS2 uses a different
base frame; change this to use the configured base frame instead (e.g., replace
the literal with the instance config variable such as self.base_frame or a
BridgeROS2-provided getter like get_base_frame()) and keep st.child_frame_id =
self.frame_id; add a safe fallback to 'base_link' only if no configured base
frame is available and ensure the chosen symbol (self.base_frame or the bridge
getter) is initialized before use.
In `@mola_demos/demos/fake_visual_odom_publisher.py`:
- Around line 46-57: The code computes self.dt = 1.0 / rate from the parameter
rate_hz without validating it; add a validation step after retrieving rate (the
value from self.get_parameter('rate_hz')) to ensure rate is > 0, and if not,
handle it (raise a clear exception, log an error via the node logger, or
fallback to a safe default) before assigning self.dt; update the block around
the get_parameter('rate_hz') call and the self.dt assignment in
fake_visual_odom_publisher.py so the node never performs 1.0 / rate when rate is
zero or negative.
- Around line 98-103: The covariance vector only sets indices 0,7,14,35 leaving
the roll/pitch entries zero; update the pose covariance so all six diagonal
variances are set (position x/y/z and orientation roll/pitch/yaw) by assigning
self.noise_xyz**2 to indices 0,7,14 and self.noise_ang**2 to indices 21,28,35
before setting msg.pose.covariance, ensuring no axis has zero variance (refer to
the cov list, self.noise_xyz, self.noise_ang, and msg.pose.covariance).
In `@mola_demos/demos/fake_wheel_odom_publisher.py`:
- Around line 47-62: The code computes self.dt = 1.0 / rate using the value
retrieved by get_parameter('rate_hz'), which will crash or produce incorrect
behavior for zero or negative values; add a guard after rate =
self.get_parameter('rate_hz').value to validate that rate is > 0 (or clamp to a
sensible default like 1.0) and log or raise an error if invalid, then compute
self.dt only after validation; update the initialization that references rate,
self.dt, and any error logging to use this validated value (look for
get_parameter('rate_hz'), the local variable rate, and the assignment to
self.dt).
- Around line 99-103: The pose covariance only sets x/y/yaw and leaves z, roll,
pitch variances as zero which biases 3D estimators; update the block that builds
cov (used to set msg.pose.covariance) to also set the diagonal entries for z,
roll and pitch (indices 14 for z-z, 21 for roll-roll, 28 for pitch-pitch) to
sensible non-zero values (either a dedicated noise params like self.noise_z and
self.noise_rp or a large variance sentinel such as 1e6 to mark them as unknown)
so 3D consumers don’t treat those axes as zero-variance. Ensure you reference
and adjust cov, self.noise_xy, self.noise_yaw and msg.pose.covariance in the
same function in FakeWheelOdomPublisher.
---
Nitpick comments:
In `@mola_demos/demos/fake_imu_publisher.py`:
- Around line 21-25: Extract the duplicated yaw_to_quaternion(yaw: float) ->
Quaternion helper into a shared demo utility module (e.g., demo_utils or
imu_utils) and have each demo publisher import it instead of redefining it;
update the three demo publisher files to import the function and any required
types (Quaternion) from the shared module, remove the local definitions, and run
tests/lint to ensure correct imports and no name collisions.
🪄 Autofix (Beta)
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: 9b4927ca-a449-4f3f-8bbd-1d8c588094d5
📒 Files selected for processing (4)
mola_demos/CMakeLists.txtmola_demos/demos/fake_imu_publisher.pymola_demos/demos/fake_visual_odom_publisher.pymola_demos/demos/fake_wheel_odom_publisher.py
✅ Files skipped from review due to trivial changes (1)
- mola_demos/CMakeLists.txt
Comment on lines
+41
to
+50
| rate = self.get_parameter('rate_hz').value | ||
| self.accel_noise = self.get_parameter('accel_noise').value | ||
| self.gyro_noise = self.get_parameter('gyro_noise').value | ||
| self.wz = self.get_parameter('wz').value | ||
| self.vx = self.get_parameter('vx').value | ||
| self.frame_id = self.get_parameter('frame_id').value | ||
|
|
||
| self.pub = self.create_publisher(Imu, topic, 10) | ||
| self.dt = 1.0 / rate | ||
| self.yaw = 0.0 |
There was a problem hiding this comment.
Validate rate_hz to prevent invalid dt.
Protect against zero/negative rates before computing timer period.
Suggested fix
rate = self.get_parameter('rate_hz').value
+ if rate <= 0:
+ raise ValueError("Parameter 'rate_hz' must be > 0")🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@mola_demos/demos/fake_imu_publisher.py` around lines 41 - 50, Validate the
rate_hz parameter before computing dt to avoid division by zero or negative
timer periods: after reading rate via self.get_parameter('rate_hz').value, check
that it is > 0 and handle invalid values (e.g. log an error with
self.get_logger().error and set a safe default rate or raise
rclpy.exceptions.RCLError), then compute self.dt = 1.0 / rate only when valid;
update any related logic that creates timers or relies on self.dt (e.g., the
publisher setup and timer creation in the FakeImuPublisher initializer) to use
the validated rate.
Comment on lines
+59
to
+60
| st.header.frame_id = 'base_link' | ||
| st.child_frame_id = self.frame_id |
There was a problem hiding this comment.
Avoid hardcoding TF parent frame (base_link).
BridgeROS2 resolves IMU TF using its configured base frame; hardcoding this can cause transform lookup failures and dropped IMU observations when deployments use a different base frame.
Suggested fix
self.declare_parameter('frame_id', 'imu_link')
+ self.declare_parameter('base_frame_id', 'base_link')
@@
self.frame_id = self.get_parameter('frame_id').value
+ self.base_frame_id = self.get_parameter('base_frame_id').value
@@
- st.header.frame_id = 'base_link'
+ st.header.frame_id = self.base_frame_id🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@mola_demos/demos/fake_imu_publisher.py` around lines 59 - 60, The IMU message
sets st.header.frame_id to the hardcoded string 'base_link', which breaks TF
lookups when BridgeROS2 uses a different base frame; change this to use the
configured base frame instead (e.g., replace the literal with the instance
config variable such as self.base_frame or a BridgeROS2-provided getter like
get_base_frame()) and keep st.child_frame_id = self.frame_id; add a safe
fallback to 'base_link' only if no configured base frame is available and ensure
the chosen symbol (self.base_frame or the bridge getter) is initialized before
use.
There was a problem hiding this comment.
Actionable comments posted: 2
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
docs/source/ros2api.rst (1)
350-355:⚠️ Potential issue | 🟡 MinorSubsection number mismatch: "7.1" should be "8.1".
The parent section was renumbered to "8. Runtime dynamic reconfiguration" but the subsection still says "7.1. Runtime parameters...". Update for consistency.
Suggested fix
8. Runtime dynamic reconfiguration ---------------------------------------- MOLA modules may expose a subset of their parameters through an interface that allows runtime reconfiguration via ROS 2 service requests: -7.1. Runtime parameters for ``mola_lidar_odometry`` +8.1. Runtime parameters for ``mola_lidar_odometry`` ======================================================🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@docs/source/ros2api.rst` around lines 350 - 355, Update the subsection numbering to match the parent section: change the "7.1. Runtime parameters for ``mola_lidar_odometry``" heading to "8.1. Runtime parameters for ``mola_lidar_odometry``" so it matches the parent heading "8. Runtime dynamic reconfiguration"; locate the subsection title line containing "Runtime parameters for ``mola_lidar_odometry``" and replace the leading "7.1." with "8.1.".
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@docs/source/ros2api.rst`:
- Around line 259-262: Update the documented allowed values for msg_type to
match the implemented handlers: remove "Image" and add "GpsFix" in the list
shown for msg_type so it reflects actual supported types; this should align the
docs with internalAnalyzeTopicsToSubscribe() which lacks an Image subscription
handler but implements GpsFix handling.
In `@mola_bridge_ros2/src/BridgeROS2.cpp`:
- Around line 1924-1931: The clang-format violation is due to formatting around
the if (topic_name.empty()) block that logs via MRPT_LOG_DEBUG_STREAM
referencing output_sensor_label; run the project’s formatter (e.g.,
clang-format-14 --style=file) on the BridgeROS2.cpp file and reformat the
surrounding lines so the if block and MRPT_LOG_DEBUG_STREAM call conform to the
repo style, then commit the updated file to satisfy CI.
---
Outside diff comments:
In `@docs/source/ros2api.rst`:
- Around line 350-355: Update the subsection numbering to match the parent
section: change the "7.1. Runtime parameters for ``mola_lidar_odometry``"
heading to "8.1. Runtime parameters for ``mola_lidar_odometry``" so it matches
the parent heading "8. Runtime dynamic reconfiguration"; locate the subsection
title line containing "Runtime parameters for ``mola_lidar_odometry``" and
replace the leading "7.1." with "8.1.".
🪄 Autofix (Beta)
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: c8a9c4c2-75f0-4fd9-9cfe-936ad3d7cd3c
📒 Files selected for processing (2)
docs/source/ros2api.rstmola_bridge_ros2/src/BridgeROS2.cpp
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
…mapping to MRPT types
Summary by CodeRabbit
New Features
Bug Fixes
Documentation