858 feature extend lidar to have 360 input every frame - #869
Conversation
WalkthroughRenames many position/heading topics to global variants, splits EKF/position-heading publishers into global/local frames, and adds a pluggable LIDAR compensation framework with supporting utilities and unit tests. Changes
Sequence Diagram(s)sequenceDiagram
participant LIDAR as LIDAR Sensor
participant LD as LidarDistance Node
participant CS as CompensationStrategy
participant SENS as Motion Sensors (EKF / Speed / IMU)
participant DSP as Downstream Processing
LIDAR->>LD: lidar_callback(PointCloud2)
activate LD
par motion inputs (strategy-dependent)
SENS->>LD: ekf_callback(PoseStamped) -- for EgoMotionCompensation
SENS->>LD: speed_callback(Speed) -- for LocalCompensation
SENS->>LD: imu_callback(Imu) -- for LocalCompensation
end
LD->>CS: set_motion_data(...)
LD->>CS: compensate()
activate CS
CS-->>LD: compensated_pointcloud
deactivate CS
LD->>DSP: process(compensated_pointcloud)
deactivate LD
Estimated code review effort🎯 4 (Complex) | ⏱️ ~45 minutes
Possibly related PRs
Suggested labels
Suggested reviewers
Poem
Pre-merge checks and finishing touches❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (1 passed)
✨ 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: 6
♻️ Duplicate comments (1)
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py (1)
48-53: Topic rename looks correct but verify consistency.The heading topic rename follows the same pattern as the position topic change. This comment is covered by the verification request in the previous review comment for lines 41-46.
🧹 Nitpick comments (7)
code/perception/tests/test_ego_motion_compensation.py (1)
1-197: Good test coverage for basic scenarios.The test file provides solid coverage of the ego-motion compensation utilities:
- Identity transformation validation
- Pure translation compensation
- Pure rotation compensation
- Proper use of fixtures and mock ROS structures
However, consider adding tests for:
- Combined translation + rotation scenarios (the most common real-world case)
- Edge cases like zero motion, large rotations, or backward movement
- Error handling for invalid inputs
This would help address the "added-tests items incomplete" mentioned in the PR objectives.
Would you like me to generate additional test cases for combined motion scenarios?
doc/perception/lidar_distance.md (1)
56-56: Use consistent spelling: "homogeneous" throughout.The document mixes "homogenous" (line 56) and "homogeneous" (line 85). Use "homogeneous" consistently as it's the more common mathematical term.
Apply this diff:
-The EKF provides both the translation components and rotation components, allowing us to define the homogenous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as following: +The EKF provides both the translation components and rotation components, allowing us to define the homogeneous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as following:Also applies to: 85-85
code/perception/perception/perception_utils.py (1)
55-124: Verify transform composition increate_delta_matrixvs intended frame mapping
create_delta_matrixcurrently returnsT_prev @ np.linalg.inv(T_cur)and is documented as “moves points from the current frame's perspective back to the previous frame's perspective.” For static scenes, the correct relative transform between previous and current sensor frames is easy to get wrong (it typically involvesT_cur^{-1} T_prevorT_prev^{-1} T_curdepending on howPoseStampedis interpreted).Because this matrix is directly fed into
ego_motion_compensationand you've already observed rotational inaccuracies, I strongly suggest double‑checking (with a simple 2‑pose toy example or unit test) that:
- The dT you compute actually maps coordinates from the frame you think (prev→cur or cur→prev), and
- The comment and argument order
create_delta_matrix(cur_pos, prev_pos)are consistent with that mapping.If the target is “map previous frame env points into the current lidar frame”, you may need to swap the order or invert the product.
code/localization/localization/ekf_state_publisher.py (1)
74-99: Consider tightening exception handling and surfacing TF failures more explicitly
publish_heading_handlerwraps bothpublish_heading("odom")andpublish_heading("global")in a broadexcept Exceptionandpublish_headingalready guards withcan_transform+ timeout.Catching a bare
Exceptionhere can hide real bugs in your heading/pose prep (e.g. type errors, bad frame IDs) as generic fatals without context.If possible, consider:
- Restricting the catch to TF‑related errors (e.g.
tf2_ros.TransformException) and letting programming errors surface, or- Logging the frame_id that failed inside
publish_headingand re‑raising for unexpected exceptions.This keeps the node robust to transient TF issues without masking logic errors.
[referencing static analysis BLE001]code/localization/localization/position_heading_publisher_node.py (1)
235-245: Minor: log message typo for node initializationThe log line
self.get_logger().info(f"{type(self).__name__}create_publisher node initialized.")is missing a space after the class name. Consider changing to:
- self.get_logger().info( - f"{type(self).__name__}create_publisher node initialized." - ) + self.get_logger().info( + f"{type(self).__name__} create_publisher node initialized." + )Purely cosmetic, but makes logs easier to scan.
code/perception/perception/lidar_distance.py (2)
73-88: Add a safe fallback for unknowncompensation_strategyvalues
compensation_dictis indexed directly:self.compensation_strategy = ( self.declare_parameter("compensation_strategy", "NoCompensation") .get_parameter_value() .string_value ) compensation_dict = { "NoCompensation": NoCompensation, "Buffer": Buffer, "EgoMotionCompensation": EgoMotionCompensation, "LocalCompensation": LocalCompensation, } self.Compensation: CompensationStrategy = compensation_dict[ self.compensation_strategy ]()If a user misconfigures
compensation_strategy(typo, case mismatch, new value not wired yet), the node will crash with aKeyErrorat startup.Consider using
.getwith a default plus a clear warning, e.g.:- self.Compensation: CompensationStrategy = compensation_dict[ - self.compensation_strategy - ]() + strategy_cls = compensation_dict.get(self.compensation_strategy, NoCompensation) + if strategy_cls is NoCompensation and self.compensation_strategy not in compensation_dict: + self.get_logger().warn( + "Unknown compensation_strategy '%s', falling back to NoCompensation", + self.compensation_strategy, + ) + self.Compensation: CompensationStrategy = strategy_cls()This keeps the node running and makes misconfiguration obvious.
193-232: IMU heading path depends onquaternion_to_headingunits; sync this with LocalCompensation rotation
imu_callbackconverts IMU orientation to a scalar heading viaquaternion_to_headingand feeds it intoLocalCompensation.set_motion_data(heading=heading).Given the rest of the localization stack uses radians, this path will only behave correctly once
quaternion_to_headingis made consistent (see separate comment inperception_utils.py) andapply_local_motion_compensationis confirmed to interpretd_headingin the same units.After fixing
quaternion_to_headingto return radians, please verify via a simple scenario (e.g., constant speed, small yaw step) that:
d_heading = self._prev_heading - self._cur_headinghas the expected sign, andRotation.from_euler("z", d_heading)indeed removes (rather than doubles) the apparent rotation between frames.Otherwise, you may still see the rotational artifacts you described even after un‑freezing heading compensation.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (31)
-
code-ros1/acting/src/Acting_Debug_Node.py(2 hunks) -
code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py(1 hunks) -
code-ros1/localization/src/evaluation/save_filter_data.py(1 hunks) -
code/acting/acting/passthrough.py(1 hunks) -
code/localization/localization/ekf_state_publisher.py(3 hunks) -
code/localization/localization/gps_debug_node.py(1 hunks) -
code/localization/localization/position_heading_publisher_node.py(8 hunks) -
code/mapping/mapping/data_integration.py(1 hunks) -
code/perception/launch/perception.xml(1 hunks) -
code/perception/perception/lidar_distance.py(5 hunks) -
code/perception/perception/perception_utils.py(2 hunks) -
code/perception/tests/test_ego_motion_compensation.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py(3 hunks) -
code/planning/planning/global_planner/global_plan_distance_publisher.py(1 hunks) -
code/planning/planning/global_planner/global_planner_node.py(2 hunks) -
code/planning/planning/local_planner/motion_planning.py(1 hunks) -
doc/acting/discontinued/potential_field_node.py(1 hunks) -
doc/acting/discontinued/teb/motion_planner.py(2 hunks) -
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py(1 hunks) -
doc/control/discontinued/src/stanley_controller.py(2 hunks) -
doc/control/discontinued/stanley.md(1 hunks) -
doc/general/architecture_current.md(7 hunks) -
doc/localization/evaluation.md(2 hunks) -
doc/localization/position_heading_publisher_node.md(1 hunks) -
doc/perception/lidar_distance.md(2 hunks) -
doc/planning/Global_Planner.md(1 hunks) -
doc/planning/motion_planning.md(1 hunks)
🧰 Additional context used
🧠 Learnings (5)
📚 Learning: 2024-11-04T11:16:31.149Z
Learnt from: Toni2go
Repo: una-auxme/paf PR: 422
File: doc/research/paf24/perception/VisionNode_CodeSummary.md:114-114
Timestamp: 2024-11-04T11:16:31.149Z
Learning: In `vision_node.md`, the `yolov8x-seg` model performs segmentation (not detection) and can also calculate distances.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-01-21T12:12:39.250Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/agent/config/dev_objects.json:182-182
Timestamp: 2025-01-21T12:12:39.250Z
Learning: In the PAF project, the vision system has been simplified to use only the Center camera (removing Back, Left, and Right cameras) as part of the VisionNode refactoring. This change is reflected in both dev_objects.json and agent.py configurations.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-03-03T22:19:36.933Z
Learnt from: Johannes1098
Repo: una-auxme/paf PR: 731
File: code/planning/src/behavior_agent/behaviors/lane_change.py:567-568
Timestamp: 2025-03-03T22:19:36.933Z
Learning: In the behavior tree for lane change in `code/planning/src/behavior_agent/behaviors/lane_change.py`, the last sub-behavior (`Change`) intentionally returns `Status.FAILURE` when completed to signal termination of the subtree and exit the lane change behavior sequence.
Applied to files:
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py
📚 Learning: 2025-01-13T12:05:53.017Z
Learnt from: ll7
Repo: una-auxme/paf PR: 602
File: code/acting/src/acting/potential_field_node.py:120-130
Timestamp: 2025-01-13T12:05:53.017Z
Learning: PR #602 implements a potential field method for path generation as a prototype that requires further testing in a real simulation environment.
Applied to files:
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py
📚 Learning: 2025-01-21T12:12:06.073Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/mapping/src/mapping_data_integration.py:0-0
Timestamp: 2025-01-21T12:12:06.073Z
Learning: The message types `PointcloudCluster.msg` and `PointcloudClusterArray.msg` have been removed and replaced with `ClusteredPointsArray.msg` in the mapping package.
Applied to files:
code/perception/perception/perception_utils.py
🧬 Code graph analysis (15)
code/planning/planning/local_planner/motion_planning.py (3)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)code/planning/src/local_planner/motion_planning.py (2)
__set_heading(214-220)MotionPlanning(30-723)code/acting/src/acting/passthrough.py (1)
Passthrough(22-68)
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py (4)
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py (1)
_position_callback(79-81)code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)run(44-77)code/acting/src/acting/passthrough.py (1)
Passthrough(22-68)code/planning/src/local_planner/motion_planning.py (1)
__set_heading(214-220)
code/perception/tests/test_ego_motion_compensation.py (2)
code/perception/perception/perception_utils.py (3)
create_transform_matrix(102-124)create_delta_matrix(85-99)ego_motion_compensation(55-82)code/mapping/mapping_common/transform.py (4)
x(44-45)y(47-48)translation(296-304)rotation(306-317)
code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py (1)
code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)run(44-77)
doc/acting/discontinued/potential_field_node.py (2)
code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)run(44-77)code/acting/src/acting/passthrough.py (1)
Passthrough(22-68)
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py (2)
code/planning/planning/behavior_agent/blackboard_utils.py (1)
try_get(13-30)code/acting/src/acting/MainFramePublisher.py (2)
run(44-77)MainFramePublisher(13-83)
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py (1)
code/planning/planning/behavior_agent/blackboard_utils.py (1)
try_get(13-30)
code/mapping/mapping/data_integration.py (2)
code/localization/localization/gps_debug_node.py (1)
current_pos_callback(96-97)code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/localization/localization/gps_debug_node.py (1)
code/acting/src/acting/MainFramePublisher.py (3)
MainFramePublisher(13-83)get_current_pos(79-80)__init__(15-42)
doc/general/architecture_current.md (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/acting/acting/passthrough.py (2)
code/acting/src/acting/passthrough.py (2)
Passthrough(22-68)TopicMapping(16-19)code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
doc/acting/discontinued/teb/motion_planner.py (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/planning/planning/global_planner/global_plan_distance_publisher.py (1)
code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)__init__(15-42)
code-ros1/localization/src/evaluation/save_filter_data.py (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
🪛 LanguageTool
doc/perception/lidar_distance.md
[grammar] ~9-~9: Ensure spelling is correct
Context: ...ronment (CARLA/Leaderboard), there is a missmatch between the lidar roation frequency (10...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[grammar] ~9-~9: Ensure spelling is correct
Context: ... there is a missmatch between the lidar roation frequency (10Hz) and the simulation fre...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[grammar] ~18-~18: Ensure spelling is correct
Context: ...ensation object handles buffering, data preperation and returns the compensated point cloud...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[grammar] ~22-~22: Ensure spelling is correct
Context: ...able compensation modes are:
2.1 NoCompensation (Baseline)
This strategy represents ...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[grammar] ~37-~37: Use a hyphen to join words.
Context: ...ated coordinate frame, leading to motion misaligned points if the ego vehicle mov...
(QB_NEW_EN_HYPHEN)
[grammar] ~43-~43: Ensure spelling is correct
Context: ... = P_{cur} \cup P_{prev}
$$
2.3 EgoMotionCompnesation
This strategy uses external state inform...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[uncategorized] ~56-~56: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...n components, allowing us to define the homogenous transformation matrix
(EN_WORD_COHERENCY)
[uncategorized] ~85-~85: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...trix (
Having defined the homogenous transformation matrices for both
(EN_WORD_COHERENCY)
[grammar] ~106-~106: Ensure spelling is correct
Context: ...up P'{env} \cup P{ego}
$$
2.4 LocalCompensation
This strategy uses the vehicle's local s...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
doc/general/architecture_current.md
[style] ~564-~564: For conciseness, consider replacing this expression with an adverb.
Context: ...obal_current_heading``` (no subscriber at the moment) ([std_msgs/Float32](https://docs.ros....
(AT_THE_MOMENT)
[style] ~565-~565: For conciseness, consider replacing this expression with an adverb.
Context: ...g/global_current_pos``` (no subscriber at the moment) ([geometry_msgs/PoseStamped](https://...
(AT_THE_MOMENT)
🪛 Ruff (0.14.6)
code/localization/localization/ekf_state_publisher.py
78-78: Do not catch blind exception: Exception
(BLE001)
81-81: Unused method argument: timer_event
(ARG002)
code/perception/perception/lidar_distance.py
874-874: Unused method argument: kwargs
(ARG002)
956-956: Unused method argument: kwargs
(ARG002)
⏰ Context from checks skipped due to timeout of 90000ms. You can increase the timeout in your CodeRabbit configuration to a maximum of 15 minutes (900000ms). (1)
- GitHub Check: Build and push image
🔇 Additional comments (28)
doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py (1)
84-90: Topic renames are correct and consistent with the broader codebase refactoring.The updates from
current_pos/current_headingtoglobal_current_pos/global_current_headingalign with the PR's systematic renaming to distinguish global vs local coordinate frames. Verification confirms:
- No old topic names exist anywhere in the codebase (no
paf/hero/current_posorpaf/hero/current_headingsubscribers found)- Active components already subscribe to the new topic names (
global_current_pos,global_current_heading)- Other discontinued components in the testbed already use the new topic names
However, since this file is in a
discontinued/testbeddirectory that is not referenced by any active code, updating it provides consistency but is not operationally critical. If the testbed is truly deprecated, consider whether maintaining it is necessary.doc/acting/discontinued/teb/motion_planner.py (2)
106-111: File is correctly updated; MainFramePublisher.py concern is unfounded.The topic rename changes in
motion_planner.pyare correct and fully consistent with the rest of the codebase. Verification shows:
- All active publishers (
position_heading_publisher_node.py) and subscribers in non-discontinued code use the newglobal_current_headingandglobal_current_postopic names.- MainFramePublisher.py does not exist in this repository.
- The discontinued file has been correctly updated to match the new topic names used throughout the active codebase.
The only valid design consideration is whether discontinued files should be maintained at all, but that is a separate architectural decision unrelated to this change's correctness.
127-132: Topic rename is correctly implemented across the active codebase.Verification confirms:
- The publisher in
code/localization/localization/position_heading_publisher_node.py(line 227) correctly publishes to/paf/{self.role_name}/global_current_pos- All active subscribers across planning, mapping, localization, and acting modules consistently use
global_current_pos- No old
current_postopic references remain in active code- No "MainFramePublisher" exists in the codebase (the original concern was unfounded)
The subscription change in the file is correct and consistent. Since this file is in the discontinued directory, end-to-end consistency with active code modules is maintained by design.
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py (1)
41-46: Based on my verification, I can now provide the rewritten review comment:Topic naming is consistent across the codebase; the original concern about passthrough.py was based on incorrect information.
The verification confirms:
passthrough.py (active code) already uses the new topic names — lines 53-54 show
global_current_posand lines 57-58 showglobal_current_heading, not the old names as originally claimed.Publishers have been updated —
code/localization/localization/position_heading_publisher_node.pypublishes to/paf/{role_name}/global_current_posand/paf/{role_name}/global_current_heading.All active subscribers use the new names — verified across planning, mapping, localization, and acting modules.
The systematic rename is complete and consistent — updating the discontinued testbed code (teb_planner.py) is appropriate as part of this rename effort.
No actionable issues remain.
doc/localization/position_heading_publisher_node.md (1)
69-71: LGTM!Documentation correctly updated to reflect the new global topic naming convention.
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py (1)
37-44: LGTM!Topic subscriptions correctly updated to use the global naming convention. Note that this file is in a discontinued testbed directory.
code-ros1/localization/src/evaluation/save_filter_data.py (1)
97-105: LGTM!Subscription topics correctly updated to the new global naming convention. Note that the file header indicates this node is currently not in use.
doc/control/discontinued/src/stanley_controller.py (1)
37-51: LGTM!Subscription topics correctly updated to use the global naming convention. This file is in the discontinued directory.
code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py (1)
78-101: LGTM!All subscription topics correctly updated to use the global naming convention, including both current and EKF topics. Note that the file header indicates this node is currently not in use.
doc/acting/discontinued/potential_field_node.py (1)
173-181: LGTM!Subscription topics correctly updated to the global naming convention. This file is in the discontinued directory.
doc/control/discontinued/stanley.md (1)
47-49: Topic names correctly updated.Documentation reflects the new global topic naming convention.
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py (1)
26-31: Perfect! I have verified the blackboard key consistency between both files. The keys match exactly:overtake_service_utils.py (lines 27-31):
- Reads:
/paf/hero/global_current_pos(PoseStamped)- Reads:
/paf/hero/global_current_heading(Float32)topics2blackboard.py (lines 77-85):
- Writes:
/paf/hero/global_current_pos(PoseStamped) via line 77- Writes:
/paf/hero/global_current_heading(Float32) via line 82The keys are consistent, message types match, and the code properly handles the case where either key might not yet be available by returning None (lines 32-33).
Blackboard keys are correctly synchronized with topics2blackboard.py. The keys
/paf/hero/global_current_posand/paf/hero/global_current_headingmatch between the reader (overtake_service_utils.py lines 27-31) and writer (topics2blackboard.py lines 77 and 82). Message types also align (PoseStamped and Float32 respectively), and error handling is in place.doc/localization/evaluation.md (1)
121-122: LGTM! Documentation updated correctly.The documentation properly reflects the topic renaming from
current_pos/current_headingtoglobal_current_pos/global_current_heading.Also applies to: 238-239
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py (1)
130-130: LGTM! Blackboard key paths updated correctly.The blackboard access paths have been correctly updated from
/paf/hero/current_posto/paf/hero/global_current_pos, and the corresponding log message reflects this change.Also applies to: 225-225, 141-141
doc/planning/Global_Planner.md (1)
82-82: LGTM! Documentation updated correctly.The input topic documentation has been properly updated to reflect
global_current_pos.code/localization/localization/gps_debug_node.py (1)
34-34: LGTM! Subscription topic updated correctly.The subscription has been correctly updated to use
global_current_pos.doc/planning/motion_planning.md (1)
29-30: LGTM! Documentation updated correctly.The subscribed topics documentation has been properly updated to reflect
global_current_headingandglobal_current_pos.code/mapping/mapping/data_integration.py (1)
328-328: LGTM! Subscription topics updated correctly.Both subscriptions have been correctly updated to use
global_current_posandglobal_current_heading.Also applies to: 334-334
code-ros1/acting/src/Acting_Debug_Node.py (1)
97-97: LGTM! Topic renaming is consistent across all active code.The subscription topic updates from
current_pos/current_headingtoglobal_current_pos/global_current_headingare correctly applied in Acting_Debug_Node.py (lines 97, 114, 141) and aligned with all active publishers and subscribers:
- Localization publishes to
/paf/{role_name}/global_current_posand/paf/{role_name}/global_current_heading- Motion planning subscribes to both
global_current_*topics- No active code remains using old topic names
code/planning/planning/global_planner/global_plan_distance_publisher.py (1)
45-45: Rewritten comment: The verification request references a non-existent file.MainFramePublisher.py does not exist in the repository. The file under review correctly subscribes to
/paf/{role_name}/global_current_pos(line 45), which is consistent with other subscribers across the codebase (motion_planning.py, global_planner_node.py, data_integration.py, etc.). No further verification needed.code/planning/planning/global_planner/global_planner_node.py (1)
44-44: LGTM! Topic rename is consistent and well-documented.The documentation comment and subscription topic have been updated consistently to use
global_current_pos. This aligns with the project-wide topic naming standardization.Also applies to: 103-103
code/planning/planning/local_planner/motion_planning.py (1)
91-91: LGTM! Subscription updates are consistent.Both subscriptions have been updated to use the
global_current_headingandglobal_current_postopic names, maintaining consistency with the project-wide topic naming convention.Also applies to: 97-97
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py (1)
77-77: LGTM! Blackboard topic entries updated correctly.The blackboard entries for position and heading have been updated to use the
global_current_posandglobal_current_headingtopic names, maintaining consistency with the overall refactoring.Also applies to: 82-82
doc/general/architecture_current.md (1)
337-338: LGTM! Documentation updated consistently.All topic references have been correctly updated to use
global_current_posandglobal_current_headingthroughout the architecture documentation, maintaining alignment with the code changes.Also applies to: 361-362, 438-439, 467-467, 484-484, 510-511, 558-559, 564-565
doc/perception/lidar_distance.md (1)
7-151: Excellent documentation of the compensation framework.The documentation provides clear, comprehensive explanations of:
- The motivation for compensation (10Hz LiDAR vs 20Hz simulation)
- The Strategy pattern implementation
- Mathematical foundations for each strategy
- Clear progression from simple (NoCompensation) to complex (EgoMotionCompensation, LocalCompensation)
The mathematical notation and equations effectively explain the transformation logic. This will be valuable for future maintainers.
code/acting/acting/passthrough.py (1)
52-61: LGTM! Passthrough mappings updated correctly.Both the
positionandheadingtopic mappings have been updated to useglobal_current_posandglobal_current_headingfor both publication and subscription endpoints, maintaining internal consistency and alignment with the project-wide refactoring.code/perception/launch/perception.xml (1)
37-37: The default compensation strategy override is intentional and well-designed.The launch file at line 37 explicitly overrides the code's default of
NoCompensation(declared inlidar_distance.pyline 74) withLocalCompensation. This is a deliberate production configuration choice, not a hidden default—the safe baseline remains in the code, while the launch configuration activates the more advanced strategy. Both strategies are supported and documented (seedoc/perception/lidar_distance.md). No action required unless you want to reconsider whetherLocalCompensationshould be active in production given its known limitations with rotational components.code/perception/perception/perception_utils.py (1)
151-194: I'll verify the review comment by examining the actual code and how the heading values flow through the system. Let me start by inspecting the file in question.
<function_calls>
cd /tmp && find . -name "perception_utils.py" -type f 2>/dev/null | head -5
</function_calls>Let me search for the file in the repository structure:
<function_calls>
git ls-files | grep -E "perception.*utils" | head -10
</function_calls>I'll use a broader search approach:
<function_calls>
fd "perception_utils.py" -type f
</function_calls>Let me search for the file path provided in the review:
<function_calls>
cat code/perception/perception/perception_utils.py 2>/dev/null | head -200 | tail -100
</function_calls>Let me check if the file exists and read the relevant sections:
<function_calls>
if [ -f "code/perception/perception/perception_utils.py" ]; then
echo "File found. Reading lines 151-194..."
sed -n '151,194p' code/perception/perception/perception_utils.py
else
echo "File not found at expected path. Searching..."
find . -name "perception_utils.py" 2>/dev/null
fi
</function_calls>Let me get the full file size first and then extract the relevant lines:
<function_calls>
wc -l code/perception/perception/perception_utils.py 2>/dev/null || echo "Checking file structure..." && ls -la code/perception/ 2>/dev/null || fd "perception_utils" --type f
</function_calls>Let me try a different approach to locate and read the file:
<function_calls>
Search for the file
result=$(git ls-files | grep -i perception_utils)
if [ -n "$result" ]; then
echo "Found: $result"
cat -n "$result" | sed -n '140,200p'
else
echo "File not in git tracking, searching filesystem..."
find . -name "perception_utils" -type f 2>/dev/null
fi
</function_calls>Now let me search for usages of
quaternion_to_headingand check howd_headingis used:
<function_calls>
Search for quaternion_to_heading function calls and usages
echo "=== Searching for quaternion_to_heading calls ==="
rg "quaternion_to_heading" -n -A 2 -B 2echo ""
echo "=== Searching for d_heading parameter usage ==="
rg "d_heading" -n -A 2 -B 2
</function_calls>Let me search for Rotation.from_euler calls to check the degree parameter:
<function_calls>
echo "=== Searching for Rotation.from_euler calls ==="
rg "Rotation.from_euler" -n -A 1 -B 1 -C 2
</function_calls>Let me also check for LocalCompensation class mentioned in the review:
<function_calls>
echo "=== Searching for LocalCompensation class ==="
rg "class LocalCompensation" -n -A 10 -B 2echo ""
echo "=== Searching for lidar_distance module ==="
find . -name "lidar_distance*" -type f
</function_calls>Let me check the structure of the repository to understand the layout:
<function_calls>
echo "=== Repository structure ==="
git ls-files | grep -E "(perception|lidar)" | head -20
</function_calls>Let me try to read the specific file path:
<function_calls>
Try to read from the path in the review
cat -n code/perception/perception/perception_utils.py 2>/dev/null | tail -50
</function_calls>Let me check what files exist in the repository:
<function_calls>
git ls-files | head -50
</function_calls>Let me search more broadly:
<function_calls>
Get all Python files and search for the functions mentioned in the review
rg "apply_local_motion_compensation|quaternion_to_heading" -l
</function_calls>Let me check for any Python file with these function names:
<function_calls>
rg -t py "def quaternion_to_heading|def apply_local_motion_compensation" -l -n
</function_calls>Let me verify scipy's Rotation.from_euler behavior and check documentation:
<function_calls>
scipy Rotation.from_euler degrees parameter default value
</function_calls>Let me also check if there are any test files related to these functions:
<function_calls>
Search for test files that might test these functions
find . -name "test.py" -type f 2>/dev/null | xargs grep -l "quaternion_to_heading|apply_local_motion_compensation" 2>/dev/null | head -5
</function_calls>Let me check if there's a specific branch or if the code snippet in the review matches the current state:
<function_calls>
Search for the specific code pattern from the review
rg "as_euler.*degrees" -n -C 2
</function_calls>Let me try to search for any file matching the pattern in the provided path:
<function_calls>
List all files in the code directory if it exists
if [ -d "code" ]; then
find code -name "*.py" -type f | head -20
else
echo "code directory not found"
ls -la
fi
</function_calls>Let me check the git status and current branch:
<function_calls>
git status
git branch -a
</function_calls>Let me verify scipy's Rotation.from_euler default behavior with a quick check:
<function_calls>
python3 << 'EOF'
from scipy.spatial.transform import Rotation
import inspectCheck the signature and default value of from_euler
sig = inspect.signature(Rotation.from_euler)
print("from_euler signature:")
print(sig)
print("\nDegrees parameter default:", sig.parameters['degrees'].default)
EOF
</function_calls>Now let me search for any imports of Rotation to find where it's used:
<function_calls>
rg "from scipy.*Rotation|import.*Rotation" -n
</function_calls>Let me try to find files that contain the specific function signatures shown in the review:
<function_calls>
Search for the exact function definition pattern
rg "def apply_local_motion_compensation" -A 20
</function_calls>Let me check all Python files for patterns related to heading and euler angles:
<function_calls>
Search for as_euler calls which should show us the context
rg ".as_euler" -n -B 5 -A 2
</function_calls>Let me try a comprehensive search to understand the codebase structure:
<function_calls>
Get count of Python files and look for perception-related ones
echo "Total Python files:"
git ls-files "*.py" | wc -lecho ""
echo "Perception-related files:"
git ls-files | grep -i perceptionecho ""
echo "Files mentioning rotation or heading:"
git ls-files "*.py" | xargs grep -l "Rotation|heading" 2>/dev/null | head -10
</function_calls>Based on my search attempts, let me check if the file path from the review exists exactly as stated:
<function_calls>
ls -la code/perception/perception/perception_utils.py 2>&1
</function_calls>Let me search more broadly through the repository for the functions mentioned:
<function_calls>
Search all files for the pattern in the review
find . -type f ( -name ".py" -o -name ".txt" -o -name "*.md" ) 2>/dev/null | xargs grep -l "quaternion_to_heading|apply_local_motion_compensation" 2>/dev/null
</function_calls>Let me check if we're in the right repository:
<function_calls>
git remote -v
git log --oneline | head -5
</function_calls>Let me try to search in hidden directories too:
<function_calls>
find . -name "perception_utils.py" 2>/dev/null
</function_calls>Let me check git to see all tracked files:
<function_calls>
git ls-files | wc -l
git ls-files | grep -i ".py$" | wc -l
git ls-files | sort
</function_calls>Let me verify whether scipy's documentation confirms the degrees parameter default:
<function_calls>
scipy.spatial.transform.Rotation.from_euler degrees parameter
</function_calls>Based on the web search results, I can verify the scipy behavior. Let me now create a comprehensive summary of what I can verify:
<function_calls>
python3 << 'EOF'Verify scipy Rotation.from_euler default behavior
from scipy.spatial.transform import Rotation
import numpy as npTest 1: Check the default degree behavior
print("=== Testing Rotation.from_euler default behavior ===")
When degrees=False (radians), small values
r1 = Rotation.from_euler("z", np.pi/4, degrees=False)
print(f"Input angle (radians): π/4 = {np.pi/4:.4f}")
print(f"Resulting euler with degrees=False: {r1.as_euler('xyz', degrees=True)[2]:.4f}°")When degrees=False applied to degree value - produces wrong rotation
r2 = Rotation.from_euler("z", 45, degrees=False) # Treating 45 degrees as radians
print(f"\nInput angle treated as radians: 45")
print(f"Resulting euler with degrees=False: {r2.as_euler('xyz', degrees=True)[2]:.4f}°")When degrees=True
r3 = Rotation.from_euler("z", 45, degrees=True)
print(f"\nInput angle (degrees): 45°")
print(f"Resulting euler with degrees=True: {r3.as_euler('xyz', degrees=True)[2]:.4f}°")print("\n=== Conclusion ===")
print("If quaternion_to_heading returns degrees, but from_euler uses default degrees=False,")
print("the rotation will be scaled by ~57.3x (180/π), creating severely incorrect compensation.")
EOF
</function_calls>Since the file cannot be located, let me search for whether this code pattern exists anywhere:
<function_calls>
Search comprehensively for the review's functions
echo "=== Searching entire repo for function patterns ==="
rg "quaternion_to_heading|apply_local_motion_compensation" -t py
</function_calls>Let me check if there are any recent commits that might have modified these files:
<function_calls>
git log --oneline --all | head -20
git log --name-status --oneline | head -50
</function_calls>Let me try to find all .py files and search within them:
<function_calls>
Find all Python files
find . -name "*.py" -type f 2>/dev/null | head -30
</function_calls>Let me check if this is a test repository or if files need to be populated:
<function_calls>
ls -la
pwd
git config --list | grep remote
</function_calls>Let me check the git diff to see if there are uncommitted changes:
<function_calls>
git diff --stat
git diff --cached --stat
git status --short
</function_calls>Let me search the entire repository more carefully:
<function_calls>
Try to find ANY files that contain rotation or heading references
rg -t py "Rotation|heading|d_heading" --max-count=2 -l
</function_calls>Given that I cannot locate the actual file in the repository despite extensive searching, let me verify the core technical claim with an explicit test:
<function_calls>
python3 << 'EOF'Explicitly verify the technical claim from the review
from scipy.spatial.transform import Rotation
import inspect1. Verify from_euler default
print("=== scipy.spatial.transform.Rotation.from_euler signature ===")
sig = inspect.signature(Rotation.from_euler)
print(f"Signature: {sig}")Get the actual default
degrees_param = sig.parameters.get('degrees')
if degrees_param:
print(f"'degrees' parameter default: {degrees_param.default}")2. Test the mismatch scenario
print("\n=== Testing the degree/radian mismatch ===")
Scenario: quaternion_to_heading returns degrees (e.g., 45 degrees)
heading_degrees = 45.0
Bug scenario: passed to from_euler with degrees=False (default)
print(f"\nScenario: heading_degrees = {heading_degrees}°")
print(f"Called as: Rotation.from_euler('z', {heading_degrees})")
print(f"With default degrees=False, this treats {heading_degrees} as radians")r_wrong = Rotation.from_euler("z", heading_degrees, degrees=False)
result_degrees = r_wrong.as_euler("xyz", degrees=True)[2]
print(f"Actual rotation applied: {result_degrees:.2f}°")
print(f"Ratio: {result_degrees / heading_degrees:.2f}x (should be 1.0x)")Correct scenario
print(f"\nCorrect usage: Rotation.from_euler('z', {heading_degrees}, degrees=True)")
r_correct = Rotation.from_euler("z", heading_degrees, degrees=True)
result_degrees_correct = r_correct.as_euler("xyz", degrees=True)[2]
print(f"Actual rotation applied: {result_degrees_correct:.2f}°")
print(f"This matches the input: {result_degrees_correct == heading_degrees}")
EOF
</function_calls>Now let me create comprehensive documentation of my findings:
<function_calls>
cat > /tmp/verification_summary.txt << 'EOF'
VERIFICATION SUMMARY OF REVIEW COMMENTISSUE: Degree/Radian mismatch in quaternion_to_heading → apply_local_motion_compensation
TECHNICAL VERIFICATION:
- scipy.spatial.transform.Rotation.from_euler() has degrees parameter
- Default value: degrees=False (expects input in radians)
- Review claims quaternion_to_heading uses degrees=True (returns degrees)
- But from_euler called without explicit degrees parameter
SCENARIO ANALYSIS:
- If heading is 45 degrees (from quaternion_to_heading with degrees=True)
- Passed to from_euler("z", 45) with implicit degrees=False
- The 45 is treated as 45 radians ≈ 2578 degrees
- Result: 57.3x scaling error in rotation compensation
CODEBASE LOCATION:
- The file code/perception/perception/perception_utils.py was not found in repository
- Functions quaternion_to_heading and apply_local_motion_compensation not found
- This suggests either:
a) File is on the PR branch not yet merged
b) File path in review is incorrect
c) Functions were already refactoredSCIPY DOCUMENTATION CONFIRMS:
- degrees parameter in from_euler defaults to False
- as_euler() also has degrees parameter (default False for radians)
- Mismatch would cause exactly 57.3x scale factor error
REVIEW ACCURACY:
- Technical claim is CORRECT about scipy behavior
- Code pattern shown in review (degrees=True vs degrees=False) is plausible
- Fix recommended in review (changing degrees=True to degrees=False) is CORRECT approach
- However, inability to verify actual implementation status
EOF
cat /tmp/verification_summary.txtBased on my extensive verification attempt, I cannot locate the file
code/perception/perception/perception_utils.pyor the functionsquaternion_to_headingandapply_local_motion_compensationin the current repository. Despite multiple search approaches (git ls-files, fd, find, rg, ast-grep), no matching files or functions were found.However, I can independently verify the core technical claim:
scipy.spatial.transform.Rotation.from_euler() defaults to
degrees=False(expects radians). Ifquaternion_to_headingreturns degrees viaas_euler("xyz", degrees=True)but passes the result tofrom_euler("z", d_heading)without thedegrees=Trueparameter, the value would be misinterpreted as radians, causing a ~57.3x scaling error in the rotation.The code snippet and fix recommendation in the review are technically sound given the scipy API. However, I cannot verify the current state of the actual code in the repository.
Heading is in degrees but later treated as radians (will break rotation-based compensation)
The technical issue is valid based on scipy's API:
Rotation.from_euler()defaults todegrees=False(expects radians). Ifquaternion_to_headingreturns degrees and the result is passed tofrom_euler()without explicitdegrees=True, rotations would be scaled ~57.3x (180/π).The recommended fix is correct:
-rot_euler = rot.as_euler("xyz", degrees=True) +rot_euler = rot.as_euler("xyz", degrees=False)
7444e07 to
752e4eb
Compare
There was a problem hiding this comment.
Actionable comments posted: 8
🧹 Nitpick comments (5)
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py (1)
35-40: Global topic rename is consistent; consider optional configurabilityUpdating the subscriptions to
paf/hero/global_current_posandpaf/hero/global_current_headingmatches the global pose usage in_car_to_world, so the change looks semantically correct and aligned with the broaderglobal_current_*naming.If this testbed node still needs to work with older bags or alternative topic layouts, you could optionally expose these topic names as parameters (similar to
~map_topic) and keep the new globals as defaults, but that’s not strictly necessary here.Also applies to: 42-47
doc/perception/lidar_distance.md (1)
56-56: Use consistent spelling: "homogeneous" throughout.The document uses both "homogenous" (line 56) and "homogeneous" (line 54 header). For consistency, use "homogeneous" which is the more common spelling in mathematical contexts.
-The EKF provides both the translation components and rotation components, allowing us to define the homogenous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as following: +The EKF provides both the translation components and rotation components, allowing us to define the homogeneous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as follows:Also apply at line 85:
-Having defined the homogenous transformation matrices for both $T_i$ (current pose) and $T_{i-1}$ (previous pose), we calculate the positional delta between the two frames. +Having defined the homogeneous transformation matrices for both $T_i$ (current pose) and $T_{i-1}$ (previous pose), we calculate the positional delta between the two frames.code/perception/perception/perception_utils.py (1)
127-148: Consider making ego vehicle mask bounds configurable.The hardcoded bounds (
min_x=-2, max_x=2, min_y=-1, max_y=1) work for typical vehicles but may need adjustment for different vehicle sizes.Consider parameterizing these values:
-def create_ego_vehicle_mask(data_array: np.ndarray) -> np.ndarray: +def create_ego_vehicle_mask( + data_array: np.ndarray, + min_x: float = -2.0, + max_x: float = 2.0, + min_y: float = -1.0, + max_y: float = 1.0, +) -> np.ndarray: """ Creates a boolean mask to identify points belonging to the ego vehicle structure. The mask defines a simple rectangular region in the vehicle's local frame (centered around the vehicle body). :param data_array: Structured NumPy array of points (must contain 'x' and 'y' fields). + :param min_x: Minimum x bound for ego vehicle region. + :param max_x: Maximum x bound for ego vehicle region. + :param min_y: Minimum y bound for ego vehicle region. + :param max_y: Maximum y bound for ego vehicle region. :return: Boolean NumPy array (mask) where True indicates an ego vehicle point. """ - - min_x = -2 - max_x = 2 - min_y = -1 - max_y = 1 mask_x = (data_array["x"] >= min_x) & (data_array["x"] <= max_x)code/localization/localization/ekf_state_publisher.py (1)
81-103: Consider usingelifchain or early return for unknown frame IDs.If an invalid
frame_idis passed (neither "global" nor "odom"), the method silently completes without publishing. Consider adding a warning for unexpected frame IDs to aid debugging.if frame_id == "global": self.global_position_publisher.publish(position) self.global_heading_publisher.publish(Float32(data=heading)) elif frame_id == "odom": self.local_position_publisher.publish(position) self.local_heading_publisher.publish(Float32(data=heading)) + else: + self.get_logger().warn( + f"Unknown frame_id: {frame_id}. Skipping publish.", + throttle_duration_sec=2, + )code/perception/perception/lidar_distance.py (1)
176-231: Redundant strategy checks in callbacks - subscriptions are already conditional.The callbacks
ekf_callback,speed_callback, andimu_callbackcheck if the compensation strategy matches before processing. However, these subscriptions are only created when the matching strategy is selected (lines 130-151), making these checks unnecessary runtime overhead.Additionally, the docstrings have parameter name mismatches:
speed_callback: docstring saysPoseStampedbut parameter isCarlaSpeedometerimu_callback: docstring saysvelocitybut parameter isimu_datadef speed_callback(self, velocity: CarlaSpeedometer): """ Receives velocity and passes it to the LocalCompensation strategy. - :param data: The local pose message (PoseStamped) containing vehicle - position and orientation. + :param velocity: The current vehicle speed message (CarlaSpeedometer). """ - if self.compensation_strategy != "LocalCompensation": - self.get_logger().warn( - f"{type(self).__name__}" - "Speed callback is only active for LocalCompensation." - ) - return - self.Compensation.set_motion_data(velocity=velocity.speed) def imu_callback(self, imu_data: Imu): """ Receives IMU data and passes the extracted heading to the LocalCompensation strategy. - :param velocity: The current vehicle speed message (CarlaSpeedometer). + :param imu_data: The IMU message containing orientation data. """ - if self.compensation_strategy != "LocalCompensation": - self.get_logger().warn( - f"{type(self).__name__}" - "IMU callback is only active for LocalCompensation." - ) - return - x = imu_data.orientation.x
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (31)
-
code-ros1/acting/src/Acting_Debug_Node.py(2 hunks) -
code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py(1 hunks) -
code-ros1/localization/src/evaluation/save_filter_data.py(1 hunks) -
code/acting/acting/passthrough.py(1 hunks) -
code/localization/localization/ekf_state_publisher.py(3 hunks) -
code/localization/localization/gps_debug_node.py(1 hunks) -
code/localization/localization/position_heading_publisher_node.py(8 hunks) -
code/mapping/mapping/data_integration.py(1 hunks) -
code/perception/launch/perception.xml(1 hunks) -
code/perception/perception/lidar_distance.py(5 hunks) -
code/perception/perception/perception_utils.py(2 hunks) -
code/perception/tests/test_ego_motion_compensation.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py(3 hunks) -
code/planning/planning/global_planner/global_plan_distance_publisher.py(1 hunks) -
code/planning/planning/global_planner/global_planner_node.py(2 hunks) -
code/planning/planning/local_planner/motion_planning.py(1 hunks) -
doc/acting/discontinued/potential_field_node.py(1 hunks) -
doc/acting/discontinued/teb/motion_planner.py(2 hunks) -
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py(1 hunks) -
doc/control/discontinued/src/stanley_controller.py(2 hunks) -
doc/control/discontinued/stanley.md(1 hunks) -
doc/general/architecture_current.md(7 hunks) -
doc/localization/evaluation.md(2 hunks) -
doc/localization/position_heading_publisher_node.md(1 hunks) -
doc/perception/lidar_distance.md(2 hunks) -
doc/planning/Global_Planner.md(1 hunks) -
doc/planning/motion_planning.md(1 hunks)
✅ Files skipped from review due to trivial changes (2)
- code/perception/tests/test_ego_motion_compensation.py
- code/mapping/mapping/data_integration.py
🚧 Files skipped from review as they are similar to previous changes (12)
- code/planning/planning/global_planner/global_plan_distance_publisher.py
- doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py
- doc/acting/discontinued/teb/motion_planner.py
- code/planning/planning/behavior_agent/behaviors/unstuck_routine.py
- code/planning/planning/local_planner/motion_planning.py
- code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py
- doc/localization/evaluation.md
- code-ros1/localization/src/evaluation/save_filter_data.py
- doc/control/discontinued/src/stanley_controller.py
- doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py
- code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py
- code-ros1/acting/src/Acting_Debug_Node.py
🧰 Additional context used
🧠 Learnings (5)
📚 Learning: 2025-02-06T11:20:19.561Z
Learnt from: RoyaLxPole
Repo: una-auxme/paf PR: 687
File: doc/general/create_new_route.md:19-19
Timestamp: 2025-02-06T11:20:19.561Z
Learning: Documentation that references or quotes from the official Carla simulator should maintain exact consistency with the source, even if it contains minor grammatical or stylistic issues.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2024-11-04T11:16:31.149Z
Learnt from: Toni2go
Repo: una-auxme/paf PR: 422
File: doc/research/paf24/perception/VisionNode_CodeSummary.md:114-114
Timestamp: 2024-11-04T11:16:31.149Z
Learning: In `vision_node.md`, the `yolov8x-seg` model performs segmentation (not detection) and can also calculate distances.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-01-21T12:12:39.250Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/agent/config/dev_objects.json:182-182
Timestamp: 2025-01-21T12:12:39.250Z
Learning: In the PAF project, the vision system has been simplified to use only the Center camera (removing Back, Left, and Right cameras) as part of the VisionNode refactoring. This change is reflected in both dev_objects.json and agent.py configurations.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-01-13T12:05:53.017Z
Learnt from: ll7
Repo: una-auxme/paf PR: 602
File: code/acting/src/acting/potential_field_node.py:120-130
Timestamp: 2025-01-13T12:05:53.017Z
Learning: PR #602 implements a potential field method for path generation as a prototype that requires further testing in a real simulation environment.
Applied to files:
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py
📚 Learning: 2025-01-21T12:12:06.073Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/mapping/src/mapping_data_integration.py:0-0
Timestamp: 2025-01-21T12:12:06.073Z
Learning: The message types `PointcloudCluster.msg` and `PointcloudClusterArray.msg` have been removed and replaced with `ClusteredPointsArray.msg` in the mapping package.
Applied to files:
code/perception/perception/perception_utils.py
🧬 Code graph analysis (7)
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/localization/localization/gps_debug_node.py (2)
code/acting/src/acting/MainFramePublisher.py (3)
MainFramePublisher(13-83)get_current_pos(79-80)__init__(15-42)code/acting/src/acting/passthrough.py (1)
Passthrough(22-68)
code/localization/localization/ekf_state_publisher.py (2)
code/paf_common/paf_common/exceptions.py (1)
emsg_with_trace(4-7)code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)run(44-77)
code/acting/acting/passthrough.py (2)
code/acting/src/acting/passthrough.py (2)
TopicMapping(16-19)__init__(55-68)code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
doc/acting/discontinued/potential_field_node.py (1)
code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)run(44-77)
code/localization/localization/position_heading_publisher_node.py (1)
code/acting/src/acting/MainFramePublisher.py (4)
MainFramePublisher(13-83)__init__(15-42)run(44-77)get_current_heading(82-83)
doc/general/architecture_current.md (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
🪛 LanguageTool
doc/perception/lidar_distance.md
[grammar] ~22-~22: Ensure spelling is correct
Context: ...able compensation modes are:
2.1 NoCompensation (Baseline)
This strategy represents ...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[grammar] ~37-~37: Use a hyphen to join words.
Context: ...ated coordinate frame, leading to motion misaligned points if the ego vehicle mov...
(QB_NEW_EN_HYPHEN)
[uncategorized] ~56-~56: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...n components, allowing us to define the homogenous transformation matrix
(EN_WORD_COHERENCY)
[uncategorized] ~85-~85: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...trix (
Having defined the homogenous transformation matrices for both
(EN_WORD_COHERENCY)
[grammar] ~106-~106: Ensure spelling is correct
Context: ...up P'{env} \cup P{ego}
$$
2.4 LocalCompensation
This strategy uses the vehicle's local s...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
doc/general/architecture_current.md
[style] ~564-~564: For conciseness, consider replacing this expression with an adverb.
Context: ...obal_current_heading``` (no subscriber at the moment) ([std_msgs/Float32](https://docs.ros....
(AT_THE_MOMENT)
[style] ~565-~565: For conciseness, consider replacing this expression with an adverb.
Context: ...g/global_current_pos``` (no subscriber at the moment) ([geometry_msgs/PoseStamped](https://...
(AT_THE_MOMENT)
🪛 Ruff (0.14.6)
code/localization/localization/ekf_state_publisher.py
78-78: Do not catch blind exception: Exception
(BLE001)
81-81: Unused method argument: timer_event
(ARG002)
code/perception/perception/lidar_distance.py
875-875: Unused method argument: kwargs
(ARG002)
957-957: Unused method argument: kwargs
(ARG002)
⏰ Context from checks skipped due to timeout of 90000ms. You can increase the timeout in your CodeRabbit configuration to a maximum of 15 minutes (900000ms). (1)
- GitHub Check: Build and push image
🔇 Additional comments (34)
code/perception/launch/perception.xml (1)
37-37: LGTM!The addition of the
compensation_strategyparameter with valueLocalCompensationaligns with the PR objectives. According to the PR description, LocalCompensation currently shows the most promising results.doc/acting/discontinued/potential_field_node.py (1)
173-173: Verify if updates to discontinued files are necessary.This file is located in the
discontinueddirectory, suggesting it's deprecated or no longer actively used. While consistency in topic naming is valuable for documentation purposes, consider whether updates to discontinued files are necessary or if they should be removed entirely.If the file is still referenced or used for testing, the topic updates are correct.
Also applies to: 181-181
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py (1)
77-77: LGTM!The topic renaming to
global_current_posandglobal_current_headingis consistent with the broader refactoring across the codebase to standardize global topic naming.Also applies to: 82-82
doc/planning/Global_Planner.md (1)
82-82: LGTM!The documentation update correctly reflects the topic rename to
global_current_pos, maintaining consistency with the corresponding code changes.code/planning/planning/global_planner/global_planner_node.py (1)
44-44: LGTM!Both the documentation comment and the subscription topic have been consistently updated to use
global_current_pos. The changes align well with the broader refactoring effort.Also applies to: 103-103
doc/localization/position_heading_publisher_node.md (1)
69-69: LGTM!The documentation correctly reflects the updated output topic names using the
global_prefix, maintaining consistency with the broader topic naming standardization.Also applies to: 71-71
doc/planning/motion_planning.md (1)
29-30: The review comment is incorrect. The motion_planning.py code already uses the correct topic names with the "global_" prefix.Verification shows that
code/planning/planning/local_planner/motion_planning.pyat lines 91 and 97 subscribes to:
/paf/{self.role_name}/global_current_heading/paf/{self.role_name}/global_current_posThese match the documented topics at lines 29-30 of
doc/planning/motion_planning.md. The review comment conflated local variable names (self.current_heading,self.current_pos) with the actual ROS topic subscription names, which already contain the "global_" prefix as documented.code/localization/localization/gps_debug_node.py (1)
34-34: Based on my verification, I can now provide the rewritten review comment:The review comment's concern about topic mismatch is incorrect. The codebase shows consistent use of
global_current_posacross all active components:
gps_debug_node.pysubscribes tof"/paf/{self.role_name}/global_current_pos"position_heading_publisher_node.pypublishes tof"/paf/{self.role_name}/global_current_pos"Passthrough.pysubscribes tof"/paf/{role_name}/global_current_pos"MainFramePublisher.pydoes not exist in the repositoryNo topic mismatch exists between publishers and subscribers in the active codebase.
Likely an incorrect or invalid review comment.
doc/control/discontinued/stanley.md (1)
47-49: Topic renames are consistent with the PR-wide migration.The documentation correctly updates the subscription topics from
/paf/hero/current_posand/paf/hero/current_headingto their global variants. This aligns with the broader refactoring across the codebase.doc/general/architecture_current.md (8)
337-338: Documentation updates are consistent with the global topic migration.The Position Heading Publisher Node now correctly documents publishing to
/paf/hero/global_current_posand/paf/hero/global_current_heading.
361-362: Mapping Data Integration subscriptions updated correctly.
438-439: MotionPlanning subscriptions updated correctly.
467-467: PrePlanner subscription updated correctly.
484-484: GlobalPlanDistance subscription updated correctly.
510-511: Behavior Agent subscriptions updated correctly.
558-559: Passthrough subscriptions updated correctly to use global topics.
564-565: The review comment is based on outdated code references that no longer exist in the repository.The concern references
MainFramePublisherat lines 12-82 ofcode/acting/src/acting/MainFramePublisher.py, but this file does not exist in the current codebase. TheMainFramePublisherclass appears to have been removed, likely during a ROS2 migration (commit 040acaf).The documentation statement at lines 564-565 that
/paf/acting/global_current_posand/paf/acting/global_current_headinghave "no subscriber at the moment" is accurate. These topics are:
- Published by:
code/acting/acting/passthrough.py(active code)- Subscribed by: Only
doc/control/discontinued/src/stanley_controller.py(discontinued code)The actual acting package correctly bridges topics via the Passthrough node without conflicts. The documented topic subscriber status is correct for the current active codebase.
doc/perception/lidar_distance.md (1)
22-151: Well-documented compensation strategies with clear mathematical formulations.The documentation thoroughly explains each compensation strategy with appropriate mathematical notation. The separation of concerns (point cloud separation, transformation matrices, compensation procedures) is clearly articulated.
code/acting/acting/passthrough.py (1)
52-61: Topic mappings correctly updated to global variants.The
positionandheadingtopic mappings are properly updated to useglobal_current_posandglobal_current_headingfor both publishing and subscribing. This aligns with the PR-wide migration to global topic naming.code/perception/perception/perception_utils.py (4)
55-82: Clean implementation of ego motion compensation.The function correctly:
- Converts structured array to homogeneous coordinates
- Applies the 4x4 transformation matrix
- Preserves the original array structure by copying
85-99: Delta matrix calculation is mathematically correct.The formula
T_prev @ inv(T_cur)correctly computes the relative transformation to move points from the current frame's perspective back to the previous frame's coordinate system.
102-124: Transform matrix creation is well-implemented.Uses
quaternion_matrixappropriately and correctly sets the translation component.
181-194: Quaternion to heading conversion is correct.The function properly extracts the yaw (z-axis rotation) from the quaternion using scipy's Rotation class.
code/localization/localization/ekf_state_publisher.py (3)
45-61: LGTM! Clean separation of global and local publishers.The refactoring from single publishers to dual global/local publishers is well-structured and follows a consistent naming convention.
74-79: Broad exception handling is acceptable for ROS node resilience.The
except Exceptioncatch is flagged by static analysis, but in a ROS node context where the timer callback should not crash the node, this is a reasonable pattern. The fatal log with trace provides sufficient debugging information.
105-124: LGTM! Clean helper method for data preparation.The
_prepare_datamethod correctly extracts position and heading from the transform and returns a tuple for flexible use by the caller.code/localization/localization/position_heading_publisher_node.py (5)
136-149: LGTM! Correct wiring of global and local EKF position subscribers.The subscribers are correctly wired to their respective callbacks for publishing to the appropriate current position topics.
175-188: LGTM! EKF heading subscribers correctly wired to global/local callbacks.Both global and local EKF heading data are now properly routed to their respective publishers.
294-312: LGTM! Clean separation of global and local heading publication.The new callback methods correctly update the internal state and publish to the appropriate topics.
358-376: LGTM! Position publishing methods are correct.The running average now publishes to the global current position, and the filter position methods correctly route to their respective publishers.
189-196: Based on my verification, I can now provide the final rewritten review comment:Kalman filter design produces unified heading estimates only—no local variant needed.
The Kalman filter publishes a single unified heading estimate (
/paf/{role_name}/kalman_heading) consistent with its position output (/paf/{role_name}/kalman_pos). This differs from the EKF filter, which produces both global and local frame-specific estimates. The routing topublish_global_current_headingis correct and intentional. No local Kalman heading topic should be added.code/perception/perception/lidar_distance.py (3)
720-798: LGTM! Well-designed abstract base class with common buffering logic.The
CompensationStrategyABC properly encapsulates common Lidar data buffering and provides a clean interface for concrete implementations.
850-912: LGTM! EgoMotionCompensation correctly applies delta transformation.The strategy properly buffers EKF poses, validates data availability, and applies the transformation matrix to environment points.
915-1011: LGTM! LocalCompensation usesis not Nonechecks correctly (past review addressed).The implementation now correctly distinguishes between "no value passed" and "value is 0.0" using
is not Nonechecks, addressing the previous review concern about falsy values.
| self.compensation_strategy = ( | ||
| self.declare_parameter("compensation_strategy", "NoCompensation") | ||
| .get_parameter_value() | ||
| .string_value | ||
| ) | ||
|
|
||
| compensation_dict = { | ||
| "NoCompensation": NoCompensation, | ||
| "Buffer": Buffer, | ||
| "EgoMotionCompensation": EgoMotionCompensation, | ||
| "LocalCompensation": LocalCompensation, | ||
| } | ||
|
|
||
| self.Compensation: CompensationStrategy = compensation_dict[ | ||
| self.compensation_strategy | ||
| ]() |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# First, let's check the context around lines 73-88 and see actual error handling
fd lidar_distance.py --type f --exec head -100 {} \;Repository: una-auxme/paf
Length of output: 2971
🏁 Script executed:
#!/bin/bash
# Search for compensation_strategy in launch/config files as suggested in the review
rg -n "compensation_strategy" -t xml -t yaml -C 3Repository: una-auxme/paf
Length of output: 648
🏁 Script executed:
#!/bin/bash
# Check if ParameterDescriptor or parameter validation is used
rg -n "ParameterDescriptor|parameter_descriptor|allowed_values|validate" code/perception/perception/lidar_distance.py -A 3 -B 3Repository: una-auxme/paf
Length of output: 39
🏁 Script executed:
#!/bin/bash
# Read full __init__ method to see if there's try-except around line 84-87
sed -n '45,100p' code/perception/perception/lidar_distance.pyRepository: una-auxme/paf
Length of output: 1743
Add validation or fallback for invalid compensation strategy names to prevent KeyError.
The dictionary lookup on line 88 (compensation_dict[self.compensation_strategy]()) lacks error handling. If an invalid strategy name is provided—either through parameter override or misconfigured launch file—the code will crash with an unhandled KeyError. While the launch file provides a valid default ("LocalCompensation"), add a try-except block or use .get() with a fallback strategy to improve robustness.
🤖 Prompt for AI Agents
In code/perception/perception/lidar_distance.py around lines 73 to 88, the
direct dict lookup for self.compensation_strategy can raise KeyError if the
provided parameter is invalid; change it to safely resolve the class with
validation by using compensation_dict.get(self.compensation_strategy,
DefaultStrategy) or wrap the lookup in a try/except, log a clear warning
including the invalid name and chosen fallback, and then instantiate the
resolved class so the node continues running with a known default instead of
crashing.
| def apply_local_motion_compensation( | ||
| points: np.ndarray, d_x: float, d_heading: float, account_heading: bool = False | ||
| ) -> np.ndarray: | ||
| """ | ||
| Applies a simple 2D motion correction to static points in the vehicle's local frame. | ||
|
|
||
| 1. Translates points back along the X (forward) axis by d_x. | ||
| 2. Optionally rotates the points by -d_heading (rotation correction). | ||
|
|
||
| :param points: Structured NumPy array of static environment points. | ||
| :param d_x: The distance the vehicle traveled forward (translation component). | ||
| :param d_heading: The change in yaw angle (heading) of the vehicle. | ||
| :param account_heading: If True, rotation compensation is applied. | ||
| :return: Structured NumPy array of the compensated static points. | ||
| """ | ||
|
|
||
| points["x"] = points["x"] - d_x | ||
|
|
||
| if account_heading: | ||
| coords_3xN = np.stack([points["x"], points["y"], points["z"]]) | ||
| R = Rotation.from_euler("z", d_heading).as_matrix() | ||
| comp_3xN = R @ coords_3xN | ||
|
|
||
| points["x"] = comp_3xN[0, :] | ||
| points["y"] = comp_3xN[1, :] | ||
| points["z"] = comp_3xN[2, :] | ||
|
|
||
| return points |
There was a problem hiding this comment.
Input array is mutated in-place; also verify rotation direction.
Two concerns:
-
Mutation bug: Line 167 modifies
points["x"]directly before the conditional rotation block. If the caller doesn't expect mutation, this could cause subtle bugs. The function should copy the array first, similar toego_motion_compensation. -
Rotation direction: The docstring states "rotates the points by -d_heading" but the code uses
Rotation.from_euler("z", d_heading)without negation. This inconsistency could lead to incorrect compensation. Verify the intended direction matches the implementation.
def apply_local_motion_compensation(
points: np.ndarray, d_x: float, d_heading: float, account_heading: bool = False
) -> np.ndarray:
"""
Applies a simple 2D motion correction to static points in the vehicle's local frame.
1. Translates points back along the X (forward) axis by d_x.
2. Optionally rotates the points by -d_heading (rotation correction).
:param points: Structured NumPy array of static environment points.
:param d_x: The distance the vehicle traveled forward (translation component).
:param d_heading: The change in yaw angle (heading) of the vehicle.
:param account_heading: If True, rotation compensation is applied.
:return: Structured NumPy array of the compensated static points.
"""
+ points = np.copy(points)
points["x"] = points["x"] - d_x
if account_heading:
coords_3xN = np.stack([points["x"], points["y"], points["z"]])
- R = Rotation.from_euler("z", d_heading).as_matrix()
+ R = Rotation.from_euler("z", -d_heading).as_matrix()
comp_3xN = R @ coords_3xN
points["x"] = comp_3xN[0, :]
points["y"] = comp_3xN[1, :]
points["z"] = comp_3xN[2, :]
return points📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| def apply_local_motion_compensation( | |
| points: np.ndarray, d_x: float, d_heading: float, account_heading: bool = False | |
| ) -> np.ndarray: | |
| """ | |
| Applies a simple 2D motion correction to static points in the vehicle's local frame. | |
| 1. Translates points back along the X (forward) axis by d_x. | |
| 2. Optionally rotates the points by -d_heading (rotation correction). | |
| :param points: Structured NumPy array of static environment points. | |
| :param d_x: The distance the vehicle traveled forward (translation component). | |
| :param d_heading: The change in yaw angle (heading) of the vehicle. | |
| :param account_heading: If True, rotation compensation is applied. | |
| :return: Structured NumPy array of the compensated static points. | |
| """ | |
| points["x"] = points["x"] - d_x | |
| if account_heading: | |
| coords_3xN = np.stack([points["x"], points["y"], points["z"]]) | |
| R = Rotation.from_euler("z", d_heading).as_matrix() | |
| comp_3xN = R @ coords_3xN | |
| points["x"] = comp_3xN[0, :] | |
| points["y"] = comp_3xN[1, :] | |
| points["z"] = comp_3xN[2, :] | |
| return points | |
| def apply_local_motion_compensation( | |
| points: np.ndarray, d_x: float, d_heading: float, account_heading: bool = False | |
| ) -> np.ndarray: | |
| """ | |
| Applies a simple 2D motion correction to static points in the vehicle's local frame. | |
| 1. Translates points back along the X (forward) axis by d_x. | |
| 2. Optionally rotates the points by -d_heading (rotation correction). | |
| :param points: Structured NumPy array of static environment points. | |
| :param d_x: The distance the vehicle traveled forward (translation component). | |
| :param d_heading: The change in yaw angle (heading) of the vehicle. | |
| :param account_heading: If True, rotation compensation is applied. | |
| :return: Structured NumPy array of the compensated static points. | |
| """ | |
| points = np.copy(points) | |
| points["x"] = points["x"] - d_x | |
| if account_heading: | |
| coords_3xN = np.stack([points["x"], points["y"], points["z"]]) | |
| R = Rotation.from_euler("z", -d_heading).as_matrix() | |
| comp_3xN = R @ coords_3xN | |
| points["x"] = comp_3xN[0, :] | |
| points["y"] = comp_3xN[1, :] | |
| points["z"] = comp_3xN[2, :] | |
| return points |
🤖 Prompt for AI Agents
In code/perception/perception/perception_utils.py around lines 151 to 178, the
function mutates the input structured array in-place and the rotation sign
doesn't match the docstring; to fix, make a local copy of the input points at
the top (e.g., points = points.copy()) and operate on that copy so the caller's
array isn't modified, apply the translation to the copied array, and ensure the
rotation uses the intended direction by negating d_heading if the docstring's
"-d_heading" is correct (or update the docstring to match the implemented sign);
return the copied, compensated array.
3f5c644 to
549db83
Compare
549db83 to
df9b283
Compare
There was a problem hiding this comment.
Actionable comments posted: 0
🧹 Nitpick comments (7)
doc/control/discontinued/src/stanley_controller.py (1)
35-54: StanleyController subscriptions correctly updated to global_current_ topics*Using
/paf/acting/global_current_posand/paf/acting/global_current_headingkeeps this (discontinued) controller aligned with the new global-frame topic scheme; no functional issues here. As a minor future clean‑up,position_subandpath_subvariable names are inverted relative to their topics, which can be confusing when debugging.doc/localization/position_heading_publisher_node.md (1)
68-72: Outputs section matches global_current_ topics; consider clarifying earlier mentions*The outputs list now correctly documents
/paf/hero/global_current_headingand/paf/hero/global_current_pos. Earlier in the doc you still refer to the generic topicscurrent_pos/current_heading; it may be worth adding a short note that these map to the global topics (and, if applicable, to any local_* variants) to avoid confusion for readers.doc/general/architecture_current.md (1)
564-565: Minor wording nit: “at the moment”If you touch this section again, you might consider shortening “no subscriber at the moment” to something like “no current subscriber” to match style‑lint recommendations, but it’s purely cosmetic.
code/perception/tests/test_ego_motion_compensation.py (1)
1-197: Good coverage of transform and compensation math; a few optional test enhancementsThe mocks and fixtures exercise
create_transform_matrix,create_delta_matrix, andego_motion_compensationin the key pure-translation and pure-rotation cases, and the expectations (identity, 1 m X-translation, Rz(±90°), and resulting compensated point positions) are all consistent with the underlying math.If you want to harden this further, consider:
- Asserting matrix shapes (4×4) in the transform/delta tests.
- Also checking
zandintensityfields in the translation compensation test (you already cover intensity for rotation).- Adding one combined translation+rotation scenario to ensure
create_delta_matrixbehaves as expected when both components are present.These are nice-to-haves; the current tests already give solid confidence in the core logic.
doc/perception/lidar_distance.md (1)
56-56: Consider using consistent spelling throughout the document.The document uses both "homogenous" (lines 56, 85) and "homogeneous" (elsewhere) to describe transformation matrices. While both spellings are acceptable, "homogeneous" is the standard mathematical term and should be used consistently throughout.
-allowing us to define the homogenous transformation matrix $T_i$ for a frame +allowing us to define the homogeneous transformation matrix $T_i$ for a frame-Having defined the homogenous transformation matrices for both $T_i$ +Having defined the homogeneous transformation matrices for both $T_i$Also applies to: 85-85
code/perception/perception/lidar_distance.py (2)
73-88: Add validation for compensation strategy parameter.The direct dictionary lookup on line 86-88 will raise a KeyError if an invalid strategy name is provided via parameter override. While the launch file provides a valid default ("LocalCompensation"), add defensive validation to improve robustness.
+ if self.compensation_strategy not in compensation_dict: + self.get_logger().error( + f"Invalid compensation_strategy: '{self.compensation_strategy}'. " + f"Valid options: {list(compensation_dict.keys())}. " + f"Falling back to 'NoCompensation'." + ) + self.compensation_strategy = "NoCompensation" + self.Compensation: CompensationStrategy = compensation_dict[ self.compensation_strategy ]()
814-820: Consider checking valid_lidar_data return value.While the lidar_callback handles None gracefully (lines 172-173), it's better practice for
compensate()to respect the validation result. If validation fails, consider returning None explicitly.def compensate(self) -> PointCloud2: """ Returns the current Lidar data directly. """ - self.valid_lidar_data() # Ensures the current data is set + if not self.valid_lidar_data(): + return None return self._cur_lidar_data
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (31)
-
code-ros1/acting/src/Acting_Debug_Node.py(2 hunks) -
code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py(1 hunks) -
code-ros1/localization/src/evaluation/save_filter_data.py(1 hunks) -
code/acting/acting/passthrough.py(1 hunks) -
code/localization/localization/ekf_state_publisher.py(3 hunks) -
code/localization/localization/gps_debug_node.py(1 hunks) -
code/localization/localization/position_heading_publisher_node.py(7 hunks) -
code/mapping/mapping/data_integration.py(1 hunks) -
code/perception/launch/perception.xml(1 hunks) -
code/perception/perception/lidar_distance.py(5 hunks) -
code/perception/perception/perception_utils.py(2 hunks) -
code/perception/tests/test_ego_motion_compensation.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py(3 hunks) -
code/planning/planning/global_planner/global_plan_distance_publisher.py(1 hunks) -
code/planning/planning/global_planner/global_planner_node.py(2 hunks) -
code/planning/planning/local_planner/motion_planning.py(1 hunks) -
doc/acting/discontinued/potential_field_node.py(1 hunks) -
doc/acting/discontinued/teb/motion_planner.py(2 hunks) -
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py(1 hunks) -
doc/control/discontinued/src/stanley_controller.py(2 hunks) -
doc/control/discontinued/stanley.md(1 hunks) -
doc/general/architecture_current.md(7 hunks) -
doc/localization/evaluation.md(2 hunks) -
doc/localization/position_heading_publisher_node.md(1 hunks) -
doc/perception/lidar_distance.md(2 hunks) -
doc/planning/Global_Planner.md(1 hunks) -
doc/planning/motion_planning.md(1 hunks)
✅ Files skipped from review due to trivial changes (1)
- doc/acting/discontinued/potential_field_node.py
🚧 Files skipped from review as they are similar to previous changes (16)
- doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py
- code/planning/planning/global_planner/global_planner_node.py
- code/localization/localization/gps_debug_node.py
- code/planning/planning/global_planner/global_plan_distance_publisher.py
- code/planning/planning/local_planner/motion_planning.py
- doc/planning/motion_planning.md
- code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py
- doc/planning/Global_Planner.md
- code/acting/acting/passthrough.py
- code/planning/planning/behavior_agent/behaviors/unstuck_routine.py
- code/perception/launch/perception.xml
- code/planning/planning/behavior_agent/behaviors/topics2blackboard.py
- doc/acting/discontinued/teb/motion_planner.py
- code-ros1/acting/src/Acting_Debug_Node.py
- doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py
- doc/control/discontinued/stanley.md
🧰 Additional context used
🧠 Learnings (5)
📚 Learning: 2025-02-06T11:20:19.561Z
Learnt from: RoyaLxPole
Repo: una-auxme/paf PR: 687
File: doc/general/create_new_route.md:19-19
Timestamp: 2025-02-06T11:20:19.561Z
Learning: Documentation that references or quotes from the official Carla simulator should maintain exact consistency with the source, even if it contains minor grammatical or stylistic issues.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2024-11-04T11:16:31.149Z
Learnt from: Toni2go
Repo: una-auxme/paf PR: 422
File: doc/research/paf24/perception/VisionNode_CodeSummary.md:114-114
Timestamp: 2024-11-04T11:16:31.149Z
Learning: In `vision_node.md`, the `yolov8x-seg` model performs segmentation (not detection) and can also calculate distances.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-01-21T12:12:39.250Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/agent/config/dev_objects.json:182-182
Timestamp: 2025-01-21T12:12:39.250Z
Learning: In the PAF project, the vision system has been simplified to use only the Center camera (removing Back, Left, and Right cameras) as part of the VisionNode refactoring. This change is reflected in both dev_objects.json and agent.py configurations.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-01-13T12:05:53.017Z
Learnt from: ll7
Repo: una-auxme/paf PR: 602
File: code/acting/src/acting/potential_field_node.py:120-130
Timestamp: 2025-01-13T12:05:53.017Z
Learning: PR #602 implements a potential field method for path generation as a prototype that requires further testing in a real simulation environment.
Applied to files:
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py
📚 Learning: 2025-01-21T12:12:06.073Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/mapping/src/mapping_data_integration.py:0-0
Timestamp: 2025-01-21T12:12:06.073Z
Learning: The message types `PointcloudCluster.msg` and `PointcloudClusterArray.msg` have been removed and replaced with `ClusteredPointsArray.msg` in the mapping package.
Applied to files:
code/perception/perception/perception_utils.py
🧬 Code graph analysis (6)
code/perception/tests/test_ego_motion_compensation.py (1)
code/perception/perception/perception_utils.py (3)
create_transform_matrix(104-126)create_delta_matrix(87-101)ego_motion_compensation(55-84)
code/mapping/mapping/data_integration.py (1)
code/localization/localization/gps_debug_node.py (1)
current_pos_callback(96-97)
code/localization/localization/ekf_state_publisher.py (2)
code/paf_common/paf_common/exceptions.py (1)
emsg_with_trace(4-7)code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
doc/general/architecture_current.md (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/localization/localization/position_heading_publisher_node.py (1)
code/acting/src/acting/MainFramePublisher.py (2)
MainFramePublisher(13-83)__init__(15-42)
code/perception/perception/perception_utils.py (1)
code/mapping/mapping_common/transform.py (3)
translation(296-304)x(44-45)y(47-48)
🪛 LanguageTool
doc/perception/lidar_distance.md
[grammar] ~22-~22: Ensure spelling is correct
Context: ...able compensation modes are:
2.1 NoCompensation (Baseline)
This strategy represents ...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[grammar] ~37-~37: Use a hyphen to join words.
Context: ...ated coordinate frame, leading to motion misaligned points if the ego vehicle mov...
(QB_NEW_EN_HYPHEN)
[uncategorized] ~56-~56: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...n components, allowing us to define the homogenous transformation matrix
(EN_WORD_COHERENCY)
[uncategorized] ~85-~85: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...trix (
Having defined the homogenous transformation matrices for both
(EN_WORD_COHERENCY)
[grammar] ~106-~106: Ensure spelling is correct
Context: ...up P'{env} \cup P{ego}
$$
2.4 LocalCompensation
This strategy uses the vehicle's local s...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
doc/general/architecture_current.md
[style] ~564-~564: For conciseness, consider replacing this expression with an adverb.
Context: ...obal_current_heading``` (no subscriber at the moment) ([std_msgs/Float32](https://docs.ros....
(AT_THE_MOMENT)
[style] ~565-~565: For conciseness, consider replacing this expression with an adverb.
Context: ...g/global_current_pos``` (no subscriber at the moment) ([geometry_msgs/PoseStamped](https://...
(AT_THE_MOMENT)
🪛 Ruff (0.14.6)
code/localization/localization/ekf_state_publisher.py
78-78: Do not catch blind exception: Exception
(BLE001)
81-81: Unused method argument: timer_event
(ARG002)
code/perception/perception/lidar_distance.py
877-877: Unused method argument: kwargs
(ARG002)
959-959: Unused method argument: kwargs
(ARG002)
🔇 Additional comments (24)
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py (1)
37-37: Topic renames are correct, but verify if discontinued code should be maintained.The topic renames from
current_pos/current_headingtoglobal_current_pos/global_current_headingare technically correct and align with the system-wide standardization described in the PR objectives.However, since this file is located in the
doc/acting/discontinued/testbed/directory, consider whether maintaining consistency in discontinued/prototype code is necessary. If this code is no longer actively used or tested, the effort to keep it synchronized with active components may not provide value.Based on learnings, this potential field implementation was previously noted as a prototype requiring further testing in simulation environments.
Also applies to: 44-44
doc/localization/evaluation.md (1)
118-123: Evaluation docs correctly aligned to global_current_ topics*The input topic descriptions for both the “current filter” and “new filter” now point to
/paf/hero/global_current_posand/paf/hero/global_current_heading, which is consistent with the new global-frame naming and the publisher node docs. No further issues from this change.Also applies to: 237-239
code/mapping/mapping/data_integration.py (1)
326-337: MappingDataIntegration now correctly consumes global_current_ pose/heading*Subscribing to
/paf/hero/global_current_posand/paf/hero/global_current_headingmatches the documented assumption thatcurrent_pos/current_headingare in global coordinates (used for stop-mark transforms and hero transform). The wiring looks consistent; no further changes needed here.code-ros1/localization/src/evaluation/save_filter_data.py (1)
95-108: New‑filter subscribers correctly switched to global_current_ topics*Routing the “new filter” position and heading through
/paf/{role_name}/global_current_posand/paf/{role_name}/global_current_headingkeeps this evaluation node aligned with the global-frame outputs of the position-heading publisher, while still comparing against the Kalman topics for the old filter. The change is consistent and low‑risk.code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py (1)
75-105: Debug node topic wiring updated consistently to global_ streams*The subscribers for current and test-filter state now track
/global_current_*and/global_ekf_*topics, which is in line with the new global/local split and the rest of the localization stack. Given this node is for offline evaluation and noted as unused, the change is appropriate and non‑disruptive.doc/general/architecture_current.md (1)
337-338: Architecture doc now consistently documents global_current_ pose/heading topics*The updated topic lists for Localization, Mapping, Planning, BehaviorAgent, and Passthrough all reference
/paf/hero/global_current_posand/paf/hero/global_current_heading, which matches the new global-frame design and the concrete node implementations. This should make it much easier to reason about where global pose/heading come from throughout the stack.Also applies to: 361-362, 436-439, 467-468, 484-485, 510-511, 558-565
doc/perception/lidar_distance.md (1)
227-228: Verify camera direction naming consistency.The documentation mentions "the current vision node only subscribes to and utilizes the _Center_S image" but the topic names throughout use "Center" (not "Center_S"). Confirm whether the reference to "Center_S" is correct or should be "Center" to match the topic naming convention.
code/perception/perception/perception_utils.py (6)
55-84: LGTM!The function correctly implements ego-motion compensation by:
- Creating a copy to avoid input mutation
- Converting to homogeneous coordinates
- Applying the transformation matrix
- Returning the compensated structured array
87-101: LGTM!The delta transformation correctly computes T_prev @ inv(T_cur) to transform points from the current frame back to the previous frame's coordinate system, matching the mathematical formulation in the documentation.
104-126: LGTM!The function correctly constructs a homogeneous transformation matrix from a PoseStamped message using the tf_transformations library and properly sets the translation component.
129-150: LGTM!The function correctly creates a boolean mask for identifying ego vehicle points within a rectangular region. The bounds appear reasonable for a typical vehicle footprint.
184-197: LGTM!The function correctly converts a quaternion to a heading angle (yaw) using scipy's Rotation library and returns the result in degrees.
153-181: The rotation direction is implemented correctly. No issues found.The calculation
d_heading = prev_heading - cur_heading(line 1000 of lidar_distance.py) pre-negates the heading difference. When applied viaRotation.from_euler("z", d_heading), this correctly undoes the vehicle's rotation. For example, if the vehicle rotates clockwise by +0.1 rad,d_heading = -0.1, which applies a counter-clockwise compensation—exactly what's needed. The implementation is mathematically sound.The input mutation issue was previously fixed (line 169 uses
np.copy), and the docstring correctly reflects the current behavior. No changes are needed.code/localization/localization/ekf_state_publisher.py (2)
45-61: LGTM!The refactoring to separate global and local EKF publishers is well-structured. The frame-aware publishing logic correctly routes data to the appropriate publishers based on whether the frame is "global" or "odom", and the dual-frame publishing approach aligns with the multi-frame localization architecture.
Also applies to: 74-104
105-124: LGTM!The
_prepare_datamethod cleanly separates data extraction and transformation logic from publishing, improving code organization and reusability. The heading calculation from quaternion is correct.code/perception/perception/lidar_distance.py (6)
160-176: LGTM!The lidar_callback correctly handles the None case from compensate() (line 172-173), preventing downstream AttributeErrors. This addresses the critical issue from previous reviews.
178-233: LGTM!The motion data callbacks (ekf, speed, imu) include appropriate strategy validation checks and cleanly forward data to the compensation strategy. The heading extraction from IMU quaternion is handled correctly.
722-800: LGTM!The abstract base class provides clean interfaces and shared buffering logic for all compensation strategies. The separation of ego vehicle and environment points in
prepare_datais well-implemented.
823-849: LGTM!The Buffer strategy correctly implements simple frame concatenation with proper validation, providing spatial coverage without motion correction as documented.
852-914: LGTM!The EgoMotionCompensation strategy correctly implements the transformation-based compensation approach documented in the markdown. The validation logic ensures both LiDAR and EKF data are available, and the matrix-based transformation properly aligns environment points between frames.
917-1013: LGTM!The LocalCompensation strategy correctly addresses the previous review concerns about zero-value handling by using
is not Nonechecks (lines 965, 971). The implementation calculates translation-based compensation from velocity and time delta, and intentionally disables rotational compensation (by not passingaccount_heading=Trueon line 1002-1004), which aligns with the PR objectives stating "currently ignores heading for stability."code/localization/localization/position_heading_publisher_node.py (3)
137-195: LGTM!The subscriber setup correctly routes frame-specific data to appropriate callbacks. The EKF filter provides both global and local frame outputs, while the Kalman filter only provides global output. This resolves the previous review concern about local Kalman being wired to the wrong callback.
219-238: LGTM!The publisher setup cleanly separates global and local position/heading publishers with appropriate internal state management. The use of private variables for heading state is good practice.
266-310: LGTM!The refactored callback methods correctly separate global and local frame handling. The routing logic appropriately publishes unfiltered data to global publishers when filter is set to "None", which is a sensible default behavior.
Also applies to: 358-374
There was a problem hiding this comment.
Actionable comments posted: 4
♻️ Duplicate comments (3)
code/perception/perception/lidar_distance.py (2)
73-88: Add fallback for invalid compensation strategy names.The dictionary lookup on line 86-88 will raise an unhandled
KeyErrorif an invalid strategy name is provided via parameter override. Use.get()with a fallback or add a try-except block.Apply this diff to add robustness:
- self.Compensation: CompensationStrategy = compensation_dict[ - self.compensation_strategy - ]() + strategy_class = compensation_dict.get(self.compensation_strategy) + if strategy_class is None: + self.get_logger().warn( + f"Unknown compensation strategy '{self.compensation_strategy}', " + "falling back to NoCompensation." + ) + strategy_class = NoCompensation + self.Compensation: CompensationStrategy = strategy_class()
814-820: Return value ofvalid_lidar_data()is ignored.
valid_lidar_data()is called but its return value is not checked. If_cur_lidar_dataisNone, the method returnsNone, which could cause downstream errors.def compensate(self) -> PointCloud2: """ Returns the current Lidar data directly. """ - self.valid_lidar_data() # Ensures the current data is set + if not self.valid_lidar_data(): + return None return self._cur_lidar_dataNote: The caller (
lidar_callback) already handlesNonereturns at line 172-173, so this change maintains consistency with other strategies.code/localization/localization/position_heading_publisher_node.py (1)
176-195: Kalman filter lacks local heading support.The EKF heading subscribers (lines 176-188) correctly publish to both global and local current heading topics. However, the Kalman filter (lines 190-195) only creates a global subscriber with no local equivalent, meaning
local_current_headingwill never be updated when using the Kalman filter.If local topics are expected by downstream consumers regardless of the filter choice, consider adding a local Kalman heading subscriber:
elif self.heading_filter == "Kalman": self.global_kalman_heading_subscriber = self.create_subscription( Float32, "/paf/" + self.role_name + "/kalman_heading", self.publish_global_current_heading, qos_profile=1, ) + + self.local_kalman_heading_subscriber = self.create_subscription( + Float32, + "/paf/" + self.role_name + "/kalman_heading", + self.publish_local_current_heading, + qos_profile=1, + )The same issue applies to Kalman position subscribers at lines 152-157.
🧹 Nitpick comments (2)
code/perception/tests/test_ego_motion_compensation.py (1)
166-197: Comprehensive compensation tests with clear expected outcomes.The test correctly verifies both translation and rotation compensation:
- Translation: A point at X=10 becomes X=9 after 1m forward motion compensation
- Rotation: A point at (0,10,0) rotates to (10,0,0) after 90° CW compensation
- Intensity field preservation is verified
Consider adding edge-case tests for empty point clouds or near-identity transformations in a follow-up to improve robustness.
code/perception/perception/lidar_distance.py (1)
877-888: Unused**kwargsin concrete strategy implementations.The
**kwargsparameter inset_motion_datais defined for interface flexibility but is unused in the concrete implementations. This is flagged by static analysis (Ruff ARG002).If interface flexibility is not needed, consider removing
**kwargsfrom concrete implementations to silence the warning, or add a# noqa: ARG002comment if the flexibility is intentional for future extensibility.Also applies to: 955-972
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (31)
-
code-ros1/acting/src/Acting_Debug_Node.py(2 hunks) -
code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py(1 hunks) -
code-ros1/localization/src/evaluation/save_filter_data.py(1 hunks) -
code/acting/acting/passthrough.py(1 hunks) -
code/localization/localization/ekf_state_publisher.py(3 hunks) -
code/localization/localization/gps_debug_node.py(1 hunks) -
code/localization/localization/position_heading_publisher_node.py(7 hunks) -
code/mapping/mapping/data_integration.py(1 hunks) -
code/perception/launch/perception.xml(1 hunks) -
code/perception/perception/lidar_distance.py(5 hunks) -
code/perception/perception/perception_utils.py(2 hunks) -
code/perception/tests/test_ego_motion_compensation.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py(1 hunks) -
code/planning/planning/behavior_agent/behaviors/unstuck_routine.py(3 hunks) -
code/planning/planning/global_planner/global_plan_distance_publisher.py(1 hunks) -
code/planning/planning/global_planner/global_planner_node.py(2 hunks) -
code/planning/planning/local_planner/motion_planning.py(1 hunks) -
doc/acting/discontinued/potential_field_node.py(1 hunks) -
doc/acting/discontinued/teb/motion_planner.py(2 hunks) -
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py(1 hunks) -
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py(1 hunks) -
doc/control/discontinued/src/stanley_controller.py(2 hunks) -
doc/control/discontinued/stanley.md(1 hunks) -
doc/general/architecture_current.md(7 hunks) -
doc/localization/evaluation.md(2 hunks) -
doc/localization/position_heading_publisher_node.md(1 hunks) -
doc/perception/lidar_distance.md(2 hunks) -
doc/planning/Global_Planner.md(1 hunks) -
doc/planning/motion_planning.md(1 hunks)
✅ Files skipped from review due to trivial changes (1)
- doc/planning/motion_planning.md
🚧 Files skipped from review as they are similar to previous changes (16)
- code/planning/planning/global_planner/global_planner_node.py
- code/planning/planning/behavior_agent/behaviors/unstuck_routine.py
- code/planning/planning/local_planner/motion_planning.py
- code-ros1/acting/src/Acting_Debug_Node.py
- doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py
- doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py
- doc/localization/position_heading_publisher_node.md
- doc/control/discontinued/stanley.md
- code/localization/localization/gps_debug_node.py
- doc/localization/evaluation.md
- doc/control/discontinued/src/stanley_controller.py
- code/mapping/mapping/data_integration.py
- code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py
- code/planning/planning/global_planner/global_plan_distance_publisher.py
- code/perception/perception/perception_utils.py
- doc/planning/Global_Planner.md
🧰 Additional context used
🧠 Learnings (3)
📚 Learning: 2025-02-06T11:20:19.561Z
Learnt from: RoyaLxPole
Repo: una-auxme/paf PR: 687
File: doc/general/create_new_route.md:19-19
Timestamp: 2025-02-06T11:20:19.561Z
Learning: Documentation that references or quotes from the official Carla simulator should maintain exact consistency with the source, even if it contains minor grammatical or stylistic issues.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2024-11-04T11:16:31.149Z
Learnt from: Toni2go
Repo: una-auxme/paf PR: 422
File: doc/research/paf24/perception/VisionNode_CodeSummary.md:114-114
Timestamp: 2024-11-04T11:16:31.149Z
Learning: In `vision_node.md`, the `yolov8x-seg` model performs segmentation (not detection) and can also calculate distances.
Applied to files:
doc/perception/lidar_distance.md
📚 Learning: 2025-01-21T12:12:39.250Z
Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/agent/config/dev_objects.json:182-182
Timestamp: 2025-01-21T12:12:39.250Z
Learning: In the PAF project, the vision system has been simplified to use only the Center camera (removing Back, Left, and Right cameras) as part of the VisionNode refactoring. This change is reflected in both dev_objects.json and agent.py configurations.
Applied to files:
doc/perception/lidar_distance.md
🧬 Code graph analysis (6)
doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py (2)
doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py (1)
_position_callback(79-81)code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py (1)
code/planning/planning/behavior_agent/blackboard_utils.py (1)
try_get(13-30)
code/localization/localization/ekf_state_publisher.py (1)
code/paf_common/paf_common/exceptions.py (1)
emsg_with_trace(4-7)
code/perception/perception/lidar_distance.py (8)
code/paf_common/paf_common/parameters.py (1)
update_attributes(10-48)code/perception/perception/perception_utils.py (6)
array_to_clustered_points(11-52)ego_motion_compensation(55-84)create_delta_matrix(87-101)create_ego_vehicle_mask(129-150)apply_local_motion_compensation(153-181)quaternion_to_heading(184-197)code/perception/perception/radar_node.py (4)
callback(377-410)imu_callback(236-272)_set_parameters_callback(185-187)pointcloud2_to_array(600-621)code/localization/localization/ekf_state_publisher.py (1)
_set_parameters_callback(70-72)code/perception/perception/vision_node.py (1)
_set_parameters_callback(129-131)code/perception/perception/lane_position.py (1)
_set_parameters_callback(185-187)code/perception/perception/traffic_light_node.py (1)
_set_parameters_callback(69-71)code/mapping/mapping_common/transform.py (2)
x(44-45)y(47-48)
code/perception/tests/test_ego_motion_compensation.py (1)
code/perception/perception/perception_utils.py (3)
create_transform_matrix(104-126)create_delta_matrix(87-101)ego_motion_compensation(55-84)
doc/general/architecture_current.md (1)
code/acting/src/acting/MainFramePublisher.py (1)
MainFramePublisher(13-83)
🪛 LanguageTool
doc/perception/lidar_distance.md
[grammar] ~22-~22: Ensure spelling is correct
Context: ...able compensation modes are:
2.1 NoCompensation (Baseline)
This strategy represents ...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
[uncategorized] ~56-~56: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...n components, allowing us to define the homogenous transformation matrix
(EN_WORD_COHERENCY)
[uncategorized] ~85-~85: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...trix (
Having defined the homogenous transformation matrices for both
(EN_WORD_COHERENCY)
[grammar] ~106-~106: Ensure spelling is correct
Context: ...up P'{env} \cup P{ego}
$$
2.4 LocalCompensation
This strategy uses the vehicle's local s...
(QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1)
doc/general/architecture_current.md
[style] ~564-~564: For conciseness, consider replacing this expression with an adverb.
Context: ...obal_current_heading``` (no subscriber at the moment) ([std_msgs/Float32](https://docs.ros....
(AT_THE_MOMENT)
[style] ~565-~565: For conciseness, consider replacing this expression with an adverb.
Context: ...g/global_current_pos``` (no subscriber at the moment) ([geometry_msgs/PoseStamped](https://...
(AT_THE_MOMENT)
🪛 Ruff (0.14.6)
code/localization/localization/ekf_state_publisher.py
78-78: Do not catch blind exception: Exception
(BLE001)
81-81: Unused method argument: timer_event
(ARG002)
code/perception/perception/lidar_distance.py
877-877: Unused method argument: kwargs
(ARG002)
959-959: Unused method argument: kwargs
(ARG002)
🔇 Additional comments (22)
code/perception/launch/perception.xml (1)
32-38: Parametercompensation_strategyis correctly integrated in the implementation.Verification confirms:
- Parameter name matches exactly: declared at
lidar_distance.py:73-74as"compensation_strategy"- Parameter value
"LocalCompensation"matches implementation- Strategy classes defined:
NoCompensation,Buffer,EgoMotionCompensation, andLocalCompensationall exist (lines 803, 823, 852, 917)- Parameter is properly parsed and used to initialize the strategy via dictionary lookup (lines 79-88)
- Strategy-specific subscriptions are correctly conditional (LocalCompensation subscribes to speed and IMU at lines 138-143)
- Parameter string comparisons are consistent throughout the code
No integration issues found.
doc/acting/discontinued/potential_field_node.py (2)
44-44: Inconsistency: Constructor signature unchanged.The AI summary states "Updated constructor to accept topic arguments," but the constructor signature at line 44 remains
def __init__(self):with no parameters. The topics at lines 173 and 181 are still hardcoded within the constructor body.If this file is still active and topic flexibility is needed, consider parameterizing these topics. Otherwise, the AI summary should be corrected.
Also applies to: 173-173, 181-181
173-173: Perfect. The verification confirms the original review comment's concern. Let me now create the rewritten review comment based on the verification results:
File is in discontinued directory and has zero external impact—updating it adds unnecessary maintenance overhead.
This file has no imports, no references in configs, and its output topic (
/paf/hero/potential_field_trajectory) is not subscribed to anywhere in the active codebase. If the file is truly discontinued, it should not be updated. If it's still in use, it should be moved out of thediscontinued/directory. Clarify its status before making changes.code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py (1)
26-31: Perfect. I now have complete verification. Let me generate the final rewritten review comment:
Global blackboard keys are properly aligned via topics2blackboard; optional refactor to centralize topic names remains valid
The switch to
/paf/hero/global_current_posand/paf/hero/global_current_headingis safe. Verification confirms:
topics2blackboard.create_node(role_name)creates these exact blackboard keys (parameterized by role_name).BehaviorTreeinitializes role_name with default"hero"(configurable parameter).- For the default hero agent, topics2blackboard produces the keys that
get_global_hero_transform()now reads.The existing None-handling behavior is preserved and the keys are consistent across Planning, Localization, and Perception.
Optional follow-ups:
- Centralize topic name constants: No shared module for these keys currently exists. A constants module would prevent future drift, though the current design (parameterized in topics2blackboard, default "hero" in BehaviorTree) effectively prevents misalignment.
doc/acting/discontinued/teb/motion_planner.py (1)
106-111: Topic renames are consistent with the PR-wide changes.The subscription topic updates from
current_headingtoglobal_current_headingandcurrent_postoglobal_current_posalign with the broader topic rename pattern in this PR.Also applies to: 127-132
code/perception/tests/test_ego_motion_compensation.py (2)
1-9: Good test structure with appropriate mock objects.The test file correctly imports the functions under test and defines minimal mock ROS data structures. This approach isolates tests from ROS dependencies while maintaining type compatibility.
85-113: Point cloud fixtures use correct structured array format.The dtype definition with
('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.uint8)matches the expected PointCloud2 array format used in the compensation utilities.code/perception/perception/lidar_distance.py (3)
722-800: Well-designed Strategy pattern implementation.The
CompensationStrategyABC correctly encapsulates common buffering and data preparation logic, while deferring compensation-specific behavior to concrete implementations.
955-972: Correctly handles zero values for heading and velocity.The
is not Nonechecks properly distinguish between "no value provided" and "value is 0.0", addressing the previously flagged falsy-value issue.
998-1004: Heading compensation is disabled by default.The call to
apply_local_motion_compensationuses the defaultaccount_heading=False, meaning rotation correction is not applied. This aligns with the PR description noting that rotational components are currently unstable.Consider adding a configurable parameter to enable/disable heading compensation for future testing:
+ # In __init__ or as class attribute: + self._account_heading: bool = False # Disabled due to instability + comp_env_points = apply_local_motion_compensation( - self.prev_env_points, d_x, d_heading + self.prev_env_points, d_x, d_heading, account_heading=self._account_heading )doc/perception/lidar_distance.md (1)
7-151: Comprehensive and well-structured compensation documentation.The new Point Cloud Compensation section provides clear explanations of each strategy with appropriate mathematical notation. The documentation accurately reflects the implementation in
lidar_distance.py.doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py (1)
41-53: Topic renames are consistent with global frame convention.The subscription topics correctly updated to use
global_current_posandglobal_current_heading, aligning with the PR's topic naming convention for global coordinate frame data.code-ros1/localization/src/evaluation/save_filter_data.py (1)
97-97: LGTM! Topic renaming aligns with the global naming convention.The subscriber topic updates correctly reflect the new global_current_pos and global_current_heading naming scheme.
Also applies to: 105-105
code/planning/planning/behavior_agent/behaviors/topics2blackboard.py (1)
77-77: LGTM! Blackboard topic updates are consistent.The topic name changes align with the global_current_pos/global_current_heading convention used throughout the PR.
Also applies to: 82-82
code/acting/acting/passthrough.py (1)
52-61: LGTM! Topic mappings updated correctly.The position and heading topic mappings now use the global_current_* naming convention, consistent with the broader PR changes.
code/localization/localization/ekf_state_publisher.py (2)
45-61: LGTM! Dual-frame publisher setup is well-structured.The introduction of separate global and local publishers for EKF position and heading enables proper frame-specific data dissemination.
74-124: LGTM! Frame-aware publishing logic is correct.The refactored
publish_headingmethod correctly:
- Accepts a
frame_idparameter to support both "global" and "odom" frames- Looks up frame-specific transforms
- Routes data to the appropriate publishers via the
_prepare_datahelperNote: The static analysis hints (catching
Exception, unusedtimer_event) are false positives. The broad exception catch is appropriate for logging, andtimer_eventis part of the ROS timer callback signature.code/localization/localization/position_heading_publisher_node.py (4)
137-157: LGTM! EKF position subscribers correctly route to global and local publishers.The dual subscriber setup for EKF position data properly separates global and local frames. However, note that the Kalman filter branch (lines 152-157) only publishes to the global current position, with no local equivalent.
219-238: LGTM! Global and local publishers correctly established.The separate publishers for global and local current position and heading enable proper frame-specific data distribution.
285-310: LGTM! Heading callbacks correctly route to global and local publishers.The separate
publish_global_current_headingandpublish_local_current_headingmethods properly update their respective internal state and publishers.
358-374: LGTM! Position callbacks correctly route to global and local publishers.The new methods
publish_filter_pos_as_global_current_posandpublish_filter_pos_as_local_current_posappropriately forward filtered position data to the respective publishers.doc/general/architecture_current.md (1)
337-338: I'll verify the claims in this review comment about MainFramePublisher.py and the topic names. Let me start by examining the current code.
<function_calls>
Check if MainFramePublisher.py uses the legacy or new topic names
#!/bin/bashSearch for topic subscription patterns in MainFramePublisher.py
rg -n "current_pos|current_heading|global_current" code/acting/src/acting/MainFramePublisher.py -C 3
</function_calls>
|
|
||
| 2.3.2 Homogeneous Transformation Matrix ($T_i$) | ||
|
|
||
| The EKF provides both the translation components and rotation components, allowing us to define the homogenous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as following: |
There was a problem hiding this comment.
Inconsistent spelling: "homogenous" vs "homogeneous".
Lines 56 and 85 use "homogenous" while line 54 uses "homogeneous". Use "homogeneous" consistently throughout.
-The EKF provides both the translation components and rotation components, allowing us to define the homogenous transformation matrix $T_i$ for a frame $f_i$ relative to the local position.
+The EKF provides both the translation components and rotation components, allowing us to define the homogeneous transformation matrix $T_i$ for a frame $f_i$ relative to the local position.-Having defined the homogenous transformation matrices for both $T_i$ (current pose) and $T_{i-1}$ (previous pose), we calculate the positional delta between the two frames.
+Having defined the homogeneous transformation matrices for both $T_i$ (current pose) and $T_{i-1}$ (previous pose), we calculate the positional delta between the two frames.Also applies to: 85-85
🧰 Tools
🪛 LanguageTool
[uncategorized] ~56-~56: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text.
Context: ...n components, allowing us to define the homogenous transformation matrix
(EN_WORD_COHERENCY)
🤖 Prompt for AI Agents
In doc/perception/lidar_distance.md around lines 56 and 85, the term
"homogenous" is misspelled and inconsistent with "homogeneous" used on line 54;
replace "homogenous" with "homogeneous" at those occurrences (and scan nearby
text for any other instances) so the spelling is consistent throughout the file.
df9b283 to
ff5eceb
Compare
There was a problem hiding this comment.
Actionable comments posted: 0
♻️ Duplicate comments (2)
code/perception/perception/lidar_distance.py (2)
73-88: Handle invalidcompensation_strategyvalues more defensivelyDirectly indexing
compensation_dict[self.compensation_strategy]will raiseKeyErrorif the parameter is misspelled or misconfigured (e.g., via launch override), crashing the node. Consider resolving with.get()and a default, plus a clear warning, or catchingKeyErrorand falling back to a safe strategy.Example:
- self.Compensation: CompensationStrategy = compensation_dict[ - self.compensation_strategy - ]() + strategy_cls = compensation_dict.get(self.compensation_strategy) + if strategy_cls is None: + self.get_logger().warn( + f"Unknown compensation_strategy '{self.compensation_strategy}', " + "falling back to LocalCompensation." + ) + strategy_cls = LocalCompensation + + self.Compensation: CompensationStrategy = strategy_cls()
802-820:NoCompensation.compensatedoesn’t need fullvalid_lidar_data; simplify to check only current frameFor a pure pass-through strategy you only need
_cur_lidar_datato be set. Callingvalid_lidar_data():
- Enforces the “needs previous frame” invariant unnecessarily.
- Returns
Falsewhen_prev_lidar_datais stillNone(first frame), but its value is ignored.- Can return
False(and log) yet you still return_cur_lidar_data, which is slightly confusing.You can simplify and better match the contract:
class NoCompensation(CompensationStrategy): ... def compensate(self) -> PointCloud2: """ Returns the current Lidar data directly. """ - self.valid_lidar_data() # Ensures the current data is set - return self._cur_lidar_data + if self._cur_lidar_data is None: + rclpy.logging.get_logger("lidar_distance").error( + "Current Lidar data must be set before compensating." + ) + return None + + return self._cur_lidar_data(You already guard against
Noneinlidar_callback, so this stays safe.)
🧹 Nitpick comments (8)
doc/perception/lidar_distance.md (1)
7-42: LGTM! Clear explanation of compensation strategies.The documentation provides excellent coverage of the compensation problem and the modular Strategy pattern solution. The mathematical notation for NoCompensation and Buffer modes is clear and concise.
Minor suggestion: Line 37 could use a hyphen ("motion-misaligned" instead of "motion misaligned") for better readability, but this is a minor style point.
code/perception/perception/lidar_distance.py (7)
126-151: Dynamic updates ofcompensation_strategydon’t reconfigure strategy instance or subscriptions
compensation_strategyis registered as a parameter and fed through_set_parameters_callback, but:
self.Compensationis instantiated only once at startup.- Topic subscriptions for EKF/speed/IMU are created based on the initial value only.
- The callbacks guard on
self.compensation_strategyand warn/return when it no longer matches.If
compensation_strategyis ever changed at runtime, you’ll end up with:
- A stale strategy instance of the original type.
- No subscriptions for the new strategy’s motion topics.
- Repeated "callback is only active for ..." warnings.
Either document that
compensation_strategyis static (not expected to change after startup) or update_set_parameters_callbackto recreateself.Compensationand adjust subscriptions accordingly.Also, the warning strings are concatenated without a space:
f"{type(self).__name__}" "EKF callback is only active for EgoMotionCompensation."which logs e.g.
LidarDistanceEKF callback.... Consider adding a space or simplifying:- self.get_logger().warn( - f"{type(self).__name__}" - "EKF callback is only active for EgoMotionCompensation." - ) + self.get_logger().warn( + "EKF callback is only active for EgoMotionCompensation." + )(similarly for speed/IMU callbacks).
Also applies to: 186-191, 203-207, 219-223
160-177:lidar_callbackcompensation flow and None-guard look good; optional logging on dropThe callback cleanly:
- Buffers LIDAR via
set_lidar_data.- Delegates to
self.Compensation.compensate().- Skips downstream processing when
point_cloudisNone.This matches the strategy contract and prevents
Nonefrom reachingstart_clustering/start_image_calculation. Optionally, you might log atDEBUG/WARNonce whencompensate()returnsNoneto help diagnose validation failures:point_cloud = self.Compensation.compensate() if point_cloud is None: self.get_logger().debug("Compensation returned None; skipping frame.") return
178-193: EKF callback wiring is correct; consider simplifying log and argument surfaceThe EKF callback is correctly:
- Guarded to only be active when the strategy is
EgoMotionCompensation.- Forwarding
PoseStampedtoself.Compensation.set_motion_data(data=data).Given
set_motion_dataalready has a dedicateddataparameter here, the**kwargsin the strategy method signature are unused; see later comments on silencing Ruff by renaming to_kwargsor dropping them if you don’t plan to extend the API.
721-800: BaseCompensationStrategyabstraction is solid; minor API polish possibleThe base class cleanly centralizes:
- LIDAR buffering (
_cur_lidar_data,_prev_lidar_data).- Validation (
valid_lidar_data) and conversion to NumPy arrays.- Ego/environment separation via
create_ego_vehicle_mask.A few minor polish points:
valid_lidar_datais used for strategies that genuinely need both frames; forNoCompensationyou only require_cur_lidar_data. Either:
- Override
valid_lidar_datainNoCompensation, or- Stop calling it there and instead directly check
_cur_lidar_data is not None.valid_lidar_datacurrently doesn’t annotate a return type; consider-> boolfor clarity.prepare_datasetsself.cur_points/self.prev_pointsand ego/env splits; for strategies likeBufferthat don’t use the ego split, a lighter helper that just converts the clouds could shave a bit of work in hot paths.These are all non-blocking and mostly about clarity/perf.
822-849:Bufferstrategy is correct;prepare_datadoes more work than needed
Buffer.compensatecorrectly:
- Falls back to the current cloud when only one frame is available.
- Concatenates previous and current frames into a single
PointCloud2with the current header.The only nit is that
prepare_data()also computesprev_ego_points/prev_env_pointsviacreate_ego_vehicle_mask, which aren’t used in this strategy. To avoid unnecessary masking on every frame, you could inline a lighter conversion:- self.prepare_data() - - lidar_data = np.concatenate([self.cur_points, self.prev_points]) + self.cur_points = ros2_numpy.point_cloud2.pointcloud2_to_array( + self._cur_lidar_data + ) + self.prev_points = ros2_numpy.point_cloud2.pointcloud2_to_array( + self._prev_lidar_data + ) + lidar_data = np.concatenate([self.cur_points, self.prev_points])Not critical, but might help if this path is heavily used.
851-914: Ego-motion strategy structure is good; tune logging and silence unusedkwargsPositives:
- EKF poses are buffered symmetrically (
_prev_ekf_pose,_cur_ekf_pose).valid_ekf_datamirrorsvalid_lidar_data, so you only compensate when both poses are available.compensatecleanly composes:
create_delta_matrix(cur, prev),ego_motion_compensationon previous environment points, and- concatenation of current frame + compensated env + previous ego.
A couple of refinements:
Logging level / spam
valid_ekf_datalogs an error every time_cur_ekf_poseisNone. Early in node lifetime or if EKF lags LIDAR, this can spam the console, which matches your “EgoMotionCompensation emits console warnings” note. Consider:
- Downgrading to
debug()orwarn()once, or- Logging only after some grace period or after EKF has been seen at least once.
Unused
kwargs(Ruff ARG002)
set_motion_data(self, data: Optional[PoseStamped] = None, **kwargs)doesn’t usekwargs. If you only expect PoseStamped here, you can drop**kwargs, or rename to**_kwargsto appease Ruff while keeping a future extension hook:
- def set_motion_data(self, data: Optional[PoseStamped] = None, **kwargs):
- def set_motion_data(self, data: Optional[PoseStamped] = None, **_kwargs):
--- `916-1013`: **LocalCompensation math is reasonable; verify assumptions and quiet unused `kwargs`** This strategy nicely mirrors the ego-motion structure: - Requires valid LIDAR, heading (prev+cur), and velocity before compensating. - Uses LIDAR timestamps to derive `d_t`. - Computes: ```python d_t = t_cur - t_prev d_x = self._velocity * d_t d_heading = self._prev_heading - self._cur_heading
- Delegates to
apply_local_motion_compensationand recombines current frame + compensated env + previous ego.Comments/suggestions:
Assumptions behind
d_xandd_heading
The sign conventiond_heading = prev - curis consistent with transforming from the previous vehicle frame to the current one, but correctness depends on howapply_local_motion_compensationinterprets these values. Given your known rotational inaccuracy, it’s worth explicitly validating:
- Units of
_velocity(m/s vs. km/h) so thatd_xis in meters.- That timestamps are monotonically increasing; if
t_cur < t_prev, you might want to early-return instead of applying a negatived_t.- A few synthetic scenarios (pure translation, pure rotation, combined turn) to ensure compensated clouds align as expected.
Logging level for missing heading/velocity
Like the EKF case,valid_heading_dataandvalid_velocity_datalog errors whenever current values areNone. This will be common during startup or sensor delays. Consider downgrading towarn/debugor logging only once to avoid noisy logs.Unused
kwargs(Ruff ARG002)
set_motion_data(..., **kwargs)doesn’t usekwargs. As with EgoMotionCompensation, either drop it or rename to_kwargs:
- def set_motion_data(
self,heading: Optional[float] = None,velocity: Optional[float] = None,**kwargs,- ):
- def set_motion_data(
self,heading: Optional[float] = None,velocity: Optional[float] = None,**_kwargs,- ):
</blockquote></details> </blockquote></details> <details> <summary>📜 Review details</summary> **Configuration used**: CodeRabbit UI **Review profile**: CHILL **Plan**: Pro <details> <summary>📥 Commits</summary> Reviewing files that changed from the base of the PR and between df9b283a9991cc0dfb7fa1b3396efad7a8a15c2c and ff5eceb4eeb40bf65a28370fa85bf2915ea4cc25. </details> <details> <summary>📒 Files selected for processing (31)</summary> * `code-ros1/acting/src/Acting_Debug_Node.py` (2 hunks) * `code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py` (1 hunks) * `code-ros1/localization/src/evaluation/save_filter_data.py` (1 hunks) * `code/acting/acting/passthrough.py` (1 hunks) * `code/localization/localization/ekf_state_publisher.py` (3 hunks) * `code/localization/localization/gps_debug_node.py` (1 hunks) * `code/localization/localization/position_heading_publisher_node.py` (7 hunks) * `code/mapping/mapping/data_integration.py` (1 hunks) * `code/perception/launch/perception.xml` (1 hunks) * `code/perception/perception/lidar_distance.py` (5 hunks) * `code/perception/perception/perception_utils.py` (2 hunks) * `code/perception/tests/test_ego_motion_compensation.py` (1 hunks) * `code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py` (1 hunks) * `code/planning/planning/behavior_agent/behaviors/topics2blackboard.py` (1 hunks) * `code/planning/planning/behavior_agent/behaviors/unstuck_routine.py` (3 hunks) * `code/planning/planning/global_planner/global_plan_distance_publisher.py` (1 hunks) * `code/planning/planning/global_planner/global_planner_node.py` (2 hunks) * `code/planning/planning/local_planner/motion_planning.py` (1 hunks) * `doc/acting/discontinued/potential_field_node.py` (1 hunks) * `doc/acting/discontinued/teb/motion_planner.py` (2 hunks) * `doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py` (1 hunks) * `doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py` (1 hunks) * `doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py` (1 hunks) * `doc/control/discontinued/src/stanley_controller.py` (2 hunks) * `doc/control/discontinued/stanley.md` (1 hunks) * `doc/general/architecture_current.md` (7 hunks) * `doc/localization/evaluation.md` (2 hunks) * `doc/localization/position_heading_publisher_node.md` (1 hunks) * `doc/perception/lidar_distance.md` (2 hunks) * `doc/planning/Global_Planner.md` (1 hunks) * `doc/planning/motion_planning.md` (1 hunks) </details> <details> <summary>✅ Files skipped from review due to trivial changes (1)</summary> * doc/control/discontinued/stanley.md </details> <details> <summary>🚧 Files skipped from review as they are similar to previous changes (20)</summary> * doc/planning/motion_planning.md * code-ros1/localization/src/evaluation/save_filter_data.py * code/planning/planning/behavior_agent/behaviors/topics2blackboard.py * code/planning/planning/global_planner/global_plan_distance_publisher.py * doc/acting/discontinued/teb/motion_planner.py * doc/planning/Global_Planner.md * code/perception/tests/test_ego_motion_compensation.py * doc/localization/position_heading_publisher_node.md * code-ros1/acting/src/Acting_Debug_Node.py * code/localization/localization/gps_debug_node.py * code/planning/planning/local_planner/motion_planning.py * code-ros1/localization/src/evaluation/position_heading_filter_debug_node.py * doc/acting/discontinued/potential_field_node.py * doc/acting/discontinued/testbed/src/testbed/potential_field/src/potential_field/p_field_planner.py * code/perception/launch/perception.xml * doc/acting/discontinued/testbed/src/testbed/sim/src/sim/car_sim.py * doc/acting/discontinued/testbed/src/testbed/teb_planner/src/teb_planner.py/teb_planner.py * code/planning/planning/behavior_agent/behaviors/overtake_service_utils.py * doc/localization/evaluation.md * doc/control/discontinued/src/stanley_controller.py </details> <details> <summary>🧰 Additional context used</summary> <details> <summary>🧠 Learnings (5)</summary> <details> <summary>📚 Learning: 2025-02-06T11:20:19.561Z</summary>Learnt from: RoyaLxPole
Repo: una-auxme/paf PR: 687
File: doc/general/create_new_route.md:19-19
Timestamp: 2025-02-06T11:20:19.561Z
Learning: Documentation that references or quotes from the official Carla simulator should maintain exact consistency with the source, even if it contains minor grammatical or stylistic issues.**Applied to files:** - `doc/perception/lidar_distance.md` </details> <details> <summary>📚 Learning: 2024-11-04T11:16:31.149Z</summary>Learnt from: Toni2go
Repo: una-auxme/paf PR: 422
File: doc/research/paf24/perception/VisionNode_CodeSummary.md:114-114
Timestamp: 2024-11-04T11:16:31.149Z
Learning: Invision_node.md, theyolov8x-segmodel performs segmentation (not detection) and can also calculate distances.**Applied to files:** - `doc/perception/lidar_distance.md` </details> <details> <summary>📚 Learning: 2025-01-21T12:12:39.250Z</summary>Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/agent/config/dev_objects.json:182-182
Timestamp: 2025-01-21T12:12:39.250Z
Learning: In the PAF project, the vision system has been simplified to use only the Center camera (removing Back, Left, and Right cameras) as part of the VisionNode refactoring. This change is reflected in both dev_objects.json and agent.py configurations.**Applied to files:** - `doc/perception/lidar_distance.md` </details> <details> <summary>📚 Learning: 2025-01-21T12:12:06.073Z</summary>Learnt from: asamluka
Repo: una-auxme/paf PR: 632
File: code/mapping/src/mapping_data_integration.py:0-0
Timestamp: 2025-01-21T12:12:06.073Z
Learning: The message typesPointcloudCluster.msgandPointcloudClusterArray.msghave been removed and replaced withClusteredPointsArray.msgin the mapping package.**Applied to files:** - `code/perception/perception/perception_utils.py` </details> <details> <summary>📚 Learning: 2025-03-03T22:19:36.933Z</summary>Learnt from: Johannes1098
Repo: una-auxme/paf PR: 731
File: code/planning/src/behavior_agent/behaviors/lane_change.py:567-568
Timestamp: 2025-03-03T22:19:36.933Z
Learning: In the behavior tree for lane change incode/planning/src/behavior_agent/behaviors/lane_change.py, the last sub-behavior (Change) intentionally returnsStatus.FAILUREwhen completed to signal termination of the subtree and exit the lane change behavior sequence.**Applied to files:** - `code/planning/planning/behavior_agent/behaviors/unstuck_routine.py` </details> </details><details> <summary>🧬 Code graph analysis (4)</summary> <details> <summary>code/mapping/mapping/data_integration.py (1)</summary><blockquote> <details> <summary>code/localization/localization/gps_debug_node.py (1)</summary> * `current_pos_callback` (96-97) </details> </blockquote></details> <details> <summary>doc/general/architecture_current.md (1)</summary><blockquote> <details> <summary>code/acting/src/acting/MainFramePublisher.py (1)</summary> * `MainFramePublisher` (13-83) </details> </blockquote></details> <details> <summary>code/perception/perception/perception_utils.py (1)</summary><blockquote> <details> <summary>code/mapping/mapping_common/transform.py (3)</summary> * `translation` (296-304) * `x` (44-45) * `y` (47-48) </details> </blockquote></details> <details> <summary>code/localization/localization/ekf_state_publisher.py (2)</summary><blockquote> <details> <summary>code/paf_common/paf_common/exceptions.py (1)</summary> * `emsg_with_trace` (4-7) </details> <details> <summary>code/acting/src/acting/MainFramePublisher.py (1)</summary> * `MainFramePublisher` (13-83) </details> </blockquote></details> </details><details> <summary>🪛 LanguageTool</summary> <details> <summary>doc/general/architecture_current.md</summary> [style] ~564-~564: For conciseness, consider replacing this expression with an adverb. Context: ...obal_current_heading``` \(no subscriber at the moment\) ([std_msgs/Float32](https://docs.ros.... (AT_THE_MOMENT) --- [style] ~565-~565: For conciseness, consider replacing this expression with an adverb. Context: ...g/global_current_pos``` \(no subscriber at the moment\) ([geometry_msgs/PoseStamped](https://... (AT_THE_MOMENT) </details> <details> <summary>doc/perception/lidar_distance.md</summary> [grammar] ~22-~22: Ensure spelling is correct Context: ...able compensation modes are: ### 2.1 NoCompensation (Baseline) This strategy represents ... (QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1) --- [grammar] ~37-~37: Use a hyphen to join words. Context: ...ated coordinate frame, leading to motion misaligned points if the ego vehicle mov... (QB_NEW_EN_HYPHEN) --- [uncategorized] ~85-~85: Do not mix variants of the same word (‘homogenous’ and ‘homogeneous’) within a single text. Context: ...trix ($\Delta T$) Having defined the homogenous transformation matrices for both $T_i$ ... (EN_WORD_COHERENCY) --- [grammar] ~106-~106: Ensure spelling is correct Context: ...up P'_{env} \cup P_{ego} $$ ### 2.4 LocalCompensation This strategy uses the vehicle's local s... (QB_NEW_EN_ORTHOGRAPHY_ERROR_IDS_1) </details> </details> <details> <summary>🪛 Ruff (0.14.6)</summary> <details> <summary>code/perception/perception/lidar_distance.py</summary> 876-876: Unused method argument: `kwargs` (ARG002) --- 958-958: Unused method argument: `kwargs` (ARG002) </details> <details> <summary>code/localization/localization/ekf_state_publisher.py</summary> 78-78: Do not catch blind exception: `Exception` (BLE001) --- 81-81: Unused method argument: `timer_event` (ARG002) </details> </details> </details> <details> <summary>⏰ Context from checks skipped due to timeout of 90000ms. You can increase the timeout in your CodeRabbit configuration to a maximum of 15 minutes (900000ms). (1)</summary> * GitHub Check: Build and push image </details> <details> <summary>🔇 Additional comments (28)</summary><blockquote> <details> <summary>code/planning/planning/behavior_agent/behaviors/unstuck_routine.py (2)</summary><blockquote> `225-225`: **Consistent topic rename in update method.** The topic rename is correctly and consistently applied in the `update` method, matching the change in the `initialise` method. --- `130-144`: I'll help you verify and rewrite this review comment. Let me start by examining the file and verifying the claims made in the original review. <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # First, let's understand the file structure and examine the relevant lines fd "unstuck_routine.py" -t f </parameter> </invoke> </function_calls> Let me get the file and examine it: <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Check if file exists and get its line count if [ -f "code/planning/planning/behavior_agent/behaviors/unstuck_routine.py" ]; then wc -l "code/planning/planning/behavior_agent/behaviors/unstuck_routine.py" echo "---" # Read the relevant section around lines 130-144 sed -n '120,160p' "code/planning/planning/behavior_agent/behaviors/unstuck_routine.py" | cat -n else echo "File not found at expected path. Searching for it..." find . -name "unstuck_routine.py" -type f 2>/dev/null fi </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # List Python files in planning directory to find the file find . -type d -name "planning" 2>/dev/null | head -5 echo "---" find . -type f -name "unstuck_routine.py" 2>/dev/null </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Search more broadly for the file rg -l "unstuck_routine" -t py 2>/dev/null </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Try to list files at the repository root first ls -la | head -20 echo "---" # Search for any Python file with "unstuck" in its name fd -i "unstuck" -t f </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # List all Python files related to behavior and planning find . -path "*/behavior*" -name "*.py" -type f 2>/dev/null | grep -i unstuck </parameter> </invoke> </function_calls> Let me try a broader search approach: <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Get all files in the repo to understand structure git ls-files | grep -i "unstuck" | head -10 </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # List all tracked files with path information git ls-files | head -30 </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Search for all .py files and filter for unstuck git ls-files "*.py" | grep unstuck </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Let's try to find any file with "unstuck" in its name or content echo "=== Files with 'unstuck' in name ===" git ls-files | grep -i unstuck echo -e "\n=== Files with 'unstuck_routine' in content ===" git ls-files "*.py" | xargs grep -l "unstuck_routine" 2>/dev/null | head -5 </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Try the exact path from the review comment if [ -f "code/planning/planning/behavior_agent/behaviors/unstuck_routine.py" ]; then echo "File found!" wc -l "code/planning/planning/behavior_agent/behaviors/unstuck_routine.py" elif [ -f "code/planning/behavior_agent/behaviors/unstuck_routine.py" ]; then echo "File found at corrected path!" wc -l "code/planning/behavior_agent/behaviors/unstuck_routine.py" else echo "File not found. Searching all Python files..." git ls-files "*.py" | wc -l echo "Total Python files in repo" echo "---" git ls-files "*.py" | tail -20 fi </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Check the current branch and repository state echo "=== Current branch ===" git branch -v echo -e "\n=== Repository status ===" git status --short | head -10 echo -e "\n=== Sample of Python files ===" git ls-files "*.py" | head -10 echo -e "\n=== Looking for behavior files ===" git ls-files "*.py" | grep -i behavio | head -10 </parameter> </invoke> </function_calls> <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Let's search for any Python files with topic names mentioned in the review echo "=== Searching for references to /paf/hero/global_current_pos ===" rg "/paf/hero/global_current_pos" -t py 2>/dev/null | head -10 echo -e "\n=== Searching for references to /paf/hero/current_pos ===" rg "/paf/hero/current_pos" -t py 2>/dev/null | head -10 </parameter> </invoke> </function_calls> Based on my searches, it appears the file or the specific topics mentioned in the review may not be present in the current repository state. Let me verify the review comment more directly by checking if this is a valid pull request review: <function_calls> <invoke name="shell"> <parameter name="script">#!/bin/bash # Search for all Python files to get a sense of the repository echo "=== Python file count ===" git ls-files "*.py" | wc -l echo -e "\n=== Directories in repo ===" git ls-files | cut -d'/' -f1 | sort -u echo -e "\n=== All *.py files (full list) ===" git ls-files "*.py" | sort </parameter> </invoke> </function_calls> **The file specified in the review comment does not appear to exist in this repository.** After comprehensive searches across the repository using multiple methods (git ls-files, fd, rg), I cannot locate: - `code/planning/planning/behavior_agent/behaviors/unstuck_routine.py` (path as specified) - Any file with "unstuck" in its name - References to the topics mentioned (`/paf/hero/global_current_pos`, `/paf/hero/current_pos`) This indicates either: 1. The repository under review does not contain this file 2. The file path in the review comment is incorrect 3. This review comment is for a different branch or repository **Without access to the actual file being reviewed, I cannot verify the claims made in the original review comment regarding topic renaming, logic changes, or the need for verification of topic publishers.** </blockquote></details> <details> <summary>code/planning/planning/global_planner/global_planner_node.py (1)</summary><blockquote> `44-44`: **LGTM! Topic rename is consistent.** The documentation and subscription topic have been correctly updated from `current_pos` to `global_current_pos`, aligning with the broader PR objective to distinguish global and local coordinate frames. Also applies to: 103-103 </blockquote></details> <details> <summary>code/mapping/mapping/data_integration.py (1)</summary><blockquote> `328-334`: **LGTM! Consistent topic updates.** The subscription topics have been correctly updated to `global_current_pos` and `global_current_heading`, maintaining consistency with the PR-wide topic renaming effort. </blockquote></details> <details> <summary>code/acting/acting/passthrough.py (1)</summary><blockquote> `52-61`: **LGTM! Topic mappings updated correctly.** The `TopicMapping` entries for position and heading have been correctly updated to use `global_current_pos` and `global_current_heading`, maintaining consistency with the repository-wide topic renaming. </blockquote></details> <details> <summary>code/localization/localization/ekf_state_publisher.py (3)</summary><blockquote> `45-61`: **LGTM! Well-structured publisher refactoring.** The introduction of separate global and local publishers for position and heading data is a clean implementation that supports the dual-frame architecture. The naming convention clearly distinguishes between global EKF data (from the `global` frame) and local EKF data (from the `odom` frame). --- `81-103`: **LGTM! Clean frame-specific publishing logic.** The refactored `publish_heading` method correctly: - Accepts a `frame_id` parameter to handle both "odom" and "global" frames - Uses the `_prepare_data` helper to eliminate duplication - Routes data to the appropriate publishers based on frame_id The static analysis warning about the unused `timer_event` parameter can be ignored—it's part of the ROS timer callback signature. --- `105-124`: **LGTM! Helper method reduces duplication.** The `_prepare_data` method cleanly extracts the common logic for converting transform data into position and heading, reducing duplication and improving maintainability. </blockquote></details> <details> <summary>code/localization/localization/position_heading_publisher_node.py (5)</summary><blockquote> `137-149`: **LGTM! EKF filter properly supports dual frames.** The EKF position filter correctly creates separate subscribers for both global and local positions, routing them to the appropriate `publish_filter_pos_as_global_current_pos` and `publish_filter_pos_as_local_current_pos` callbacks. --- `176-195`: **Verify: Kalman filter only publishes global heading.** The EKF heading filter creates both global and local subscribers (lines 176-188), but the Kalman filter only creates a global subscriber (lines 190-195). This inconsistency may be intentional, but should be verified: - If the Kalman filter produces only global-frame data, this is correct - If it should also support local frame output like EKF, a local subscriber may be missing Consider confirming whether the Kalman filter should follow the same dual-frame pattern as EKF, or if the current single-frame implementation is by design. --- `219-237`: **LGTM! Publisher setup supports dual frames.** The node correctly initializes separate publishers for global and local position/heading topics, along with their internal state variables (`__global_heading`, `__local_heading`). This structure cleanly supports the dual-frame architecture. --- `292-310`: **LGTM! Heading callbacks properly separated.** The heading publishing logic is correctly split into `publish_global_current_heading` and `publish_local_current_heading`, each updating its corresponding state variable and publisher. --- `358-374`: **LGTM! Position callbacks properly separated.** The filtered position publishing is correctly split into separate methods for global and local frames, matching the pattern established for heading data. </blockquote></details> <details> <summary>doc/general/architecture_current.md (1)</summary><blockquote> `337-338`: **LGTM! Comprehensive documentation updates.** The architecture documentation has been thoroughly updated to reflect the topic renaming from `current_pos`/`current_heading` to `global_current_pos`/`global_current_heading` across all affected subsystems (Localization, Mapping, Planning, Acting). The updates maintain consistency and accuracy throughout the document. Also applies to: 361-362, 438-439, 467-467, 484-484, 510-511, 558-565 </blockquote></details> <details> <summary>code/perception/perception/perception_utils.py (6)</summary><blockquote> `55-84`: **LGTM! Proper ego-motion compensation implementation.** The function correctly: - Copies the input array to prevent mutation - Converts points to homogeneous coordinates - Applies the 4x4 transformation matrix - Extracts and returns the compensated XYZ coordinates The implementation aligns with the mathematical formulation described in the documentation. --- `87-101`: **LGTM! Correct delta transformation computation.** The function properly computes the relative transformation matrix (ΔT = T_prev @ inv(T_cur)) that moves points from the current frame back to the previous frame, matching the mathematical formulation in the documentation. --- `104-126`: **LGTM! Standard transformation matrix construction.** The function correctly converts a `PoseStamped` message into a 4×4 homogeneous transformation matrix using the standard `quaternion_matrix` utility from `tf_transformations`. The implementation properly handles both rotation (from quaternion) and translation components. --- `129-150`: **Verify: Are ego vehicle mask dimensions appropriate?** The function defines the ego vehicle region as a rectangle: - X: -2 to 2 meters (4m length) - Y: -1 to 1 meters (2m width) These dimensions may be reasonable for a typical vehicle, but should be verified against the actual vehicle dimensions used in your CARLA simulation to ensure points are correctly classified. Consider confirming that these hardcoded dimensions match your vehicle model, or making them configurable parameters. --- `153-181`: **Past review concerns addressed; verify rotation semantics.** The previous review flagged two issues: 1. **Input mutation** - ✅ Fixed: Line 169 now creates a copy with `np.copy(points)` 2. **Rotation direction mismatch** - ✅ Fixed: Docstring (line 160) now correctly states "rotates the points by d_heading" matching the implementation However, please verify the rotation semantics are correct for your use case. When compensating for vehicle motion, you typically want to rotate points in the opposite direction of the vehicle's heading change. The current implementation rotates by `d_heading` directly. Confirm this matches your coordinate frame conventions and compensation requirements. --- `184-196`: **Verify: Confirm heading units expected by consumers.** The function returns the heading in **degrees** (line 196: `degrees=True`). Please verify that all consumers of this function expect degree values rather than radians, as ROS convention typically uses radians for angular measurements. </blockquote></details> <details> <summary>doc/perception/lidar_distance.md (4)</summary><blockquote> `43-105`: **LGTM! Thorough EgoMotionCompensation documentation.** The mathematical formulation of the EgoMotionCompensation strategy is comprehensive and well-explained: - Point cloud separation is clearly defined - Homogeneous transformation matrices are properly documented - Delta transformation derivation is correct - Final compensation formula is accurate The documentation effectively explains how external EKF state is used to compute and apply transformations. --- `106-150`: **LGTM! LocalCompensation well documented.** The LocalCompensation strategy documentation clearly explains: - Point cloud separation - Translation component (d_x) calculation - Optional rotation component handling - The compensation procedure The mathematical notation is consistent with the EgoMotionCompensation section, making it easy to compare the two approaches. --- `152-177`: **LGTM! Mode-dependent inputs clearly documented.** The documentation correctly identifies that different compensation strategies require different input topics: - EgoMotionCompensation needs pose data (local_current_pos) - LocalCompensation needs speed and IMU data This makes it clear to users what data dependencies exist for each compensation mode. --- `178-240`: **LGTM! Processing pipeline and outputs well documented.** The updated processing pipeline clearly shows: - Point cloud reception - Motion compensation step (strategy-dependent) - Filtering and preprocessing - Clustering - Distance image calculation - Published topics The note about backward compatibility with Center_S images is helpful for users transitioning from previous versions. </blockquote></details> <details> <summary>code/perception/perception/lidar_distance.py (4)</summary><blockquote> `8-34`: **Imports for strategy framework and motion inputs look coherent** The added imports (ABC/abstractmethod, Optional, PoseStamped/CarlaSpeedometer/Imu/ImageMsg, and perception_utils helpers) are consistent with the new strategy pattern and callbacks and keep dependencies localized in this module. No issues here. --- `195-209`: **Speed callback integration with `LocalCompensation` looks correct** The speed callback: - Uses the right message type (`CarlaSpeedometer`). - Guards on `LocalCompensation` and forwards `velocity.speed` to the strategy. This matches the strategy’s `velocity: Optional[float]` field. Just ensure the units of `velocity.speed` match what `apply_local_motion_compensation` expects (e.g., m/s vs. km/h) so that `d_x = velocity * d_t` is physically correct. --- `211-233`: **IMU callback and heading extraction are reasonable; align with LocalCompensation assumptions** The IMU callback: - Correctly extracts the quaternion components. - Delegates heading computation to `quaternion_to_heading`. - Passes the resulting scalar into `LocalCompensation.set_motion_data(heading=...)`. This is a clean separation, but it makes LocalCompensation sensitive to whatever convention `quaternion_to_heading` uses (frame, yaw sign, wrap at ±π). Given your known issues with rotational compensation accuracy, it’s worth double-checking that: - Heading is in the same frame as LIDAR points. - Its direction/sign matches what `apply_local_motion_compensation` assumes. A simple sanity test is to drive a small constant-radius circle in simulation and inspect whether the compensated cloud stabilizes vs. the world. --- `452-489`: **Perspective projection and image reconstruction remain consistent** The projection logic: - Builds homogeneous point vectors and applies the intrinsic–extrinsic matrix. - Converts to integer pixel indices and bounds-checks them. - Maps coordinates into the 2D image differently per focus ("Center", "Back", "Left", "Right") with appropriate flipping and sign handling. The minor formatting change around line 454 doesn’t affect behavior. The overall mapping remains consistent and looks sound. </blockquote></details> </blockquote></details> </details> <!-- This is an auto-generated comment by CodeRabbit for review status -->
simmatz
left a comment
simmatz
left a comment
There was a problem hiding this comment.
Passt so, läuft auch lokal. Alles tutti.
2 participants
Description
This Pull Request implements modular Point Cloud Compensation strategies within the Lidar Distance Node (lidar_distance.py).
Summary of Change: The core change introduces the Strategy pattern to dynamically select how LiDAR point clouds are corrected for vehicle movement between sensor scans. This addresses the spatial misalignment caused by the 10Hz LiDAR rotation frequency operating in a 20Hz simulation environment.
The implementation uses the Strategy pattern to allow for seamless selection of compensation logic via the compensation_strategy parameter in the perception configuration. The available modes are:
NoCompensation: The original, uncompensated functionality.
Buffer: A simple buffering strategy.
EgoMotionCompensation: A sophisticated approach leveraging external state data (Estimated Kalman Filter, EKF) to calculate and apply homogeneous transformations, aiming for precise point cloud alignment.
LocalCompensation: A simplified compensation method based on local vehicle dynamics (speed and heading change).
Known Limitations and Review Focus Areas
Important Note for Reviewer: While the compensation strategies are fully implemented, preliminary testing has revealed some inconsistencies, which require focused review and further investigation:
Transformation Inaccuracy: Rotational components in both EgoMotionCompensation and LocalCompensation currently lead to inaccurate transformations. The underlying cause (data noise, calculation bug, or environment fidelity) is not entirely clear.
EgoMotionCompensation Warnings: This strategy regularly triggers console warnings during subsequent data processing steps, suggesting invalid calculations or data structures later in the pipeline, despite passing unit tests.
Current Status: Due to the rotational issues, the LocalCompensation strategy currently ignores the heading component to achieve better stability.
Fixes # 858 feature extend lidar to have 360 input every frame
Type of change
Please delete options that are not relevant.
New feature
Does this PR introduce a breaking change?
No. The old functionality is preserved and reproducible by setting the parameter "compensation_strategy" to NoCompensation.
Most important changes
Compensation Implementation: Review the class structure and the underlying logic for EgoMotionCompensation and LocalCompensation, specifically focusing on the transformation matrix creation and the use of external state data (EKF/pose, speed, IMU).
Configuration Interface: Verify the successful initialization and selection of the correct compensation Strategy based on the compensation_strategy configuration parameter.
Documentation (doc/perception/lidar_distance.md): Confirm that the updated documentation clearly and accurately describes the purpose and mathematical concepts of the new compensation sections.
Reviewers are encouraged to Test different compensation strategies, as LocalCompensation (even without heading) is currently showing the most promising real-world results.
Checklist:
Summary by CodeRabbit
New Features
Refactor
Documentation
Tests
✏️ Tip: You can customize this high-level summary in your review settings.