894 radar compensation - #896
Merged
Merged
Conversation
Contributor
|
Note Reviews pausedIt looks like this branch is under active development. To avoid overwhelming you with review comments due to an influx of new commits, CodeRabbit has automatically paused this review. You can configure this behavior by changing the Use the following commands to manage reviews:
Use the checkboxes below for quick actions:
📝 WalkthroughWalkthroughAdds hero-speed ingestion into radar velocity computation, refactors cluster velocity calculation into a RadarNode method, introduces a mapping Changes
Sequence Diagram(s)sequenceDiagram
participant Ego as Ego Vehicle
participant Radar as RadarNode
participant Hero as CarlaSpeedometer
participant Mapping as MappingDataIntegration
participant Entity as Entity
Hero->>Radar: hero_speed_callback (subscribe)
Ego->>Radar: send radar points / clusters
Radar->>Radar: extract & transform points to radar origin
Hero-->>Radar: provide hero speed (when available)
Radar->>Radar: self.calculate_cluster_velocity(points_with_labels)
Radar->>Radar: compute ego-motion-compensated per-point velocities
Radar->>Radar: aggregate per-cluster Motion2D (apply hero speed if present)
Radar-->>Mapping: clustered points + Motion2D array
Mapping->>Mapping: compare motion first-component to classification_threshold
alt below threshold
Mapping->>Mapping: set motion = None (discard)
else above threshold
Mapping->>Entity: use motion for entity creation
end
Entity->>Entity: get_meta_markers -> append speed (km/h) text
Estimated code review effort🎯 3 (Moderate) | ⏱️ ~20 minutes Possibly related PRs
Suggested labels
Suggested reviewers
Poem
🚥 Pre-merge checks | ✅ 3 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (3 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
Contributor
There was a problem hiding this comment.
Actionable comments posted: 3
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
code/mapping/mapping/data_integration.py (1)
689-695:⚠️ Potential issue | 🟡 MinorConsider making the velocity threshold configurable and clarify the filtering logic.
Several concerns with this filtering approach:
Magic number: The threshold
1.5(m/s ≈ 5.4 km/h) should be documented or made a configurable parameter.X-velocity only: This only checks the x-component of the motion vector. A slow object moving perpendicular to the hero could have low x-velocity but significant total velocity.
Private attribute access: Accessing
_matrix[0]directly couples to internal implementation. Consider using the public API.💡 Suggested improvements
+# Threshold below which motion is considered unreliable (m/s) +MOTION_VELOCITY_THRESHOLD = 1.5 + motion = None if motion_array_converted is not None: motion = motion_array_converted[cluster_mask][0] if self.hero_speed is not None: - if np.abs(motion.linear_motion._matrix[0]) < 1.5: + # Filter out low-velocity motion to avoid misclassifying stationary objects + if motion.linear_motion.length() < MOTION_VELOCITY_THRESHOLD: motion = NoneOr make it a node parameter for easier tuning.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping/mapping/data_integration.py` around lines 689 - 695, Replace the hard-coded x-velocity check with a configurable threshold and use the motion object's public API and full velocity magnitude: when computing motion for a cluster (see motion_array_converted[cluster_mask][0] and the hero_speed guard), compute the full linear velocity magnitude via the public accessor on motion.linear_motion (avoid _matrix[0]) and compare it to a new parameter (e.g., velocity_threshold or node param) instead of the magic 1.5; also document the parameter and ensure the logic clearly states it only applies when self.hero_speed is set.
🧹 Nitpick comments (6)
code/mapping/mapping/data_integration.py (1)
638-639: Remove or complete the commented-out code.This commented-out line appears to be incomplete implementation. Either:
- Complete the azimuth angle handling if it's needed, or
- Remove the commented code to avoid confusion
🧹 Suggested cleanup
- # azimutharray = np.array(data.azimuth_angle) if data.azimuth_angle else None -🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping/mapping/data_integration.py` around lines 638 - 639, The commented-out line creating azimutharray is incomplete and should be resolved: either remove the comment to clean up dead code, or implement proper azimuth-angle handling by converting data.azimuth_angle into a numpy array (e.g., set azimutharray = np.array(data.azimuth_angle) if data.azimuth_angle is not None else None) and ensure any downstream references to azimutharray or methods expecting azimuth data are updated (search for azimutharray and data.azimuth_angle in this module to update uses accordingly).code/perception/perception/perception_utils.py (1)
18-31: Update docstring to document the newobject_azimuth_arrayparameter.The
object_azimuth_arrayparameter was added but the docstring does not document it.📝 Suggested docstring update
def array_to_clustered_points( stamp: Time, points, point_indices, object_speed_array=None, object_class_array=None, header_id="hero", object_azimuth_array=None, ): """ Convert the given points and point indices to a ClusteredPointsArray message. Args: points: numpy array with shape (N, 3) point_indices: numpy array with the shape (N,) object_speed_array: numpy array with the shape (N,) object_class_array: numpy array with the shape (N,) header_id: string + object_azimuth_array: numpy array with the shape (N,) containing azimuth angles Returns: ClusteredPointsArray message """🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/perception_utils.py` around lines 18 - 31, The docstring for the function that converts points to a ClusteredPointsArray (the docstring shown above the function that accepts object_azimuth_array) is missing documentation for the new parameter object_azimuth_array; update the Args section to add a line describing object_azimuth_array (e.g., that it is a numpy array with shape (N,) containing azimuth angles for each object, expected units/range if applicable, and that it aligns 1:1 with points/point_indices), keeping the style consistent with the existing entries for object_speed_array and object_class_array so readers can find parameter shape and meaning.code/mapping_interfaces/msg/ClusteredPointsArray.msg (1)
17-19: Note: Theazimuth_anglefield is added but currently unused.The radar node passes
object_azimuth_array=Nonewhen creating clustered points (line 558 in radar_node.py), so this field won't be populated. Consider either:
- Implementing the azimuth angle population in radar_node.py, or
- Adding a TODO comment indicating this is a placeholder for future use
This isn't blocking, but tracking unused fields helps maintain code clarity.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping_interfaces/msg/ClusteredPointsArray.msg` around lines 17 - 19, The new ClusteredPointsArray.msg field azimuth_angle is currently never populated because the radar node passes object_azimuth_array = None; either implement population of azimuth_angle by wiring the radar node's object_azimuth_array into the ClusteredPointsArray creation logic (ensure the code that constructs ClusteredPointsArray uses the object_azimuth_array variable to set the float64[] azimuth_angle field), or add a clear TODO comment next to the azimuth_angle declaration in ClusteredPointsArray.msg indicating it is a placeholder and that object_azimuth_array must be wired in later; reference the azimuth_angle field name and the object_azimuth_array variable when making the change so the intent is explicit.code/planning/planning/behavior_agent/behaviors/intersection.py (1)
61-68: Clarify the relationship between this change and the radar compensation PR.The doubling of
INTERSECTION_START_MIN_DISTANCEfrom 5.0m to 10.0m seems unrelated to the core radar ego-motion compensation changes. Could you clarify:
- Was this change necessary due to the new radar behavior?
- What issue was observed that motivated this increase?
Consider documenting the rationale in the commit message or PR description for future reference.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/planning/planning/behavior_agent/behaviors/intersection.py` around lines 61 - 68, The change doubling INTERSECTION_START_MIN_DISTANCE from 5.0 to 10.0 lacks explanation and may be unrelated to the radar ego-motion compensation PR; add a brief rationale (why it was necessary, what observed behavior motivated the increase, and whether it is required by the new radar logic) to the commit message or PR description and also add an inline comment above INTERSECTION_START_MIN_DISTANCE in intersection.py referencing the observed symptom (e.g., delayed approach triggering, false negatives with radar compensation) and whether this is a temporary workaround or a permanent tuning change so future reviewers can understand the linkage to the radar changes.code/perception/perception/radar_node.py (2)
726-731: Consider logging when fallback velocity is used.The fallback to
Motion2D(Vector2.new(0.0, 0.0), 0.0)at line 729 could mask cases where labels are unexpectedly missing fromavg_motion. Consider adding a debug log when this fallback is triggered to aid troubleshooting.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/radar_node.py` around lines 726 - 731, Add a debug log when the fallback Motion2D(Vector2.new(0.0, 0.0), 0.0) is used so missing avg_motion entries are visible: inside the comprehension that populates motion_array[valid_mask] (using variables motion_array, valid_mask, avg_motion, labels, Motion2D, Vector2) detect when label not in avg_motion and call the module logger.debug or process logger with the label and a brief context (e.g., "avg_motion missing for label"), then return the fallback Motion2D; keep the same comprehension behavior but extract the fallback path to emit the log before constructing the fallback object.
550-551: Remove commented-out code.This commented line is part of the incomplete azimuth angle implementation. Either complete the feature or remove the dead code.
🧹 Suggested cleanup
- # azimuthArray = [m.to_ros_msg() for m in azimuthArray] -🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/radar_node.py` around lines 550 - 551, Remove the dead commented-out line related to the incomplete azimuth angle feature: delete the commented line "# azimuthArray = [m.to_ros_msg() for m in azimuthArray]" from radar_node.py (the azimuthArray handling near the azimuth angle implementation) or, if you intend to implement the feature now, replace it with the active conversion using azimuthArray = [m.to_ros_msg() for m in azimuthArray] and ensure any referenced method to_ros_msg() and variable azimuthArray are defined and tested; prefer removing the commented code if the feature is not being completed in this PR.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@code/perception/perception/radar_node.py`:
- Around line 695-708: calculate_cluster_velocity currently compensates only for
linear ego motion (using self.hero_speed) and ignores self.delta_heading;
compute the rotational (tangential) ego-induced velocity for each point using
the stored self.delta_heading (angular rate) and the point's relative x/y
position, convert to a Vector2 (e.g., v_rot = omega cross r -> (-omega * y,
omega * x) assuming right-handed coords) and add this rotational velocity to the
existing speed_vector + point_motion_vector before writing into motion_vectors;
update calculate_cluster_velocity to reference self.delta_heading when not None
and ensure units (radians/sec) are used consistently with hero_speed
compensation.
- Around line 647-669: The code reorders points by vstacking transformed_points0
and transformed_points1 which breaks alignment with labels and later uses like
labels[valid_mask]; instead apply the translations in-place to preserve original
ordering: compute translation0/translation1 from
self.sensor_config["RADAR0"/"RADAR1"] and subtract them from
points_with_labels[radar0mask, :3] and points_with_labels[radar1mask, :3]
respectively, then remove the np.vstack so points_with_labels and labels remain
aligned for valid_mask and motion_array assignments.
- Around line 609-610: The comment on the sensor flip is misleading: update the
comment near the sensor_name == "RADAR1" branch (where data_array[:, [0, 1]] *=
-1 is applied) to state that only the x and y coordinate axes are mirrored for
the rear-facing radar and that the radial velocity column (e.g., column 3) is
intentionally left unchanged; reference sensor_name, data_array, and the
indexing [:, [0, 1]] so reviewers can locate and verify the behavior.
---
Outside diff comments:
In `@code/mapping/mapping/data_integration.py`:
- Around line 689-695: Replace the hard-coded x-velocity check with a
configurable threshold and use the motion object's public API and full velocity
magnitude: when computing motion for a cluster (see
motion_array_converted[cluster_mask][0] and the hero_speed guard), compute the
full linear velocity magnitude via the public accessor on motion.linear_motion
(avoid _matrix[0]) and compare it to a new parameter (e.g., velocity_threshold
or node param) instead of the magic 1.5; also document the parameter and ensure
the logic clearly states it only applies when self.hero_speed is set.
---
Nitpick comments:
In `@code/mapping_interfaces/msg/ClusteredPointsArray.msg`:
- Around line 17-19: The new ClusteredPointsArray.msg field azimuth_angle is
currently never populated because the radar node passes object_azimuth_array =
None; either implement population of azimuth_angle by wiring the radar node's
object_azimuth_array into the ClusteredPointsArray creation logic (ensure the
code that constructs ClusteredPointsArray uses the object_azimuth_array variable
to set the float64[] azimuth_angle field), or add a clear TODO comment next to
the azimuth_angle declaration in ClusteredPointsArray.msg indicating it is a
placeholder and that object_azimuth_array must be wired in later; reference the
azimuth_angle field name and the object_azimuth_array variable when making the
change so the intent is explicit.
In `@code/mapping/mapping/data_integration.py`:
- Around line 638-639: The commented-out line creating azimutharray is
incomplete and should be resolved: either remove the comment to clean up dead
code, or implement proper azimuth-angle handling by converting
data.azimuth_angle into a numpy array (e.g., set azimutharray =
np.array(data.azimuth_angle) if data.azimuth_angle is not None else None) and
ensure any downstream references to azimutharray or methods expecting azimuth
data are updated (search for azimutharray and data.azimuth_angle in this module
to update uses accordingly).
In `@code/perception/perception/perception_utils.py`:
- Around line 18-31: The docstring for the function that converts points to a
ClusteredPointsArray (the docstring shown above the function that accepts
object_azimuth_array) is missing documentation for the new parameter
object_azimuth_array; update the Args section to add a line describing
object_azimuth_array (e.g., that it is a numpy array with shape (N,) containing
azimuth angles for each object, expected units/range if applicable, and that it
aligns 1:1 with points/point_indices), keeping the style consistent with the
existing entries for object_speed_array and object_class_array so readers can
find parameter shape and meaning.
In `@code/perception/perception/radar_node.py`:
- Around line 726-731: Add a debug log when the fallback
Motion2D(Vector2.new(0.0, 0.0), 0.0) is used so missing avg_motion entries are
visible: inside the comprehension that populates motion_array[valid_mask] (using
variables motion_array, valid_mask, avg_motion, labels, Motion2D, Vector2)
detect when label not in avg_motion and call the module logger.debug or process
logger with the label and a brief context (e.g., "avg_motion missing for
label"), then return the fallback Motion2D; keep the same comprehension behavior
but extract the fallback path to emit the log before constructing the fallback
object.
- Around line 550-551: Remove the dead commented-out line related to the
incomplete azimuth angle feature: delete the commented line "#
azimuthArray = [m.to_ros_msg() for m in azimuthArray]" from radar_node.py (the
azimuthArray handling near the azimuth angle implementation) or, if you intend
to implement the feature now, replace it with the active conversion using
azimuthArray = [m.to_ros_msg() for m in azimuthArray] and ensure any referenced
method to_ros_msg() and variable azimuthArray are defined and tested; prefer
removing the commented code if the feature is not being completed in this PR.
In `@code/planning/planning/behavior_agent/behaviors/intersection.py`:
- Around line 61-68: The change doubling INTERSECTION_START_MIN_DISTANCE from
5.0 to 10.0 lacks explanation and may be unrelated to the radar ego-motion
compensation PR; add a brief rationale (why it was necessary, what observed
behavior motivated the increase, and whether it is required by the new radar
logic) to the commit message or PR description and also add an inline comment
above INTERSECTION_START_MIN_DISTANCE in intersection.py referencing the
observed symptom (e.g., delayed approach triggering, false negatives with radar
compensation) and whether this is a temporary workaround or a permanent tuning
change so future reviewers can understand the linkage to the radar changes.
ℹ️ Review info
Configuration used: defaults
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (7)
code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.pycode/mapping/mapping/data_integration.pycode/mapping/mapping_common/entity.pycode/mapping_interfaces/msg/ClusteredPointsArray.msgcode/perception/perception/perception_utils.pycode/perception/perception/radar_node.pycode/planning/planning/behavior_agent/behaviors/intersection.py
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
reviewed
Feb 26, 2026
simoooong
left a comment
simoooong
left a comment
Collaborator
There was a problem hiding this comment.
Look at comments, other than that looks good to me :+1
Contributor
There was a problem hiding this comment.
Actionable comments posted: 6
♻️ Duplicate comments (1)
code/perception/perception/radar_node.py (1)
645-661:⚠️ Potential issue | 🔴 CriticalCluster label alignment is broken during radar-origin translation.
You drop the label column when rebuilding arrays and also reorder rows via
np.vstack, so downstream “per-cluster average” no longer uses original cluster labels correctly.Proposed fix (preserve order and labels)
- translation0 = np.array([sensor0_x, -sensor0_y, sensor0_z]) - transformed_points0 = np.column_stack( - ( - points_with_labels[radar0mask, :3] - translation0, - points_with_labels[radar0mask, 3], - ) - ) - - translation1 = np.array([sensor1_x, -sensor1_y, sensor1_z]) - transformed_points1 = np.column_stack( - ( - points_with_labels[radar1mask, :3] - translation1, - points_with_labels[radar1mask, 3], - ) - ) - - points_with_labels = np.vstack((transformed_points0, transformed_points1)) + transformed = points_with_labels.copy() # keep [x,y,z,v,label] and order + translation0 = np.array([sensor0_x, -sensor0_y, sensor0_z]) + translation1 = np.array([sensor1_x, -sensor1_y, sensor1_z]) + transformed[radar0mask, :3] -= translation0 + transformed[radar1mask, :3] -= translation1 + points_with_labels = transformed🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/radar_node.py` around lines 645 - 661, The current translation drops the label column and concatenates rows out of original order: when creating transformed_points0/transformed_points1 you only keep :3 and label column but then np.vstack reorders rows so cluster labels no longer align. Fix by preserving the full label column and the original row ordering: update points_with_labels in-place for each mask (e.g., assign points_with_labels[radar0mask, :3] = points_with_labels[radar0mask, :3] - translation0 and similarly for radar1mask) or build transformed arrays that include the full fourth label column and then place them back into their original index positions instead of using np.vstack; reference variables: transformed_points0, transformed_points1, points_with_labels, radar0mask, radar1mask.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@code/mapping/mapping/data_integration.py`:
- Line 356: The long description strings causing Ruff E501 are on the pipeline
parameter and the static_hint declaration; shorten or break those long string
literals so they do not exceed line-length (e.g., split the description into
multiple adjacent string literals inside parentheses or refactor the wording to
be more concise) and apply the same change to the static_hint description to
ensure both Line 356 and Line 704 comply with the line-length limit; update the
description arguments in the functions/constructors that set pipeline and
static_hint accordingly.
- Around line 351-362: The declared parameter self.classification_threshold is
currently assigned the Parameter object returned by declare_parameter; change
the assignment to extract the numeric value (use .double_value) so
self.classification_threshold becomes a float instead of a Parameter object;
update the declaration site that calls
declare_parameter("classification_threshold", 1.5,
descriptor=ParameterDescriptor(...)) and mirror how other numeric params are
handled (e.g., use .double_value) so later numeric comparisons (e.g., in the
method that compares against classification_threshold) won't raise a type error.
- Around line 704-705: The current check uses only
motion.linear_motion._matrix[0] to threshold motion and can drop objects moving
in y; replace that scalar check with the vector magnitude of the linear motion
(e.g., compute the Euclidean norm of the relevant components from
motion.linear_motion._matrix) and compare that magnitude to
self.classification_threshold, setting motion = None if the norm is below the
threshold; update the condition in the mapping/data_integration.py logic where
motion is inspected (reference: motion.linear_motion._matrix and
self.classification_threshold) so it uses np.linalg.norm(...) of the motion
vector instead of the single x component.
In `@code/perception/perception/radar_node.py`:
- Line 13: Remove the unused import Float32 from the top of
perception/radar_node.py and delete the stray whitespace-only blank line near
the bottom of the file (the extra blank at ~line 189) so the module no longer
triggers F401/W293; search for the import statement "from std_msgs.msg import
Float32" and remove it, and remove the empty whitespace-only line in the
surrounding function or module content to restore proper formatting.
- Around line 686-699: The current code only writes into motion_vectors inside
the if self.hero_speed is not None branch, discarding measured
point_motion_vector when hero speed is unavailable; update the logic in
RadarNode (the block around motion_vectors, hypspeed/xspeed/yspeed,
speed_vector, vec and point_motion_vector) so that when self.hero_speed is None
you fall back to assigning the raw measured motion (point_motion_vector.x(),
point_motion_vector.y()) into motion_vectors[i,0] and [i,1] and still set
motion_vectors[i,2] = point[-1]; keep the existing compensated-vector assignment
when hero speed is present.
- Around line 549-550: Call to array_to_clustered_points in radar_node.py is
passing an unsupported keyword object_azimuth_array which causes a TypeError;
remove the object_azimuth_array=None argument from the
array_to_clustered_points(...) invocation so the call matches the function
signature (array_to_clustered_points with params stamp, points, point_indices,
optional object_speed_array, object_class_array, header_id) and ensure only
supported args (e.g., object_speed_array, object_class_array, header_id="hero")
are passed.
---
Duplicate comments:
In `@code/perception/perception/radar_node.py`:
- Around line 645-661: The current translation drops the label column and
concatenates rows out of original order: when creating
transformed_points0/transformed_points1 you only keep :3 and label column but
then np.vstack reorders rows so cluster labels no longer align. Fix by
preserving the full label column and the original row ordering: update
points_with_labels in-place for each mask (e.g., assign
points_with_labels[radar0mask, :3] = points_with_labels[radar0mask, :3] -
translation0 and similarly for radar1mask) or build transformed arrays that
include the full fourth label column and then place them back into their
original index positions instead of using np.vstack; reference variables:
transformed_points0, transformed_points1, points_with_labels, radar0mask,
radar1mask.
ℹ️ Review info
Configuration used: defaults
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
code/mapping/mapping/data_integration.pycode/mapping_interfaces/msg/ClusteredPointsArray.msgcode/perception/perception/radar_node.py
🚧 Files skipped from review as they are similar to previous changes (1)
- code/mapping_interfaces/msg/ClusteredPointsArray.msg
Comment on lines
+351
to
+362
| self.classification_threshold = ( | ||
| self.declare_parameter( | ||
| "classification_threshold", | ||
| 1.5, | ||
| descriptor=ParameterDescriptor( | ||
| description="Threshold under which an entity is classified as stationary", | ||
| floating_point_range=[ | ||
| FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) | ||
| ], | ||
| ), | ||
| ) | ||
| ) |
Contributor
There was a problem hiding this comment.
classification_threshold is stored as a Parameter object instead of a float.
At Line 704 this is compared numerically, which can raise a type error at runtime. Convert the declared parameter to .double_value like the other numeric params.
Proposed fix
self.classification_threshold = (
self.declare_parameter(
"classification_threshold",
1.5,
descriptor=ParameterDescriptor(
description="Threshold under which an entity is classified as stationary",
floating_point_range=[
FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1)
],
),
)
+ .get_parameter_value()
+ .double_value
)📝 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.
Suggested change
| self.classification_threshold = ( | |
| self.declare_parameter( | |
| "classification_threshold", | |
| 1.5, | |
| descriptor=ParameterDescriptor( | |
| description="Threshold under which an entity is classified as stationary", | |
| floating_point_range=[ | |
| FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) | |
| ], | |
| ), | |
| ) | |
| ) | |
| self.classification_threshold = ( | |
| self.declare_parameter( | |
| "classification_threshold", | |
| 1.5, | |
| descriptor=ParameterDescriptor( | |
| description="Threshold under which an entity is classified as stationary", | |
| floating_point_range=[ | |
| FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) | |
| ], | |
| ), | |
| ) | |
| .get_parameter_value() | |
| .double_value | |
| ) |
🧰 Tools
🪛 GitHub Actions: Lint python code with ruff
[error] 356-356: Ruff check: E501 Line too long in data_integration.py:356 (94 > 88).
🪛 GitHub Check: Lint python code with ruff
[failure] 356-356: Ruff (E501)
code/mapping/mapping/data_integration.py:356:89: E501 Line too long (94 > 88)
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@code/mapping/mapping/data_integration.py` around lines 351 - 362, The
declared parameter self.classification_threshold is currently assigned the
Parameter object returned by declare_parameter; change the assignment to extract
the numeric value (use .double_value) so self.classification_threshold becomes a
float instead of a Parameter object; update the declaration site that calls
declare_parameter("classification_threshold", 1.5,
descriptor=ParameterDescriptor(...)) and mirror how other numeric params are
handled (e.g., use .double_value) so later numeric comparisons (e.g., in the
method that compares against classification_threshold) won't raise a type error.
Comment on lines
+704
to
+705
| if np.abs(motion.linear_motion._matrix[0]) < self.classification_threshold: | ||
| motion = None |
Contributor
There was a problem hiding this comment.
Thresholding only linear_motion.x can misclassify moving objects as stationary.
Line 704 uses only the x-component; objects moving mainly in y can be dropped incorrectly. Use vector magnitude for classification.
Proposed fix
- if np.abs(motion.linear_motion._matrix[0]) < self.classification_threshold:
+ if motion.linear_motion.length() < self.classification_threshold:
motion = None🧰 Tools
🪛 GitHub Check: Lint python code with ruff
[failure] 704-704: Ruff (E501)
code/mapping/mapping/data_integration.py:704:89: E501 Line too long (95 > 88)
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@code/mapping/mapping/data_integration.py` around lines 704 - 705, The current
check uses only motion.linear_motion._matrix[0] to threshold motion and can drop
objects moving in y; replace that scalar check with the vector magnitude of the
linear motion (e.g., compute the Euclidean norm of the relevant components from
motion.linear_motion._matrix) and compare that magnitude to
self.classification_threshold, setting motion = None if the norm is below the
threshold; update the condition in the mapping/data_integration.py logic where
motion is inspected (reference: motion.linear_motion._matrix and
self.classification_threshold) so it uses np.linalg.norm(...) of the motion
vector instead of the single x component.
Comment on lines
549
to
550
| object_azimuth_array=None, | ||
| ) |
Contributor
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Verify signature of array_to_clustered_points and call sites.
rg -n -C3 '^def array_to_clustered_points\(' code/perception/perception/perception_utils.py
rg -n -C3 'array_to_clustered_points\(' code/perception/perception/radar_node.pyRepository: una-auxme/paf
Length of output: 823
🏁 Script executed:
#!/bin/bash
# Read the complete function signature
head -n 30 code/perception/perception/perception_utils.py | tail -n 25
# Check exact lines 549-550 and surrounding context in radar_node.py
sed -n '540,570p' code/perception/perception/radar_node.pyRepository: una-auxme/paf
Length of output: 1867
Runtime error: array_to_clustered_points() does not accept object_azimuth_array parameter.
The function signature at code/perception/perception/perception_utils.py lines 11-17 only accepts object_speed_array, object_class_array, header_id, and the positional arguments. Passing object_azimuth_array=None at line 549 will cause a TypeError: unexpected keyword argument at runtime.
Function signature
def array_to_clustered_points(
stamp: Time,
points,
point_indices,
object_speed_array=None,
object_class_array=None,
header_id="hero",
):Remove the object_azimuth_array=None argument from the call.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@code/perception/perception/radar_node.py` around lines 549 - 550, Call to
array_to_clustered_points in radar_node.py is passing an unsupported keyword
object_azimuth_array which causes a TypeError; remove the
object_azimuth_array=None argument from the array_to_clustered_points(...)
invocation so the call matches the function signature (array_to_clustered_points
with params stamp, points, point_indices, optional object_speed_array,
object_class_array, header_id) and ensure only supported args (e.g.,
object_speed_array, object_class_array, header_id="hero") are passed.
Comment on lines
+686
to
+699
| if self.hero_speed is not None: | ||
| # bend ego motion vector towards object and compensate movement | ||
| hypspeed = self.hero_speed.speed * cos_azimuth_per_point | ||
| xspeed = hypspeed * cos_azimuth_per_point | ||
| yspeed = hypspeed * np.sin(azimuth_per_point) | ||
| speed_vector = Vector2.new(xspeed, yspeed) | ||
|
|
||
| vec = speed_vector + point_motion_vector | ||
|
|
||
| # array of the points motions in the form | ||
| # (x-velocity, y-velocity, label) | ||
| motion_vectors[i, 0] = vec.x() | ||
| motion_vectors[i, 1] = vec.y() | ||
| motion_vectors[i, 2] = point[-1] |
Contributor
There was a problem hiding this comment.
Measured point motion is discarded when hero speed is unavailable.
motion_vectors is only filled inside if self.hero_speed is not None, so valid points default to zero motion until speed arrives. Keep raw point motion as fallback.
Proposed fix
- if self.hero_speed is not None:
+ vec = point_motion_vector
+ if self.hero_speed is not None:
# bend ego motion vector towards object and compensate movement
hypspeed = self.hero_speed.speed * cos_azimuth_per_point
xspeed = hypspeed * cos_azimuth_per_point
yspeed = hypspeed * np.sin(azimuth_per_point)
speed_vector = Vector2.new(xspeed, yspeed)
-
- vec = speed_vector + point_motion_vector
-
- # array of the points motions in the form
- # (x-velocity, y-velocity, label)
- motion_vectors[i, 0] = vec.x()
- motion_vectors[i, 1] = vec.y()
- motion_vectors[i, 2] = point[-1]
+ vec = speed_vector + point_motion_vector
+
+ # array of the points motions in the form
+ # (x-velocity, y-velocity, label)
+ motion_vectors[i, 0] = vec.x()
+ motion_vectors[i, 1] = vec.y()
+ motion_vectors[i, 2] = point[-1]🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@code/perception/perception/radar_node.py` around lines 686 - 699, The current
code only writes into motion_vectors inside the if self.hero_speed is not None
branch, discarding measured point_motion_vector when hero speed is unavailable;
update the logic in RadarNode (the block around motion_vectors,
hypspeed/xspeed/yspeed, speed_vector, vec and point_motion_vector) so that when
self.hero_speed is None you fall back to assigning the raw measured motion
(point_motion_vector.x(), point_motion_vector.y()) into motion_vectors[i,0] and
[i,1] and still set motion_vectors[i,2] = point[-1]; keep the existing
compensated-vector assignment when hero speed is present.
Contributor
There was a problem hiding this comment.
♻️ Duplicate comments (5)
code/mapping/mapping/data_integration.py (2)
351-362:⚠️ Potential issue | 🔴 Critical
classification_thresholdis aParameterobject, not a float.At Line 704, numeric comparison uses
self.classification_threshold, but Lines 351-362 never extract.double_value. This can fail at runtime.Proposed fix
self.classification_threshold = ( self.declare_parameter( "classification_threshold", 1.5, descriptor=ParameterDescriptor( description="Threshold when an entity is classified as stationary", floating_point_range=[ FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) ], ), ) + .get_parameter_value() + .double_value )#!/bin/bash # Verify the declaration extracts a numeric value (double_value) instead of keeping Parameter. rg -n -C3 "classification_threshold" code/mapping/mapping/data_integration.pyExpected: the declaration chain includes
.get_parameter_value().double_value.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping/mapping/data_integration.py` around lines 351 - 362, The declared self.classification_threshold is being stored as a Parameter object instead of a numeric float; change the declaration in the block that calls declare_parameter(...) so you assign the numeric value via .get_parameter_value().double_value (e.g., set self.classification_threshold = self.declare_parameter(...).get_parameter_value().double_value) so comparisons later (the code that references self.classification_threshold) operate on a float rather than a Parameter object.
704-706:⚠️ Potential issue | 🟠 MajorUse motion magnitude for stationary filtering, not only x-component.
Line 704 checks only one component. Objects moving mainly on y can be incorrectly treated as stationary.
Proposed fix
- if (np.abs(motion.linear_motion._matrix[0]) < - self.classification_threshold): + if motion.linear_motion.length() < self.classification_threshold: motion = None🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping/mapping/data_integration.py` around lines 704 - 706, The stationary check currently inspects only the x-component (np.abs(motion.linear_motion._matrix[0])) which misclassifies motions along y; instead compute the vector magnitude (e.g., np.linalg.norm of motion.linear_motion._matrix or the relevant 2D/3D slice) and compare that magnitude to self.classification_threshold; also guard against motion or motion.linear_motion being None before computing the norm and set motion = None when the magnitude is below the threshold (update the code around the existing check referencing motion and motion.linear_motion._matrix).code/perception/perception/radar_node.py (3)
685-699:⚠️ Potential issue | 🟠 MajorPreserve raw point motion when hero speed is unavailable.
At Line 685,
motion_vectorsis only populated insideif self.hero_speed is not None. Without hero speed, measuredpoint_motion_vectoris dropped and cluster motion collapses to defaults.Proposed fix
- if self.hero_speed is not None: + vec = point_motion_vector + if self.hero_speed is not None: # bend ego motion vector towards object and compensate movement hypspeed = self.hero_speed.speed * cos_azimuth_per_point xspeed = hypspeed * cos_azimuth_per_point yspeed = hypspeed * np.sin(azimuth_per_point) speed_vector = Vector2.new(xspeed, yspeed) - vec = speed_vector + point_motion_vector - # array of the points motions in the form - # (x-velocity, y-velocity, label) - motion_vectors[i, 0] = vec.x() - motion_vectors[i, 1] = vec.y() - motion_vectors[i, 2] = point[-1] + # array of the points motions in the form + # (x-velocity, y-velocity, label) + motion_vectors[i, 0] = vec.x() + motion_vectors[i, 1] = vec.y() + motion_vectors[i, 2] = point[-1]🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/radar_node.py` around lines 685 - 699, The current block in radar_node.py only writes to motion_vectors when self.hero_speed is not None, dropping measured point_motion_vector otherwise; modify the logic in the method containing this block so that when self.hero_speed is None you preserve the raw measured motion by setting vec = point_motion_vector (instead of skipping the write) and still populate motion_vectors[i,0], motion_vectors[i,1], motion_vectors[i,2] with vec.x(), vec.y(), and point[-1]; keep use of the existing Vector2/point_motion_vector variables and existing array assignment so cluster motion is correct whether or not hero_speed is available.
542-549:⚠️ Potential issue | 🔴 CriticalRemove unsupported
object_azimuth_arrayargument to avoid a runtimeTypeError.At Line 548,
array_to_clustered_points(...)is called withobject_azimuth_array=None, but that argument is not accepted by the function signature incode/perception/perception/perception_utils.py.Proposed fix
clusteredpoints = array_to_clustered_points( self.get_clock().now(), clusterPointsNpArray, indexArray, motionArray, header_id="hero/RADAR", - object_azimuth_array=None, )🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/radar_node.py` around lines 542 - 549, Call to array_to_clustered_points includes an unsupported keyword object_azimuth_array which causes a TypeError; remove the object_azimuth_array=None argument from the call in radar_node (the array_to_clustered_points invocation) so it matches the function signature in perception_utils (array_to_clustered_points). Ensure no other callers pass that keyword and run tests to confirm no remaining references to object_azimuth_array.
643-663:⚠️ Potential issue | 🔴 CriticalVelocity aggregation loses cluster labels and misaligns results.
Lines 643-659 rebuild points with only
[x, y, z, velocity], then Line 662 treats the last column as labels. That means velocity values are used as labels during averaging/assignment, andnp.vstackalso changes row order.Proposed fix (preserve labels + original ordering)
- translation0 = np.array([sensor0_x, -sensor0_y, sensor0_z]) - transformed_points0 = np.column_stack( - ( - points_with_labels[radar0mask, :3] - translation0, - points_with_labels[radar0mask, 3], - ) - ) - - translation1 = np.array([sensor1_x, -sensor1_y, sensor1_z]) - transformed_points1 = np.column_stack( - ( - points_with_labels[radar1mask, :3] - translation1, - points_with_labels[radar1mask, 3], - ) - ) - - points_with_labels = np.vstack((transformed_points0, transformed_points1)) + translation0 = np.array([sensor0_x, -sensor0_y, sensor0_z]) + translation1 = np.array([sensor1_x, -sensor1_y, sensor1_z]) + transformed = points_with_labels.copy() + transformed[radar0mask, :3] -= translation0 + transformed[radar1mask, :3] -= translation1 + points_with_labels = transformedAlso applies to: 659-660, 716-721
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/perception/perception/radar_node.py` around lines 643 - 663, The code rebuilds transformed_points0/1 with only [x,y,z,velocity] causing velocity to be mistaken for cluster labels and row order to change; fix by preserving all original columns (so labels remain in their original column) and keep original row ordering when applying per-radar transforms: for each mask (radar0mask, radar1mask) compute transformed_xyz = points_with_labels[mask, :3] - translationX and then horizontally concatenate transformed_xyz with the remaining original columns points_with_labels[mask, 3:] (e.g., via np.hstack or np.concatenate along axis=1) to form transformed_pointsX, and instead of simply vstacking, write these transformed rows back into a copy of points_with_labels at the same original indices (e.g., out_points = points_with_labels.copy(); out_points[radar0mask] = transformed_points0; out_points[radar1mask] = transformed_points1) so labels and ordering remain correct before downstream filtering.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Duplicate comments:
In `@code/mapping/mapping/data_integration.py`:
- Around line 351-362: The declared self.classification_threshold is being
stored as a Parameter object instead of a numeric float; change the declaration
in the block that calls declare_parameter(...) so you assign the numeric value
via .get_parameter_value().double_value (e.g., set self.classification_threshold
= self.declare_parameter(...).get_parameter_value().double_value) so comparisons
later (the code that references self.classification_threshold) operate on a
float rather than a Parameter object.
- Around line 704-706: The stationary check currently inspects only the
x-component (np.abs(motion.linear_motion._matrix[0])) which misclassifies
motions along y; instead compute the vector magnitude (e.g., np.linalg.norm of
motion.linear_motion._matrix or the relevant 2D/3D slice) and compare that
magnitude to self.classification_threshold; also guard against motion or
motion.linear_motion being None before computing the norm and set motion = None
when the magnitude is below the threshold (update the code around the existing
check referencing motion and motion.linear_motion._matrix).
In `@code/perception/perception/radar_node.py`:
- Around line 685-699: The current block in radar_node.py only writes to
motion_vectors when self.hero_speed is not None, dropping measured
point_motion_vector otherwise; modify the logic in the method containing this
block so that when self.hero_speed is None you preserve the raw measured motion
by setting vec = point_motion_vector (instead of skipping the write) and still
populate motion_vectors[i,0], motion_vectors[i,1], motion_vectors[i,2] with
vec.x(), vec.y(), and point[-1]; keep use of the existing
Vector2/point_motion_vector variables and existing array assignment so cluster
motion is correct whether or not hero_speed is available.
- Around line 542-549: Call to array_to_clustered_points includes an unsupported
keyword object_azimuth_array which causes a TypeError; remove the
object_azimuth_array=None argument from the call in radar_node (the
array_to_clustered_points invocation) so it matches the function signature in
perception_utils (array_to_clustered_points). Ensure no other callers pass that
keyword and run tests to confirm no remaining references to
object_azimuth_array.
- Around line 643-663: The code rebuilds transformed_points0/1 with only
[x,y,z,velocity] causing velocity to be mistaken for cluster labels and row
order to change; fix by preserving all original columns (so labels remain in
their original column) and keep original row ordering when applying per-radar
transforms: for each mask (radar0mask, radar1mask) compute transformed_xyz =
points_with_labels[mask, :3] - translationX and then horizontally concatenate
transformed_xyz with the remaining original columns points_with_labels[mask, 3:]
(e.g., via np.hstack or np.concatenate along axis=1) to form
transformed_pointsX, and instead of simply vstacking, write these transformed
rows back into a copy of points_with_labels at the same original indices (e.g.,
out_points = points_with_labels.copy(); out_points[radar0mask] =
transformed_points0; out_points[radar1mask] = transformed_points1) so labels and
ordering remain correct before downstream filtering.
Contributor
There was a problem hiding this comment.
♻️ Duplicate comments (2)
code/mapping/mapping/data_integration.py (2)
702-705:⚠️ Potential issue | 🟠 MajorThresholding only
linear_motion.xcan misclassify lateral motion as stationary.Use velocity magnitude (x/y norm) for classification instead of a single component.
Proposed fix
- if ( - np.abs(motion.linear_motion._matrix[0]) - < self.classification_threshold - ): + velocity_xy = np.asarray(motion.linear_motion._matrix).reshape(-1)[:2] + if np.linalg.norm(velocity_xy) < self.classification_threshold: motion = None🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping/mapping/data_integration.py` around lines 702 - 705, The current check only tests motion.linear_motion._matrix[0] against classification_threshold which can mislabel lateral movement as stationary; update the classification in the method using motion.linear_motion._matrix to compute the velocity magnitude (e.g., np.linalg.norm of the x and y components from motion.linear_motion._matrix) and compare that magnitude to self.classification_threshold instead of checking only index 0, ensuring both x and y are considered when deciding stationary vs moving.
351-360:⚠️ Potential issue | 🔴 Critical
classification_thresholdis stored as aParameter, not a float.This assignment should extract
.double_value; otherwise Line 704 performs numeric comparison against aParameterobject and can fail at runtime.Proposed fix
- self.classification_threshold = self.declare_parameter( - "classification_threshold", - 1.5, - descriptor=ParameterDescriptor( - description="Threshold when an entity is classified as stationary", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) - ], - ), - ) + self.classification_threshold = ( + self.declare_parameter( + "classification_threshold", + 1.5, + descriptor=ParameterDescriptor( + description="Threshold when an entity is classified as stationary", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) + ], + ), + ) + .get_parameter_value() + .double_value + )In ROS 2 rclpy, what does Node.declare_parameter(...) return, and what is the idiomatic way to retrieve a float value for numeric comparisons?🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@code/mapping/mapping/data_integration.py` around lines 351 - 360, The declared attribute classification_threshold is currently assigned the Parameter object returned by self.declare_parameter; change the assignment to store the numeric float instead by extracting the parameter's double value (e.g., use the declared Parameter's get_parameter_value().double_value) so downstream numeric comparisons (e.g., in the code around the numeric check at line ~704) operate on a float rather than a Parameter object; update any usages expecting a float accordingly.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Duplicate comments:
In `@code/mapping/mapping/data_integration.py`:
- Around line 702-705: The current check only tests
motion.linear_motion._matrix[0] against classification_threshold which can
mislabel lateral movement as stationary; update the classification in the method
using motion.linear_motion._matrix to compute the velocity magnitude (e.g.,
np.linalg.norm of the x and y components from motion.linear_motion._matrix) and
compare that magnitude to self.classification_threshold instead of checking only
index 0, ensuring both x and y are considered when deciding stationary vs
moving.
- Around line 351-360: The declared attribute classification_threshold is
currently assigned the Parameter object returned by self.declare_parameter;
change the assignment to store the numeric float instead by extracting the
parameter's double value (e.g., use the declared Parameter's
get_parameter_value().double_value) so downstream numeric comparisons (e.g., in
the code around the numeric check at line ~704) operate on a float rather than a
Parameter object; update any usages expecting a float accordingly.
6 tasks
7 tasks
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Description
Please include a summary of the change and which issue is fixed. Please also include relevant motivation and context. List any dependencies that are required for this change.
Fixes #892 High Velocities on stationary objects
Type of change
Please delete options that are not
Does this PR introduce a breaking change?
Yes but all functionality is still usable
Most important changes
Checklist:
Summary by CodeRabbit
New Features
Improvements
Documentation