Position Change of RADAR1 and cross traffic detection using radar data - #874
Conversation
📝 WalkthroughWalkthroughAdds radar and cross-traffic detection: new ROS launch files and a RadarDump node, expands radar FOV, adds CROSS_TRAFFIC_SPEED_THRESHOLD, and integrates priority cross-traffic checks and an emergency Bool publisher into intersection behaviors and the behavior tree. Changes
Sequence Diagram(s)sequenceDiagram
participant Perception as Perception (Radar)
participant Mapping as Mapping (Cross-Traffic)
participant Planning as Planning (Intersection)
participant Vehicle as Vehicle (Controller)
Perception->>Mapping: Publish obstacle points
Mapping->>Mapping: build detection mask & filter by speed (CROSS_TRAFFIC_SPEED_THRESHOLD)
Mapping-->>Planning: Return (detected, mask)
Planning->>Planning: check_priority_cross_traffic()
alt Priority cross-traffic detected
Planning->>Vehicle: set_line_stop()
Planning->>Planning: publish emergency_pub Bool(true)
else No priority cross-traffic
Planning->>Vehicle: proceed
Planning->>Planning: publish emergency_pub Bool(false)
end
Estimated code review effort🎯 4 (Complex) | ⏱️ ~45 minutes Possibly related issues
Possibly related PRs
Suggested labels
Suggested reviewers
Poem
🚥 Pre-merge checks | ✅ 2 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (2 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 |
…consistantly after coming to a stop
267e524 to
4b64f40
Compare
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 2
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (2)
code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py (1)
42-53: Reduce RADAR0 and RADAR1 horizontal_fov from 130° to 30–60°.CARLA's radar is a simplified raycast-based implementation (not physics-based), and setting horizontal_fov to 130° will significantly degrade detection accuracy:
- Lower angular resolution: fixed ray count spread over wider cone reduces detection density for small/nearby objects
- Ghost detections & depth errors: sparse rays hitting edges produce spurious/phantom detections and incorrect velocity/depth readings
- Unrealistic for use case: automotive mid-range radars (cross-traffic detection) typically use 20–60° HFOV, not 130°
CARLA's documentation and examples show 30–35° FOV. A value of 130° approaches short-range radar specs and will cause the raycast sensor to degrade. Reduce to 30–60° for realistic cross-traffic detection performance.
code/planning/planning/behavior_agent/behavior_tree.py (1)
258-274: Remove duplicatecurr_behavior_pubdeclaration.Lines 258-260 create a publisher with a hardcoded topic, but lines 267-269 immediately overwrite it with a role-based topic using
self.role_name. The first declaration is dead code and should be removed.♻️ Proposed fix
# Publishers - self.curr_behavior_pub = self.create_publisher( - String, "/paf/hero/curr_behavior", 1 - ) self.marker_publisher = self.create_publisher( MarkerArray, "/paf/hero/behavior_tree/debug_markers", 1 ) self.info_publisher = self.create_publisher( Marker, "/paf/hero/behavior_tree/info_marker", 1 ) self.curr_behavior_pub = self.create_publisher( String, f"/paf/{self.role_name}/curr_behavior", 1 )Also consider whether
marker_publisherandinfo_publishershould useself.role_namefor consistency with the role-based topic pattern established bycurr_behavior_pubandemergency_pub.
🤖 Fix all issues with AI agents
In `@code/mapping/mapping_common/map.py`:
- Around line 950-1002: The has_cross_traffic function is broken: it uses an
invalid self parameter, mixes Map and MapTree APIs, and calls several methods
incorrectly; replace it by either removing it and delegating to the existing
check_cross_traffic(map: Map, tree: MapTree) in intersection.py, or rewrite to
the same signature and logic as check_cross_traffic (accept separate map and
tree args), use e.entity.transform.translation() (call), compute other_pos_self
with hero_tf_inv * other_pos_world (use * operator), and compute rotated
velocity with hero_tf_inv * Vector2.new(v_world.x(), v_world.y()); also ensure
you only call Map methods on the Map instance and MapTree methods on the MapTree
instance and import/ reuse CROSS_TRAFFIC_SPEED_THRESHOLD and mask construction
logic from intersection.py.
In `@code/perception/perception/radar_raw_debugger.py`:
- Around line 6-8: The two bare string literals ("This file can be used to
output the raw data from the radar sensors." and "To execute, this file must be
run in a terminal while the simulation is running.") should be converted into a
proper module docstring at the top of the file (before any imports) or turned
into comments; move those exact sentences to the file start as a triple-quoted
docstring (PEP 257) or prefix them with # to make them comments so they are not
evaluated and discarded at runtime.
🧹 Nitpick comments (4)
code/perception/perception/radar_raw_debugger.py (3)
10-15: Consider making the topic configurable via a ROS parameter.The hardcoded topic works for debugging but limits flexibility. For a debug utility, it would be useful to switch between RADAR0/RADAR1 without code changes.
Optional: Parameterize the topic
class RadarDump(Node): def __init__(self): super().__init__("radar_dump") - topic = "/carla/hero/RADAR0" # Radar whose data is to be displayed + self.declare_parameter("topic", "/carla/hero/RADAR0") + topic = self.get_parameter("topic").get_parameter_value().string_value self.sub = self.create_subscription(PointCloud2, topic, self.cb, 10) self.get_logger().info(f"Listening on {topic}")
28-34: Preferenumerate()withitertools.islicefor cleaner iteration.The manual counter pattern can be replaced with more idiomatic Python.
Suggested refactor
+from itertools import islice + # first 10 points - i = 0 - for p in point_cloud2.read_points(msg, field_names=field_names, skip_nans=True): - self.get_logger().info(f"P{i}: {p}") - i += 1 - if i >= 10: - break + points = point_cloud2.read_points(msg, field_names=field_names, skip_nans=True) + for i, p in enumerate(islice(points, 10)): + self.get_logger().info(f"P{i}: {p}")
37-42: Add exception handling for graceful shutdown on interrupt.If the node is interrupted (e.g., Ctrl+C),
destroy_node()andshutdown()may be skipped. Wrapping in try/finally ensures proper cleanup.Suggested fix
def main(): rclpy.init() node = RadarDump() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown()code/planning/planning/behavior_agent/behaviors/intersection.py (1)
102-114: Duplicate constant:CROSS_TRAFFIC_SPEED_THRESHOLDis defined in both files.This constant is also defined in
code/mapping/mapping_common/map.pyat line 34. Having the same constant in two places creates a maintenance burden and risk of drift.Consider consolidating by:
- Keeping it only in
map.pyand importing it here, or- Moving all cross-traffic constants to a shared constants module
♻️ Option 1: Import from map.py
-from mapping_common.map import Map, MapTree +from mapping_common.map import Map, MapTree, CROSS_TRAFFIC_SPEED_THRESHOLD # Cross traffic parameter -CROSS_TRAFFIC_SPEED_THRESHOLD = 2.5 # m/s CROSS_CHECK_DISTANCE = 15.0
📜 Review details
Configuration used: defaults
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (7)
code/agent/launch/agent.manual.xmlcode/leaderboard_launcher/launch/ros_bridge.dev.xmlcode/leaderboard_launcher/leaderboard_launcher/paf_agent_base.pycode/mapping/mapping_common/map.pycode/perception/perception/radar_raw_debugger.pycode/planning/planning/behavior_agent/behavior_tree.pycode/planning/planning/behavior_agent/behaviors/intersection.py
🧰 Additional context used
🧠 Learnings (2)
📚 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:
code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py
📚 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/intersection.py
🧬 Code graph analysis (2)
code/mapping/mapping_common/map.py (3)
code/mapping/mapping_common/shape.py (6)
Polygon(245-420)Rectangle(130-190)to_shapely(117-126)to_shapely(176-190)to_shapely(230-241)to_shapely(381-387)code/mapping/mapping_common/transform.py (12)
Transform2D(262-433)new_translation(355-363)Vector2(116-258)new(74-76)new(173-175)length(121-129)to_shapely(93-94)inverse(319-325)x(44-45)y(47-48)translation(296-304)rotation(306-317)code/mapping/mapping_common/entity.py (2)
get_front_x(624-632)to_shapely(501-507)
code/planning/planning/behavior_agent/behavior_tree.py (2)
code/planning/planning/behavior_agent/behaviors/intersection.py (1)
Enter(749-871)code/planning/planning/behavior_agent/behaviors/overtake.py (1)
Enter(504-572)
🔇 Additional comments (18)
code/perception/perception/radar_raw_debugger.py (2)
1-4: LGTM!Imports are appropriate for a ROS 2 node subscribing to PointCloud2 messages.
45-46: LGTM!Standard Python entry point pattern.
code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py (1)
54-65: Verify RADAR1 position matches PR objectives.The PR description states "RADAR1 position changed to the front and middle of the car," but the code shows RADAR1 at
x=-2.0(rear) andy=-1.5(offset right), withyaw=180°(facing backward). The commit history mentions the position was "changed back to old position" — please confirm this is the intended final configuration for cross-traffic detection.Additionally, both radars are offset to
y=-1.5(right side). For effective cross-traffic detection at intersections, consider whether coverage on the left side is sufficient.code/agent/launch/agent.manual.xml (1)
1-23: LGTM with minor observations.The launch file correctly sets up the manual control environment with perception, carla_manual_control, rqt_gui, and rviz nodes.
Two minor notes:
- The
control_loop_rateargument (line 3) is declared but unused sinceacting.launchis commented out. Consider removing it or uncommenting the acting include if needed.- There's a double blank line at line 20 (cosmetic).
code/leaderboard_launcher/launch/ros_bridge.dev.xml (1)
1-43: LGTM!The launch file properly orchestrates the CARLA bridge, object spawning, agent, and test-route components with consistent argument propagation. The use of environment variable for
CARLA_SIM_HOSTand sensible defaults for development (Town12, 100s timeout) are appropriate.code/mapping/mapping_common/map.py (1)
34-35: LGTM!The constant is appropriately defined at module level.
code/planning/planning/behavior_agent/behavior_tree.py (2)
8-8: LGTM!The
Boolimport is correctly added for the emergency publisher.
103-109: LGTM!The
emergency_pubis correctly passed to bothintersection.Waitandintersection.Enterbehaviors, enabling emergency signaling when cross-traffic is detected.code/planning/planning/behavior_agent/behaviors/intersection.py (10)
2-2: LGTM!The
mathimport is used formath.hypotin the cross-traffic check functions.
11-11: LGTM!The
Boolimport is correctly added for emergency signaling.
116-152: LGTM!The
check_cross_trafficfunction correctly:
- Builds a detection mask positioned ahead of the hero
- Uses
math.hypotfor efficient speed calculation- Returns a consistent
(clear, mask)tupleMinor: The docstring is in German. Consider translating to English for consistency.
155-195: LGTM!The
check_priority_cross_trafficfunction follows the same pattern ascheck_cross_trafficwith appropriate threshold differentiation for fast/emergency vehicles.Minor: Same note about German docstring.
519-531: LGTM!The
Waitconstructor correctly accepts and stores theemergency_pubparameter.
664-666: Verify: Significant reduction in stopline wait time.The wait time at unknown traffic light state was reduced from 2.0 seconds to 0.5 seconds. This is a 75% reduction that could affect safety at intersections without detected traffic lights.
Ensure this change has been tested in scenarios where traffic light detection is unreliable or absent.
755-765: LGTM!The
Enterconstructor correctly accepts and stores theemergency_pubparameter, matching theWaitbehavior signature.
770-781: LGTM!The
initialisemethod correctly stores the waypoint and intersection type for use inupdate().
802-844: LGTM with note on consistency.The priority cross-traffic check in
Enteris implemented correctly and mirrors the pattern inWait. The emergency signaling logic (publishTruewhen ego speed exceeds threshold,Falseotherwise) is sound.Note: Unlike
Wait, this check runs for all intersection types including LEFT turns. Verify this inconsistency withWait(lines 573-611) is intentional.
573-611: Priority cross-traffic check is inconsistently applied between Wait and Enter behaviors.The Wait behavior skips the priority cross-traffic check when
self.intersection_type == CarlaRoute.LEFT(line 573), while the Enter behavior performs this check unconditionally (line 809). This creates an inconsistency: fast cross-traffic (e.g., emergency vehicles) approaching a left-turning vehicle will not trigger an emergency stop in the Wait state.The docstring at line 558 claims "oncoming traffic is checked before proceeding" for left turns, but the code at line 573 explicitly skips
check_priority_cross_traffic()for LEFT turns. This suggests either the docstring is outdated or a separate oncoming traffic check exists that was not implemented.If the intent is to handle left turns differently, this should be clarified and the docstring updated accordingly. If it's unintentional, the condition at line 573 should be removed to match the Enter behavior.
✏️ Tip: You can disable this entire section by setting review_details to false in your review settings.
lukasmarz-dev
left a comment
lukasmarz-dev
left a comment
There was a problem hiding this comment.
The implementation of check_priority_cross_traffic is useful and significantly improves behavior at intersections, especially with fast cross traffic (e.g., emergency vehicles). The logic is easy to understand and is neatly integrated into the update method.
However, it is noticeable that has_cross_traffic and check_cross_traffic are currently not being used. Either the connection is still missing here, or the code could be removed to avoid confusion. Overall, however, this is a good and comprehensible extension.
… been removed because they are no longer used.
There was a problem hiding this comment.
Actionable comments posted: 3
🧹 Nitpick comments (1)
code/planning/planning/behavior_agent/behaviors/intersection.py (1)
485-491: Extract shared priority-cross-traffic handling to one helper.
Wait.update()andEnter.update()now carry near-duplicate stop/emergency logic. Centralizing this into a single helper would reduce drift and prevent branch inconsistencies.Also applies to: 533-571, 720-725, 762-805
🤖 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 485 - 491, The Wait.update() and Enter.update() methods contain duplicated stop/emergency handling logic; extract that shared logic into a single helper method (e.g., _handle_stop_and_emergency or handle_priority_traffic) on the same class so both updates call it. The helper should accept self and use the same members used in the duplicated blocks (stop_client, emergency_pub, curr_behavior_pub, clock and any local state that decides emergency vs stop), perform identical checks/requests and publishing, and return any status/value the update methods expect so their behavior remains unchanged; then replace the duplicated code in Wait.update, Enter.update (and the similar blocks at the other noted ranges) with a single helper call to avoid drift and keep branches consistent.
🤖 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/planning/planning/behavior_agent/behaviors/intersection.py`:
- Around line 552-562: The code sets self.emergency_pub.publish(Bool(data=True))
when ego_speed > SELF_EMERGENCY_THRESHOLD but does not clear the topic in the
corresponding else branch (the branches that create the "no emergency brake"
reason); update both occurrences (the block using SELF_EMERGENCY_THRESHOLD and
the similar block later) to publish the clear state by calling
self.emergency_pub.publish(Bool(data=False)) in the non-emergency branch so
downstream consumers cannot remain in a stale emergency state, leaving the
existing reason strings unchanged.
- Around line 116-156: check_priority_cross_traffic currently treats any fast
actor in the mask as blocking; restrict this by checking trajectory conflict
before returning False. In the check_priority_cross_traffic function, after
computing speed from motion.linear_motion, compute the actor's velocity vector
in hero/world frame and the approach/crossing direction (e.g., vector from actor
position to hero or the hero's forward approach vector derived from
hero.get_front_x()/hero.transform), then test whether the actor's velocity has a
component towards the conflict path (dot product below/above a small threshold)
or would cross the hero's lane (angle between vectors within a configurable
angular threshold). Only if both speed > PRIORITY_SPEED_THRESHOLD and the
direction/conflict test indicates a true crossing/toward-ego trajectory should
you return False, otherwise continue and ultimately return True; keep existing
mask return semantics.
- Around line 624-626: The dwell timer for unknown-light must start when the
agent actually reaches the stopline, so reset/start self.stop_time when the
measured distance to the stopline is within WAIT_TARGET_DISTANCE (i.e., when
dist <= WAIT_TARGET_DISTANCE) and only then check the elapsed time to set
self.over_stop_line; update the logic around the existing check that sets
self.over_stop_line (referencing self.stop_time and self.over_stop_line in
intersection.py) to assign self.stop_time = self.clock.now() on first arrival at
the stopline (or when dist transitions to <= WAIT_TARGET_DISTANCE) and use that
timestamp for the 0.5s wait comparison.
---
Nitpick comments:
In `@code/planning/planning/behavior_agent/behaviors/intersection.py`:
- Around line 485-491: The Wait.update() and Enter.update() methods contain
duplicated stop/emergency handling logic; extract that shared logic into a
single helper method (e.g., _handle_stop_and_emergency or
handle_priority_traffic) on the same class so both updates call it. The helper
should accept self and use the same members used in the duplicated blocks
(stop_client, emergency_pub, curr_behavior_pub, clock and any local state that
decides emergency vs stop), perform identical checks/requests and publishing,
and return any status/value the update methods expect so their behavior remains
unchanged; then replace the duplicated code in Wait.update, Enter.update (and
the similar blocks at the other noted ranges) with a single helper call to avoid
drift and keep branches consistent.
ℹ️ Review info
Configuration used: defaults
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
code/mapping/mapping_common/map.pycode/planning/planning/behavior_agent/behaviors/intersection.py
🚧 Files skipped from review as they are similar to previous changes (1)
- code/mapping/mapping_common/map.py
simoooong
left a comment
simoooong
left a comment
There was a problem hiding this comment.
Lukas already reviewed, just approval for fixed comments
3 participants
Description
The position of radar1 has been changed to the front and middle of the car.
Cross traffic can be detected by the radar data and at every intersection possible cross traffic is checked.
Fixes # (issue)
#844 Improve lateral object detection
#872 Position change of the RADAR1
#840 Watch out for cross traffic at stop signs
Type of change
Please delete options that are not relevant.
Most important changes
Checklist:
Summary by CodeRabbit
New Features
Improvements