726 overtake lane free motion aware #729
Conversation
…-to-fix-overtaking-crashes' into 726-overtake-lane-free-motion-aware # Conflicts: # code/mapping/src/mapping_data_integration.py
…o 726-overtake-lane-free-motion-aware # Conflicts: # code/mapping/config/mapping.cfg # code/mapping/src/mapping_data_integration.py
…take-lane-free-motion-aware # Conflicts: # code/mapping/src/mapping_data_integration.py
WalkthroughThe changes update configuration parameters and control flows across multiple modules. In the mapping configuration file, the polygon simplification tolerance default and range are modified. The mapping common module now includes an enhanced Entity class with clearer documentation and a new delta velocity calculation method. The map checking logic is restructured by adding a new lane state, a motion awareness parameter, and altered control flow to decide lane availability. Additionally, overtaking behavior parameters in the planning module are adjusted. Changes
Sequence Diagram(s)sequenceDiagram
participant Caller as Client
participant MT as MapTree
participant LC as LaneChecker
Caller->>MT: is_lane_free(..., motion_aware=True)
Note over MT: Initialize lane_state = TO_BE_CHECKED, lane_box = None
alt Check method is "lanemarking" or "fallback"
MT->>LC: is_lane_free_lanemarking()
LC-->>MT: Return updated lane_state
else Check method is "rectangle" or error state
MT->>LC: is_lane_free_rectangle()
LC-->>MT: Return updated lane_state
end
alt lane_box is None or lane_state != TO_BE_CHECKED
MT-->>Caller: Return SHAPE_ERR
else
alt motion_aware is True
MT->>MT: Filter colliding entities by hero velocity
end
MT-->>Caller: Return FREE or BLOCKED
end
Suggested reviewers
Poem
✨ Finishing Touches
Thank you for using CodeRabbit. We offer it for free to the OSS community and would appreciate your support in helping us grow. If you find it useful, would you consider giving us a shout-out on your favorite social media? 🪧 TipsChatThere are 3 ways to chat with CodeRabbit:
Note: Be mindful of the bot's finite context window. It's strongly recommended to break down tasks such as reading entire modules into smaller chunks. For a focused discussion, use review comments to chat about specific files and their changes, instead of using the PR comments. CodeRabbit Commands (Invoked using PR comments)
Other keywords and placeholders
CodeRabbit Configuration File (
|
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 4
🧹 Nitpick comments (39)
code/control/config/pure_pursuit_config.yaml (1)
4-4: Parameter update to improve control behaviorIncreasing the
k_pubvalue from 0.8 to 1.0 appears to be part of a coordinated effort to refine control parameters, likely to enhance the pure pursuit algorithm's performance for the new motion-aware functionality.Add newline at end of file
The file is missing a newline character at the end, which is a minor style issue flagged by the static analysis tool.
k_lad: 0.85 min_la_distance: 3 max_la_distance: 25 -k_pub: 1.0 +k_pub: 1.0 +🧰 Tools
🪛 YAMLlint (1.35.1)
[error] 4-4: no new line character at the end of file
(new-line-at-end-of-file)
code/planning/srv/OvertakeStatus.srv (1)
8-9: Consider clarifying the type of offsetThe comment provides some context, but it could be more specific about what kind of offset is being represented (e.g., lateral offset from lane center, longitudinal offset from target vehicle).
-# Offset of the current overtake +# Lateral offset from the original lane for the current overtake maneuver (in meters)code/planning/srv/SpeedAlteration.srv (1)
12-12: Consider adding a comment for the success fieldWhile the purpose of the
successfield is straightforward, adding a comment would maintain consistency with the documentation of other fields.-bool success +# Indicates whether the speed alteration request was successfully processed +bool successcode/planning/srv/StartOvertake.srv (2)
3-3: Consider adding bounds for the offset parameterWhile the parameter description is clear, consider adding documentation about the valid range for the
offsetvalue to prevent extreme trajectory shifts that could be unsafe.
14-17: Consider adding default values for transition lengthsThe transition lengths are important for smooth overtaking. Consider adding comments about recommended default values or valid ranges to guide implementation.
code/planning/src/behavior_agent/behaviors/leave_parking_space.py (1)
190-191: Consider preserving logging in terminate method.The terminate method has been simplified to just
pass, removing previous logging functionality. While this simplifies the code, consider whether logging termination state might be useful for debugging.- def terminate(self, new_status): - pass + def terminate(self, new_status): + """Called when the behavior terminates.""" + rospy.logdebug(f"{self.name} terminated with status {new_status}")code/planning/src/behavior_agent/behaviors/stop_mark_service_utils.py (1)
95-97: Consider adding type checking for transform.The code applies a transform to the stop mark without verifying that
hero_transformis of the expected type. Consider adding a check to ensure it's a valid transform object.if not is_global: + if not isinstance(hero_transform, Transform2D): + rospy.logerr(f"Invalid hero_transform type: {type(hero_transform)}") + return None e.transform = hero_transform * e.transformcode/planning/src/behavior_agent/behaviors/topics2blackboard.py (1)
90-93: Clarify usage of “acc_velocity” versus other velocity topics.
Introducing the/paf/{role_name}/acc_velocitytopic is fine, but ensure consistency with existing velocity topics (like/paf/{role_name}/target_velocity) to avoid confusion in the codebase.code/planning/src/global_planner/global_planner_node.py (4)
46-48: Provide context or configuration for magic number 5.
Currently,last_agent_positions_count_target = 5is a hard-coded value. Consider adding a comment or a config parameter to clarify or allow customization.self.last_agent_positions_count_target = 5 + # TODO: Document or externalize this value to configuration if needed
217-217: Consider log level.
If publishing a trajectory is not a cause for concern or warning, consider usingloginfoinstead oflogwarn.- self.logwarn("PrePlanner: published trajectory") + self.loginfo("PrePlanner: published trajectory")
263-268: Handling insufficient position history.
This pattern is understandable, but document that new positions are ignored until enough samples accumulate. This can help future maintainers.+ # Explanation: we need at least N positions for stabilization logic before storing agent_pos
274-280: Use a named constant or config for distance threshold.
The value 0.5 is repeated; making it a named constant can improve clarity.- if pos_point.distance_to(agent_point) > 0.5: + STABILIZATION_THRESHOLD = 0.5 + if pos_point.distance_to(agent_point) > STABILIZATION_THRESHOLD:code/mapping/ext_modules/mapping_common/map.py (1)
560-570: Filtering colliding entities by motion.
This approach is aligned with ignoring vehicles moving away. The threshold 0.5 is repeated—consider a named constant for clarity.code/mapping/src/mapping_data_integration.py (3)
81-85: Service for updating stop marks
Consider adding safeguards for unexpected requests so the node can handle malformed data more gracefully.
169-189:update_stopmarks_callbackmethod
Uses a straightforward approach to replace and append stop marks. If the node becomes multi-threaded, consider thread-safety (e.g., locks) aroundself.stop_marks.
514-528: Stop marks integration in the map
Good approach to transform global stop marks into the hero’s coordinate space. Ensure no concurrency issues withself.stop_marks.code/mapping/ext_modules/mapping_common/entity.py (2)
483-486: Clarify the purpose and usage constraints.The docstring states that
get_delta_forward_velocity_ofonly applies if the other entity is in front. It instructs users to invert the result if the other entity is behind. Consider clarifying whether an exception, a zero value, or a specific error code would be more appropriate instead of requiring manual inversion.
540-549: Misleading docstring referencing 'width'.The docstring mentions “Returns the local x length from the center to the front of the entity” but concludes with “width.” Consider updating the docstring to avoid confusion between the terms "length" and "width."
- """Returns the local x length from the center to the front of the entity - - Returns: - float: width - """ + """Returns the local x distance from the center to the front of the entity + + Returns: + float: front_x + """code/planning/src/behavior_agent/behaviors/overtake.py (11)
18-18: OvertakeStatusResponse usage.Ensure that you handle all status codes from
OvertakeStatusResponseor gracefully fallback to a default if new statuses are introduced in the future.
23-30: Explicit grouping of utility functions.Organizing your overtake-related service utilities here clarifies the code’s purpose. A possible improvement is to unify or reduce the import lines if you foresee further expansions.
42-42: Global ID for overtake space marks.Defining a constant
OVERTAKE_SPACE_STOPMARKS_IDis neat for clarity, but consider namespacing if you have multiple specialized stopmarks in the future.
64-72: Functionunset_space_stop_markfor clearing previously placed mark.Pairs nicely with
set_space_stop_mark, ensuring the map remains clean of stale stop marks. Consider adding logs or debug messages here for easier troubleshooting.
125-126: Adjustable front mask size & trajectory length.These parameters are critical to accurate collision detection. Ensure they align with the maximum length that your local planner can handle.
128-128: Empty collision mask scenario.Properly returning
FAILUREis a good approach. You might also want to log relevant debug info for diagnosing why no valid path was built.
340-345: Lane check for oncoming traffic.
is_lane_freewith fallback method is a robust solution. Revisit if you notice corner cases in intersections.
354-356: Start overtaking with a transition length.Adjusting
start_transition_lengthto 5.0 might be scenario-specific. Consider making it configurable.
476-478: Clarity in checking obstacle speed.The comment line about standing obstacles is good. Possibly reorder code to keep condition checks near relevant logic blocks.
544-547: Requesting overtake status inline.This pattern is repeated in multiple states. If usage expands, consider a helper method to reduce repetition.
548-556: Immediate success on OVERTAKING.Well-structured approach. Evaluate if a short transitional state is needed before we finalize success.
code/planning/src/local_planner/ACC.py (10)
2-2: Unnecessary inline comment aboutimport math.If no special disclaimers or usage notes are needed, the comment can be omitted to maintain cleanliness.
37-37: ACC marker color changed.Good for better visual differentiation from other debug markers.
74-74: Local trajectory subscription.No concerns. But if the local planner updates at a high frequency, verify overall system performance.
117-118:__set_speed_limitoverride.Straightforward. Possibly log or debug-print the new limit to visualize changes in real time.
178-183: Filtering colliders and stop marks in one pass.This helps unify logic around limiting speed. Ensure no other flags need filtering (e.g., ignoring certain ephemeral objects).
187-188: Behavior check for smaller mask size.This dynamic scaling is a good idea, but confirm that 3.5 is enough buffer for edge cases.
240-242: Applying external speed limit.Maintains internal consistency. Keep an eye on potential confusion if
speed_limit < external_speed_limit.
243-257: Curve-based speed logic.Minimizing speed to handle potential curves is prudent. This large block merges dynamic constraints effectively. Provide thorough tests for scenario transitions.
295-297: PI controller initialization for distance and speed.No immediate issues, but consider adding constraints or saturations if input values are unexpected.
298-302: Extracting gains from ROS params.This is fine. Consider caching them or storing in an object if these are re-used frequently.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (51)
code/acting/src/acting/passthrough.py(1 hunks)code/agent/config/rviz_config.rviz(8 hunks)code/control/config/Controller.cfg(1 hunks)code/control/config/controller_config.yaml(1 hunks)code/control/config/pure_pursuit_config.yaml(1 hunks)code/control/launch/control.launch(1 hunks)code/control/src/debug_wrapper.py(1 hunks)code/control/src/pure_pursuit_controller.py(5 hunks)code/control/src/stanley_controller.py(2 hunks)code/control/src/vehicle_controller.py(2 hunks)code/control/src/velocity_controller.py(1 hunks)code/mapping/CMakeLists.txt(3 hunks)code/mapping/config/mapping.cfg(2 hunks)code/mapping/ext_modules/mapping_common/entity.py(7 hunks)code/mapping/ext_modules/mapping_common/hero.py(1 hunks)code/mapping/ext_modules/mapping_common/map.py(8 hunks)code/mapping/ext_modules/mapping_common/markers.py(3 hunks)code/mapping/ext_modules/mapping_common/mask.py(8 hunks)code/mapping/ext_modules/mapping_common/transform.py(1 hunks)code/mapping/launch/mapping.launch(1 hunks)code/mapping/msg/Entity.msg(1 hunks)code/mapping/msg/TypeStopMark.msg(1 hunks)code/mapping/src/mapping_data_integration.py(8 hunks)code/mapping/srv/UpdateStopMarks.srv(1 hunks)code/mapping/tests/mapping_common/test_entity.py(1 hunks)code/mapping/tests/mapping_common/test_shapely.py(0 hunks)code/perception/launch/perception.launch(1 hunks)code/perception/src/radar_node.py(7 hunks)code/planning/CMakeLists.txt(2 hunks)code/planning/config/ACC.cfg(1 hunks)code/planning/config/acc_config.yaml(1 hunks)code/planning/launch/planning.launch(0 hunks)code/planning/msg/NavigationPoint.msg(0 hunks)code/planning/msg/Trajectory.msg(0 hunks)code/planning/src/behavior_agent/behavior_tree.py(2 hunks)code/planning/src/behavior_agent/behaviors/behavior_speed.py(1 hunks)code/planning/src/behavior_agent/behaviors/intersection.py(7 hunks)code/planning/src/behavior_agent/behaviors/leave_parking_space.py(4 hunks)code/planning/src/behavior_agent/behaviors/overtake.py(12 hunks)code/planning/src/behavior_agent/behaviors/overtake_service_utils.py(1 hunks)code/planning/src/behavior_agent/behaviors/speed_alteration.py(1 hunks)code/planning/src/behavior_agent/behaviors/stop_mark_service_utils.py(1 hunks)code/planning/src/behavior_agent/behaviors/topics2blackboard.py(3 hunks)code/planning/src/global_planner/global_planner_node.py(9 hunks)code/planning/src/local_planner/ACC.py(9 hunks)code/planning/src/local_planner/collision_check.py(0 hunks)code/planning/src/local_planner/motion_planning.py(4 hunks)code/planning/srv/EndOvertake.srv(1 hunks)code/planning/srv/OvertakeStatus.srv(1 hunks)code/planning/srv/SpeedAlteration.srv(1 hunks)code/planning/srv/StartOvertake.srv(1 hunks)
💤 Files with no reviewable changes (5)
- code/planning/msg/NavigationPoint.msg
- code/mapping/tests/mapping_common/test_shapely.py
- code/planning/launch/planning.launch
- code/planning/src/local_planner/collision_check.py
- code/planning/msg/Trajectory.msg
✅ Files skipped from review due to trivial changes (2)
- code/control/src/debug_wrapper.py
- code/planning/src/behavior_agent/behaviors/behavior_speed.py
🧰 Additional context used
🪛 YAMLlint (1.35.1)
code/control/config/pure_pursuit_config.yaml
[error] 4-4: no new line character at the end of file
(new-line-at-end-of-file)
🔇 Additional comments (224)
code/mapping/msg/TypeStopMark.msg (1)
1-2: Clear addition of reason field to TypeStopMark messageThis addition of a
reasonfield to theTypeStopMarkmessage enables more descriptive stop mark entities by allowing the system to include textual explanations or rationales for stop marks. This enhancement aligns well with the PR objectives for motion-aware functionality.code/mapping/msg/Entity.msg (1)
15-16: Good integration of TypeStopMark into Entity messageAdding the
TypeStopMarkfield to theEntitymessage is a logical extension that complements the introduction of theTypeStopMarkmessage type. This change allows entities to have associated stop mark information with reasons, which supports the motion-aware functionality targeted in this PR, particularly for overtaking scenarios.code/control/launch/control.launch (1)
14-14:❓ Verification inconclusive
Timing adjustment for vehicle controller
Increasing the
loop_sleep_timefrom 0.1 to 0.2 seconds reduces the frequency at which the vehicle controller interacts with the Carla simulator. This change aligns with the hotfix described in the comments (lines 5-13) to address timing issues between the controller and simulator.The modification appears to be part of a coordinated update across multiple configuration files to ensure consistent timing parameters throughout the system.
Given that this is labeled as a "hotfix" in the comments, verify whether there are plans to implement a more permanent solution as suggested in the comments:
🏁 Script executed:
#!/bin/bash # Search for related TODOs or issues about the timing hotfix rg -i "carla.*timing|timing.*hotfix|timing.*fix|loop_sleep_time.*fix" -g "!*.launch"Length of output: 86
Review Comment Update: Timing Adjustment Hotfix
The update increasing the
loop_sleep_timefrom 0.1 to 0.2 seconds aligns with the hotfix described in the comments (lines 5–13) of thecode/control/launch/control.launchfile. A grep search outside of the launch files for any additional related TODOs or references to a permanent solution for this timing issue returned no results. Please manually verify whether there are any plans to implement a more permanent fix for the timing adjustments, or confirm that this hotfix is intended as the final update.code/mapping/ext_modules/mapping_common/transform.py (1)
47-49: Good addition - clear and efficient implementation!This
distance_tomethod is a useful utility that concisely calculates the distance between two points by leveraging the existingvector_toandlengthmethods. The implementation is correct, properly typed, and follows the class's established patterns.code/control/config/controller_config.yaml (2)
4-4: Verify the impact of disabling the Stanley controllerThe Stanley controller is now being disabled by setting
stanley_offtoTrue. This change appears to be part of the motion-aware feature mentioned in the PR objectives, but it might have significant implications for vehicle control.Can you confirm that disabling the Stanley controller is intentional and has been tested with the new motion-aware lane free functionality?
6-6: Verify the implications of the new loop sleep timeAdding a
loop_sleep_timeof 0.2 seconds could affect the responsiveness of the control system. According to the summary, this aligns with similar changes in other configuration files.Has this value been tested to ensure it provides the right balance between system responsiveness and computational efficiency?
code/planning/srv/OvertakeStatus.srv (1)
4-7: Well-defined status constants for overtakingThe status constants are clearly defined with appropriate values for representing different overtaking states.
code/planning/srv/SpeedAlteration.srv (1)
1-8: Well-structured speed alteration service with clear documentationThe service provides a flexible approach to speed control with separate fields for values and activation status. The comments clearly explain the purpose of each field.
code/planning/srv/EndOvertake.srv (1)
1-12: Well-structured service definition with clear purposeThe service definition for ending an overtaking maneuver is clear and well-documented. The parameters are logically organized with the request fields providing flexibility for either immediate termination or position-based ending of the maneuver.
code/control/src/stanley_controller.py (5)
7-7: Good addition of rospy importAdding the rospy import is necessary for using the throttled logging functions.
90-94: Improved logging with throttlingReplacing
self.logwarnwithrospy.logwarn_throttleis a good practice to prevent log flooding when conditions persist for extended periods.
97-102: Consistent throttling implementationThe throttle duration of 1.0 second is consistent with the other throttled warnings in this function.
106-111: Consistent throttling implementationThis throttled warning follows the same pattern as the others in this function.
115-120: Consistent throttling implementationThis throttled warning follows the same pattern as the others in this function.
code/control/config/Controller.cfg (2)
12-12: Reasonable constraint for manual throttleLimiting the manual throttle range to [-1.0, 1.0] ensures values stay within the standard expected range for vehicle control inputs.
15-15: Appropriate sleep time configurationAdding a configurable loop sleep time parameter with a default of 0.2s and reasonable min/max boundaries gives proper control over the controller's update frequency.
code/mapping/tests/mapping_common/test_entity.py (1)
88-99: Good test coverage for StopMark entityThe added test follows the same pattern as other entity conversion tests, ensuring proper serialization and deserialization of the StopMark entity. This is important for maintaining the integrity of stop mark data through ROS message conversion.
code/control/src/velocity_controller.py (1)
131-131: Increased velocity threshold for standstill detection.The threshold for considering the vehicle at a standstill has been increased from 0.01 to 0.1. This change means the vehicle will now remain stationary for a broader range of low target velocities (0.01-0.1), which improves stability in stop scenarios.
This modification aligns with the overtaking functionality mentioned in the PR objectives, as it would help ensure more deterministic behavior when the vehicle needs to come to a complete stop.
code/control/src/vehicle_controller.py (2)
28-28: Updated documentation to reflect new loop_sleep_time value.The docstring has been properly updated to match the new default loop_sleep_time value of 0.2 seconds.
176-176: Increased loop_sleep_time default value.The default value for loop_sleep_time has been increased from 0.1 to 0.2 seconds, which will reduce the frequency at which control commands are sent to the simulator.
This change should reduce CPU usage and provide more time for processing between control frames, potentially improving system stability on slower hardware. It's consistent with other configuration changes mentioned in the related files.
code/perception/launch/perception.launch (2)
95-95: Enabled radar clustering.Clustering for radar data processing has been enabled by changing this parameter from False to True.
This change supports the motion-aware flag functionality mentioned in the PR objectives by allowing the system to identify and track discrete objects, which is essential for determining if objects are moving away from the hero vehicle in overtaking scenarios.
88-88:❓ Verification inconclusive
Reduced DBSCAN minimum samples requirement.
The minimum number of samples required for forming a cluster in the DBSCAN algorithm has been reduced from 3 to 1, which will allow for the detection of smaller object clusters.
This change could affect the radar's ability to distinguish between actual objects and noise. Please ensure that reducing this value doesn't lead to false positive detections.
🏁 Script executed:
#!/bin/bash # Description: Find and review DBSCAN implementation to understand potential impact rg -A 10 "dbscan_samples" --type pythonLength of output: 70
🏁 Script executed:
#!/bin/bash # Re-run search for DBSCAN-related parameters across the entire codebase without file type restrictions. rg -A 10 "dbscan_samples"Length of output: 3501
Action: Verify Impact of Reduced DBSCAN Minimum Samples Parameter
The launch file now sets
dbscan_samplesto 1 (overriding the default of 3 inradar_node.py). This change allows the clustering algorithm to detect smaller clusters, but it might also increase false positive detections due to noise being misinterpreted as valid clusters.
- Confirm that the DBSCAN implementation in
code/perception/src/radar_node.pycorrectly handles the lower threshold without compromising target detection.- Review testing or simulation results to ensure that lowering
dbscan_samplesdoes not lead to an undesirable increase in noise clusters.code/mapping/srv/UpdateStopMarks.srv (1)
1-12: New service for stop mark management.This service provides a structured way to update stop marks with proper identification, coordinate information, and management controls.
The service definition includes:
idto uniquely identify a set of stop marksmarksarray containing Entity objects with global coordinatesdelete_all_othersflag to control broader stop mark management- Response fields for operation status and feedback
This addition supports the motion-aware functionality mentioned in the PR objectives, likely enabling dynamic updates to stop marks based on the movement of detected objects during overtaking scenarios.
code/mapping/ext_modules/mapping_common/hero.py (2)
1-12: Well-documented hero car constants.The hero car dimensions and documentation are clear and appropriate. Nice inclusion of the reference to the actual vehicle specifications.
15-36: Clean implementation of hero entity creation.The function correctly constructs a Car entity with appropriate parameters. I appreciate the comprehensive docstring and logical structuring.
code/planning/src/behavior_agent/behavior_tree.py (2)
25-25: Appropriate module import.The speed_alteration module is properly imported and aligns with the PR objective of adding motion-aware functionality.
125-125:✅ Verification successful
Verify integration of new speed alteration behaviors.
The SpeedAlterationSetupBehavior and SpeedAlterationRequestBehavior appear to be properly integrated into the behavior tree. These behaviors will support the motion-aware functionality mentioned in the PR objectives.
Also applies to: 129-129
🏁 Script executed:
#!/bin/bash # Description: Check that the new behaviors are properly implemented in the speed_alteration module # Search for the implementation of SpeedAlterationSetupBehavior and SpeedAlterationRequestBehavior rg -A 5 "class SpeedAlterationSetupBehavior|class SpeedAlterationRequestBehavior" code/planning/src/behavior_agent/behaviors/speed_alteration.pyLength of output: 592
Integration of Speed Alteration Behaviors Confirmed
The new implementations of
SpeedAlterationSetupBehaviorandSpeedAlterationRequestBehaviorhave been verified in thecode/planning/src/behavior_agent/behaviors/speed_alteration.pyfile. Both behaviors are correctly defined and integrated into the behavior tree (lines 125 and 129), supporting the motion-aware functionality outlined in the PR objectives.
- Verified that both classes exist with proper implementations.
- Confirmed that behavior tree references (
speed_alteration.SpeedAlterationSetupBehavior()) are aligned with these implementations.code/mapping/launch/mapping.launch (1)
5-5:❓ Verification inconclusive
Radar clustering now enabled by default.
This change enables radar clustering by default, which aligns with the motion-aware functionality described in the PR objectives. Verify that corresponding radar processing code supports this change.
🏁 Script executed:
#!/bin/bash # Description: Check for related radar clustering parameters in other launch files # Look for related radar clustering parameters in other launch files rg "enable_radar_cluster|enable_clustering" --type=launchLength of output: 90
Action Required: Verify Radar Clustering Parameter Usage
The initial script encountered an error due to the unrecognized file type "launch." Please verify the radar clustering parameters across launch files manually or by re-running a corrected search. For example, you can run the following shell script to look through all files with a “.launch” extension:
#!/bin/bash # Description: Check for related radar clustering parameters in .launch files rg "enable_radar_cluster|enable_clustering" -g '*.launch'Ensure that no other launch file unintentionally overrides or conflicts with the default radar clustering setting established in
code/mapping/launch/mapping.launch.code/planning/CMakeLists.txt (3)
68-71: New service definitions for overtaking and speed alteration.The addition of StartOvertake.srv, EndOvertake.srv, OvertakeStatus.srv, and SpeedAlteration.srv services aligns well with the PR objective of introducing motion-aware functionality for lane free operation, particularly in overtaking scenarios.
82-87: Clean formatting of generate_messages section.The reformatting improves readability while maintaining the same dependencies.
60-62:❓ Verification inconclusive
Message files section commented out.
The NavigationPoint.msg and Trajectory.msg message files have been removed from the build configuration. Ensure these are no longer used elsewhere in the codebase or have been replaced by alternative implementations.
🏁 Script executed:
#!/bin/bash # Description: Verify that the removed message types are not referenced elsewhere # Check for references to the removed message types rg "NavigationPoint\.msg|Trajectory\.msg" --type-not=cmakeLength of output: 60
Attention: Verify Removed Message References
The removal of the NavigationPoint.msg and Trajectory.msg message files from the build configuration in
code/planning/CMakeLists.txt(lines 60–62) appears correct. The initial search usingrg "NavigationPoint\.msg|Trajectory\.msg" --type-not=cmakereturned no references in non-CMake files, suggesting that these message types are not used elsewhere. However, since no output was produced (which might be due to limitations of the search command), please manually verify the repository to ensure that no inadvertent references to these files remain.
- Action Required: Manually confirm that neither "NavigationPoint.msg" nor "Trajectory.msg" is referenced in any other parts of the codebase.
code/mapping/config/mapping.cfg (2)
16-16: Changing default forenable_raw_lidar_pointsfrom True to FalseThis change disables raw lidar points input to mapping by default. Verify that this won't impact existing functionality that might depend on raw lidar data.
19-19: New stop marks feature enabledThis parameter enables the new stop marks feature provided by the UpdateStopMarks service. Ensure this is properly tested with the motion-aware functionality mentioned in the PR objectives.
code/mapping/CMakeLists.txt (3)
41-41: New message type for stop marksAddition of
TypeStopMark.msgaligns with the stop marks feature mentioned in the mapping configuration.
45-49: New service for updating stop marksEnabling service generation for
UpdateStopMarks.srvsupports the stop marks feature mentioned in the PR objectives. This is a good implementation of the required infrastructure for motion-aware behavior.
116-116: New hero module dependenciesThe addition of
hero.candhero.htmlas dependencies suggests new functionality related to the main vehicle (hero). This aligns with the motion-aware behavior mentioned in the PR objectives.Also applies to: 125-125
code/planning/srv/StartOvertake.srv (1)
1-17: Well-structured overtake service definitionThe service definition is comprehensive with clearly documented parameters for controlling the overtake maneuver. It provides good flexibility with options for specifying start/end positions and transition lengths.
code/planning/config/acc_config.yaml (3)
6-7: New parameters for hard approach handlingThese parameters will help control the vehicle's behavior when approaching obstacles at high speeds, which is valuable for the overtaking scenarios mentioned in the PR.
9-9: Modified acceleration factorIncreasing the
acceleration_factorto 1.0 may result in more aggressive acceleration. Ensure this change has been tested thoroughly in various scenarios, especially during overtaking maneuvers.
11-15: New curve handling parametersThese parameters provide detailed control over the vehicle's speed in curves of different radii. This is excellent for safely implementing the overtaking functionality mentioned in the PR objectives.
code/planning/src/behavior_agent/behaviors/intersection.py (9)
11-14: Well-structured import organization.The addition of the stop mark service utilities is well organized, using parentheses for multi-line imports which improves readability.
38-38: Good constant naming.Using a clear and descriptive all-caps constant for the intersection stop marks identifier follows good Python naming conventions.
41-61: Well-implemented stop mark functions.The two utility functions
set_stop_markandunset_stop_markare well-designed:
- Clear type hints for the proxy parameter
- Consistent use of the
INTERSECTION_STOPMARKS_IDconstant- Appropriate descriptive reasons for each operation
- Clean separation of concerns
The
set_stop_markcreates a rectangle mask for traffic lights whileunset_stop_markclears it when needed.
183-183: Added stop mark proxy to Approach class.Good integration of the stop mark functionality with the existing
Approachclass.
271-272: Appropriate application of stop mark for red/yellow lights.The stop mark is correctly set when the traffic light is red, yellow, or absent, which aligns with the stopping behavior.
277-278: Properly clearing stop mark for green lights.The code correctly removes the stop mark when the traffic light turns green, allowing the vehicle to proceed.
372-372: Added stop mark proxy to Wait class.Consistent implementation across classes to ensure the stop mark functionality is available throughout the intersection behavior.
509-509: Added stop mark proxy to Enter class.Maintains consistency with the other classes in the intersection behavior tree.
543-543: Properly clearing stop mark when entering intersection.The code correctly removes any stop marks when actively entering the intersection, ensuring the vehicle doesn't receive contradictory signals.
code/mapping/ext_modules/mapping_common/markers.py (5)
2-2: Good import choice for sequence checking.Using the
collections.abc.Sequenceimport is a more robust approach than checking for specific sequence types like lists or tuples, allowing for greater flexibility.
27-28: Clear documentation of supported types.The updated documentation clearly outlines all the supported types for the
baseparameter, making it easier for users to understand what can be passed to the function.
56-62: Well-implemented LineString marker conversion.This implementation properly:
- Creates a LINE_STRIP marker with appropriate thickness
- Iterates through the LineString coordinates
- Converts each coordinate to a Point2 object
- Adds each point to the marker
66-70: Efficient Point2 to Circle marker conversion.The implementation intelligently:
- Creates a transform from the Point2 position
- Uses a Circle with appropriate radius
- Converts to marker with the transform
70-90: Comprehensive Sequence to Arrow marker implementation.The arrow handling is well implemented with:
- Proper length checking (2 points required)
- Type checking to ensure both elements are Point2 objects
- Clear error messages for invalid sequences
- Appropriate arrow styling with scale settings
- Vector calculation between the two points
This provides a useful visualization for directional data.
code/acting/src/acting/passthrough.py (2)
33-34: Updated topic subscription for target velocity.The topic name change from
target_velocitytoacc_velocityaligns with the PR objective to support motion-aware functionality. This change ensures the system uses the correct velocity information from the acceleration controller.
38-40: Updated trajectory topic names for consistency.Both the publisher and subscriber names have been updated to use
trajectory_localinstead of justtrajectory, which better indicates the nature of the trajectory data being used. This naming is more specific and reduces potential confusion with other trajectory types.code/planning/src/behavior_agent/behaviors/speed_alteration.py (6)
1-9: Well-structured imports and dependencies.The imports are cleanly organized, with standard library imports first, followed by ROS-specific imports and then the service-specific imports. The typing import enhances code readability and maintainability.
11-16: Clear constant definitions with documentation.The constants for blackboard identifiers are well-defined with descriptive docstrings. Using string constants instead of hardcoded strings throughout the code follows best practices.
19-23: Simple and effective speed override function.The
add_speed_overridefunction has a clear purpose and implementation. It directly sets the speed override value in the blackboard without unnecessary complexity.
25-32: Smart speed limit implementation.The
add_speed_limitfunction intelligently:
- Retrieves the current limit value (if any)
- Takes the minimum of the new and current limit (when applicable)
- Updates the blackboard with the most restrictive limit
This ensures safety by always respecting the most restrictive speed limit.
34-48: Clean setup behavior implementation.The
SpeedAlterationSetupBehaviorclass:
- Has clear documentation
- Initializes the blackboard connection in the constructor
- Resets both speed control values to None in its update method
- Always returns SUCCESS since its operation is deterministic
This provides a clean slate for speed controls at the beginning of execution.
50-83: Well-designed request behavior.The
SpeedAlterationRequestBehaviorclass effectively:
- Sets up the ROS service connection and waits for it to be available
- Reads both speed values from the blackboard
- Constructs the request with appropriate active flags based on the presence of values
- Sends the request to the service
This design properly separates the concerns of defining speed values and communicating them to the service.
code/planning/src/behavior_agent/behaviors/leave_parking_space.py (6)
1-5: Appropriate new imports added for enhanced functionality.The additions of shapely and stop mark service utils provide the necessary dependencies for the new lane-free motion-aware functionality.
Also applies to: 8-14
43-43: Good initialization pattern for stop mark functionality.Creating the stop proxy early during initialization and tracking the stop mark state with a typed boolean variable follows good design patterns.
Also applies to: 75-76
77-99: Well-structured method for adding initial stop marks.The
add_initial_stopmethod effectively:
- Creates a lane mask using the mapping functionality
- Validates the polygon shape before creating the stop mark
- Utilizes the stop_proxy to register the stop mark with an appropriate reason
This implementation aligns well with the PR objective of enhancing lane awareness for overtaking scenarios.
129-139: Motion-aware lane checking implemented correctly.The update method now properly checks for the necessary data (acc_speed, map, hero_transform, trajectory) before proceeding with lane status checks. The lane free filter with the "lanemarking" check method is appropriate for this use case.
Also applies to: 140-155
156-171: Good cleanup of stop marks when lane becomes free.When the lane state changes to FREE, the code correctly:
- Updates the behavior state
- Properly removes stop marks by passing an empty list
- Sets the finished flag
- Returns a clear debug status message
This ensures resources are properly cleaned up.
177-186: Improved error handling and debug message.The error message now provides comprehensive information about which specific data is missing, making debugging much easier.
code/planning/src/behavior_agent/behaviors/stop_mark_service_utils.py (5)
1-20: Well-designed service proxy creation function.The
create_stop_marks_proxyfunction properly:
- Creates the ROS service proxy
- Waits for the service to be available before returning
- Uses proper typing for the return value
This ensures that callers receive a ready-to-use service proxy.
22-55: Clear and well-documented function header.The documentation for
update_stop_marksis thorough and follows best practices:
- Describes the function's purpose
- Documents all parameters
- Provides defaults and explains behavior
- Documents the return value
- Includes helpful notes about usage
This makes the function much more maintainable and easier for other developers to use correctly.
56-62: Good defensive programming.The code properly:
- Handles the case of None input for marks
- Validates global transform availability before proceeding
- Returns early if transform is unavailable
This prevents potential runtime errors.
63-98: Comprehensive handling of different stop mark types.The function elegantly handles multiple input types:
- StopMark objects (preserving existing properties)
- Shape2D objects (creating new StopMark with the shape)
- shapely.Polygon objects (converting to Shape2D then creating StopMark)
The error handling for unsupported types is also appropriate.
100-106: Effective request building and service call.The function correctly:
- Converts entities to ROS messages
- Builds a properly formatted request
- Returns the response from the service call
This completes the service utility with all necessary functionality.
code/perception/src/radar_node.py (7)
359-365: Improved clustering functionality with simpler return value.The function now correctly returns just the cluster labels rather than the entire DBSCAN object, making it more focused and easier to use.
366-371: Proper point cloud creation with cluster labels.The code now uses the cluster labels correctly when creating the point cloud data, which is consistent with the return value change of the
cluster_datafunction.
403-404: Updated function signature for better readability.The function arguments now more clearly reflect what is being passed to the
generate_cluster_infofunction, in line with its updated implementation.
407-417: Added fallback handling when clustering is disabled.This adds crucial functionality to handle the non-clustering case, ensuring that entity data is still published even when clustering is disabled. This aligns with the motion-aware requirements in the PR.
452-452: Correctly mirroring velocity for RADAR1 sensor.The addition of mirroring the velocity component for the rear-facing radar sensor is essential for accurate motion detection, especially for overtaking scenarios mentioned in the PR objectives.
677-705: Simplified cluster_data function implementation.The function now has a cleaner interface that returns only what's needed (the labels), which is a good design improvement.
945-969: Updated cluster_info generation to use labels directly.The function now correctly uses
cluster_labelsdirectly instead of expecting a clustering object, making it consistent with the updatedcluster_datafunction.code/agent/config/rviz_config.rviz (4)
66-72: Enhanced visualization configuration for better debugging.The changes properly configure the visibility settings:
- Global Trajectory: set to false (to reduce visual clutter)
- Local Trajectory: set to true (for immediate path visualization)
- PurePursuit Debug: set to true (for controller debugging)
These changes support better visualization during testing of the motion-aware functionality.
165-169: Renamed path display to "Global Trajectory" with adjusted Z offset.Renaming from "Path" to "Global Trajectory" makes the purpose clearer, and increasing the Z offset from 703 to 705 helps prevent visual overlapping with other elements.
211-242: Added "Local Trajectory" and "PurePursuit Debug" visualizations.These new visualizations are important for debugging the motion-aware behavior, particularly for:
- Seeing the immediate planned path with Local Trajectory
- Visualizing controller behavior with PurePursuit Debug
This is aligned with the PR's objectives around improving overtaking capabilities.
271-289: Adjusted camera view settings for better perspective.The camera settings have been adjusted:
- Increased distance: from ~23 to ~30 (better field of view)
- Reduced pitch: from ~0.40 to ~0.19 (more overhead perspective)
- Adjusted yaw: from ~3.21 to ~3.14 (centered view)
These changes provide a better perspective for observing motion-aware behaviors.
code/planning/src/behavior_agent/behaviors/topics2blackboard.py (1)
95-97: Confirm necessity of “lane_change_distance” entry.
According to the summary,/paf/{role_name}/lane_change_distancewas renamed/removed in favor of/paf/{role_name}/acc_velocity, yet it remains here referencingLaneChange. Revisit whether this leftover entry is still needed or has become redundant.Likely an incorrect or invalid review comment.
code/planning/config/ACC.cfg (3)
15-16: Re-check default zero values for close-approach limits.
When an obstacle is near, having bothhard_approach_distanceandhard_approach_speedset to 0.0 by default may render the feature inactive. Confirm these defaults are intended.
18-18: Validate new maximum acceleration factor.
Raising the ceiling foracceleration_factorto 2.0 could lead to more aggressive accelerations. Ensure the system has been tested to handle higher acceleration without stability issues.
20-24: Add test coverage for new curve-handling parameters.
Parameters likecurve_line_angleandmin_curve_distanceexpand curve-handling logic. Verify they are fully documented and covered by unit or integration tests to prevent regressions.code/control/src/pure_pursuit_controller.py (10)
5-5: Transition to rospy import is consistent.
Importingrospyaligns with your usage of ROS-specific logging functions likerospy.logwarn_throttle.
12-20: Confirm mapping_common dependencies are installed.
The new imports frommapping_commonintroduce additional package requirements. Make sure that this dependency is included in your environment or documented.
29-31: Refined naming for marker constants.
DefiningMARKER_NAMESPACEandPP_CONTROLLER_MARKER_COLORclarifies marker usage. Values appear descriptive and consistent.
60-66: Debug marker publisher positively aids troubleshooting.
Publishing to/paf/{self.role_name}/control/pp_debug_markerswill help visualize the pure pursuit logic, no issues found.
100-104: Throttled warnings reduce spam.
Usingrospy.logwarn_throttle(1.0, ...)ensures repetitive warnings do not flood the logs. This is a good practice.
108-113: Additional throttled logging for velocity checks.
Similarly, restricting repeated velocity-not-set warnings is beneficial.
116-123: Error handling for missing steering angle.
Recognizing when__calculate_steer()fails (returnsNone) and logging an explicit error improves debugging. Consider if the car should default to zero steering to maintain a safe fallback.
127-127: Return type now accommodates optional output.
Changing the signature to-> Optional[float]accurately represents possible failure scenarios and aligns with the new error handling approach.
132-178: Refactored trajectory math using mapping_common.
This new approach leverages helper functions (build_trajectory_from_start,split_line_at, etc.), simplifying the code. The debug-markers logic is also well structured.
188-188: Improved path validation.
Discarding empty paths with a log message prevents erroneous pure pursuit calculations and clarifies system state.code/planning/src/global_planner/global_planner_node.py (13)
2-3: Imports for handling collections and deque look good.
No issues spotted; this is a standard Python approach.
17-18: Importing Point2 appears consistent with usage.
No concerns identified.
68-68: Underscore callback naming is fine.
Using a “private” method_global_route_callbackas a ROS topic callback is acceptable; no changes needed.
95-96: Refactoring to separate callback logic is a good practice.
This dedicated method reduces clutter and improves maintainability.
98-98: Well-defined update method for global route.
The method name is descriptive, and the docstring suffices.
131-131: Log statement grammar is correct.
No issues here; the message is clear for debugging.
252-254: Logic for retrying preplanning is clear.
Storing and reusingglobal_route_backupensures resilience. Good job.
270-272: Assigning agent_pos and building custom Point2 object.
This is clear and straightforward for continued math-based operations.
281-282: Queue operations on last_agent_positions look fine.
A ring-buffer approach with popleft/append is appropriate.
284-290: Check for fallback usage of agent_pos/agent_ori.
Returning early sets them to None, so ensure all downstream usage is guarded.
292-292: Assigning stable position to agent.
No issues found here.
296-296: Expression in log message is clear.
No further improvement needed.
299-299: Method reuse is appropriate.
Reusingupdate_global_route(... )for re-planning is well-structured.code/mapping/ext_modules/mapping_common/map.py (6)
25-25: New enum value for lane state is sensible.
DefiningTO_BE_CHECKED = 2helps unify the lane-check pipeline.
256-256: Docstring clarifies the map property.
This comment properly indicates the tree contains a filtered subset while referencing the full map.
487-488: Introducing motion awareness parameter.
Addingmotion_aware: boolinis_lane_freeis consistent with the PR’s goal of ignoring entities moving away.
523-535: Conditional usage of lanemarking check is well-structured.
Ifcheck_methodis "lanemarking" or "fallback," the code attempts the specialized approach first. The fallback is gracefully handled. Good approach.
537-539: Fallback to rectangle when lanemarking fails.
This logic is consistent with your docstring. Nicely done.
571-574: Return correct final lane state.
Logic is consistent with your definitions: no collisions → FREE, otherwise BLOCKED.code/mapping/ext_modules/mapping_common/mask.py (9)
1-1: Expanded typing import is fine.
This accommodates the new functionality with optional types and lists.
10-10: Added geometry_msgs imports for new functions.
This is correct usage to handle Pose, Point, and PoseStamped.
95-97: Validate minimum segment length while splitting lines.
The new conditions for requiring at least two points are good; ensures no partial geometry errors.
155-166: New ros_path_to_line conversion logic.
The iteration overposes_viewis straightforward, and building the Shapely LineString is correct.
169-176: New line_to_ros_path function.
Implementation is consistent with the reverse ofros_path_to_line; straightforward approach.
179-179: Support for both NavPath and LineString in build_trajectory.
This flexibility is beneficial for advanced path usage.
214-219: Indices clamping for NavPath is elegantly handled.
Slicing poses fromcurrent_wp_idxtocurrent_wp_idx+max_wp_countis a safe approach.
330-335: New build_lead_vehicle_collision_masks function.
Design to combine static front mask plus a trajectory-based mask is logical.
394-439: build_trajectory_from_start function.
Implementation for adjusting the local path to the hero’s start point is clear and matches your motion-aware logic.code/mapping/src/mapping_data_integration.py (15)
10-11: Imports look fine
No issues spotted.
13-16: Additional imports are well-organized
These new references to mapping_common modules are consistent with the rest of the codebase.
26-27: New ROS message and service imports
Everything appears correct.
55-62: New class fields for stop marks and hero position
The typed hints and documentation string are helpful. Nice addition.
66-68: Stop marks dictionary initialization
Straightforward initialization with no issues.
69-74: Subscription for current position
The subscription setup is correct. Ensure the topic name matches upstream configuration.
75-80: Subscription for heading
Looks consistent with the existing patterns for subscribing to Float32 data.
87-87: Comment line
No functional change.
137-138: Comment lines
No functional change.
156-156: Timer creation
Ensureself.ratecannot be zero or negative to avoid runtime errors.
190-191:heading_callbackmethod
Sets the heading directly. Straightforward and clear.
193-194:current_pos_callbackmethod
Correctly converts ROS message into Point2.
465-467: Creating hero entity
Assigning timestamp and motion to the hero entity is correctly implemented.
470-475:publish_new_map_handlermethod
Exception handling is good to prevent node crashes.
478-481: Early return if hero data is missing
Prevents generating incomplete maps—this is a prudent check.code/planning/src/local_planner/motion_planning.py (21)
3-7: New import statements
Imports from math, numpy, and shapely look properly declared.
11-16: Importing geometry messages, std_msgs, and service references
Appears accurate.
17-26: Added overtake-related service definitions
Matches the usage in the rest of the file.
28-31: New imports from mapping_common
Seem required for the transform operations.
39-50: Docstring additions
Clear explanation of the node’s role in trajectory and overtaking. Good documentation.
59-65: Overtake request fields
Well-defined optional usage with straightforward defaults.
67-76: Trajectory-related fields
Logical approach for handling global trajectory state.
92-94: New callback in__set_global_trajectory
Ensures we re-init if global trajectory updates.
96-101: Speed limit from OpenDrive
Allows dynamic handling of speed limits. Implementation is correct.
109-127: Services for starting/stopping/querying overtake
Well-defined. Just confirm no naming collisions with other modules.
131-141: Global and local trajectory publishers
Publishes for debug and consumption. Straightforward approach.
146-175:__start_overtake_callbackmethod
Properly merges offsets if another overtake is active. Handling logic is comprehensive.
176-191:__end_overtake_callbackand partial logic for__overtake_status_callback
Clean approach to finalize or cancel an overtake.
205-214: Docstring for__set_current_pos+ partial subscription
Accurate explanation and usage.
215-294:publish_local_trajectorymethod
Generates local trajectory from global. The logic to handle re-initialization if out of range is good.
294-325:_update_current_global_waypoint_idxmethod
Smart indexing approach to reduce computational overhead on very long paths.
326-414:_apply_overtakemethod
Offsetting the trajectory and splitting it at start/end distances is well conceived. Checking distance from the front of the hero entity is also a good approach.
416-426: Re-initializing global trajectory
Resetsinit_trajectoryto handle newly received paths.
427-434: Speed limits from OpenDrive
Receives array of float values to map to waypoints. Straightforward.
435-437:runmethod
Runs the node’s spin loop with no issues.
439-451:signhelper function
Simple and effective approach usingmath.copysign.code/planning/src/behavior_agent/behaviors/overtake_service_utils.py (13)
1-2: Initial imports
Straightforward usage of rospy and typing.
3-4: py_trees.blackboard import
Allows data retrieval from blackboard. No issues.
5-14: Planning service imports
Covers all messages for overtake lifecycle.
16-18: mapping_common imports
Used for coordinate transformations. Looks good.
19-21: geometry_msgs / std_msgs
Consistent with usage in transforms.
23-29:create_start_overtake_proxy
Waits for service, returns Proxy. Straightforward pattern.
31-35:create_end_overtake_proxy
Same pattern for end of overtaking. No concerns.
37-43:create_overtake_status_proxy
Waits on the status service, consistent style.
45-62:get_global_hero_transformutility
Pulls data from blackboard. Solid fallback to None if missing data.
64-67: Deprecated_get_global_hero_transform
Redirects to the new function. Clear deprecation note.
69-132:request_start_overtake
Performs local-to-global transform, sets the request fields, and calls the service. Good clarity in docstring.
134-174:request_end_overtake
Same approach for ending or aborting overtake. Logic is consistent.
176-188:request_overtake_status
Simple pass-through function to fetch current overtake status.code/mapping/ext_modules/mapping_common/entity.py (4)
507-529: Good addition of a general velocity comparison method.Introducing
get_delta_velocity_ofprovides a unified way to compute relative motion while automatically handling whether the other entity is in front or behind. This increases clarity and prevents common errors.
696-739: Useful newStopMarkentity for virtual obstacles.Adding a dedicated
StopMarkclass aligns with your objective of handling custom stop line logic. Be sure to test complex cases where multipleStopMarkentities overlap.
746-746: Reintroduced method signature.Double-check that overriding
to_markerhere does not skip extra visual details that might be relevant for pedestrians. If you removed any previously inherited logic, confirm it's no longer needed.
755-762: Consistent addition ofStopMarkin supported classes.Registering
StopMarkin_entity_supported_classesensures robust entity deserialization. Looks good.code/planning/src/behavior_agent/behaviors/overtake.py (18)
12-12: Removed or changed imports.Re-check that references to
StopMarkare indeed required here and that no unnecessary imports remain after refactoring.
14-14: Minor import addition.Importing
Vector2andPoint2helps with transformations. This looks consistent with usage patterns in the file.
31-34: Stop mark service utilities in use.These lines import
create_stop_marks_proxyandupdate_stop_marks. Confirm that no exception handling is required if the service is unavailable or times out.
43-62: Functionset_space_stop_markis well-written.It sets up a
StopMarkbehind the obstacle to ensure safe distance. Verify that backward translation is always the required approach for every obstacle shape or orientation.
110-112: Safe fallback when trajectory is missing.Returning
FAILUREwith a debug status iftrajectory_localisNoneis clear and prevents undefined behavior.
121-123: Collision mask creation for obstacle detection.Using the hero's front x offset and width is logical. Please confirm that
hero.get_front_x()accounts for possible shape rotations.
173-173: Stop proxy creation for clearing & setting stop marks.Fine approach. No issues identified.
180-180: Unsetting space stop mark during initialization.Helps avoid leftover stop marks from previous states. Good housekeeping measure.
271-272: Creating the start-overtake and stop-mark proxies.Be sure to keep an eye on exception handling if services are not available.
333-337: Conditional usage of stop space.Using
set_space_stop_markonly when the obstacle is essentially stationary is a nice detail.
409-409: Start-overtake proxy in Wait behavior.Looks consistent with usage in Approach.
411-411: Stop proxy in Wait behavior.Parallel usage pattern across Approach and Wait classes.
480-482: Reverting stop mark.Good housekeeping again. Ensures no leftover
StopMark.
483-488: Re-checking lane free logic.Verifying that
LaneFreeState.FREEmeans truly open for overtaking. Confirm fallback coverage for edge cases (e.g., merges or splits).
497-499: Triggering overtaking from Wait state.Using
start_transition_length=0.0here might cause an abrupt maneuver if not carefully tested.
543-543: Unsetting space stop mark on Enter.Makes sense, as the vehicle is about to move.
557-559: Fail fast on NO_OVERTAKE.Appropriate to abort. Adhere to your car's safety design guidelines.
561-563: Fail on unknown statuses.Good fallback approach unless you plan to handle newly added statuses more gracefully.
code/planning/src/local_planner/ACC.py (27)
3-3: Type hints import.Using
from typing import Optional, List, Tupleis standard, no issues found.
5-5: Shapely import.No issues; presumably the environment has shapely installed.
13-13: Additional messages import.Brings in
Float32andStringfor steering, speed, or behavior updates. Looks correct.
19-20: New imports for marking & shapes.Checks out since references to
mapping_common.markersandmapping_common.shapeappear throughout.
25-25: Entity usage.Makes sense for hero data extraction and collision checks.
27-27: Transform imports used for collision masks.The new transformations for the front mask rely on
Vector2andPoint2. This is consistent.
30-34: Service imports for speed alteration.Implementing partial support for dynamic speed changes is in line with the new architecture.
46-46:trajectory_localattribute.Renaming from a generic
trajectoryis consistent with the local planner approach.
48-51: Default speed limit and new variables.These additions clarify new external constraints (
speed_override,external_speed_limit, andsteer). Be cautious with concurrency if multiple publishers override these simultaneously.
61-61: Method reference mismatch.You changed the subscription callback to
self.__set_map. Confirm that the method signature is still correct and doesn’t conflict with older code references (like__get_map).
66-66: Speed limit subscription.Reading from
/speed_limitis consistent with the loaded config. Confirm the frequency of changes remains stable.
89-96: Steer subscription.Storing steer angle for collision mask rotation is a neat trick. Watch out for sign convention mismatch between your pure pursuit and ACC logic.
107-112: Service initialization for speed alteration.Ensures external components can dynamically change the ACC’s speed. This addresses various scenario-based constraints.
164-164: Early return if map or trajectory is missing.This is a safe default to avoid driving blindly.
185-186: Tree building with filter function.Consistent approach. Validate performance if numerous filters or large maps are used.
[approve]
190-190: Reverting to larger mask.Increasing to 5.5 is consistent with standard ACC buffer.
192-198: Building collision masks with rotation factor.Incorporating steering angle is a strong improvement for shaping the forward collision boundary.
205-205: Appending debug marker.Useful for runtime visualization. No issues found.
211-211: Initializing debug marker text.A concise string for ACC status overview is good for quick checks.
230-233: Appending lead vehicle info to debug text.Clear approach to assist with real-time debugging.
236-238: Capping speed to local speed limit.A straightforward approach ensuring we never exceed the set limit.
258-261: Speed override.Allows a direct override in special cases. Make sure you revert or reset this override reliably.
262-263: Diagnostic text expansions.Adding final speed and reason is excellent for clarity.
303-310: Distance-based speed adjustment logic.Looks correct for typical ACC usage. Double-check units and scale factors to avoid unintended high/low speeds.
312-313: Hard approach fallback.If the lead vehicle is beyond a certain distance, ensure we maintain a minimal approach speed. Good for not stalling the flow.
321-323: New trajectory-based corner speed function.Method signature is clear, returning speed and debug markers.
410-420: Comprehensive speed alteration handler.Good use of a single service callback to handle overrides and speed limits. Validate that partial requests (e.g., only override) are processed correctly.
code/planning/src/behavior_agent/behaviors/topics2blackboard.py
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (4)
code/mapping/ext_modules/mapping_common/map.py (4)
25-25: Document the newTO_BE_CHECKEDenum value.
Consider adding a short comment or docstring clarifying under which circumstancesTO_BE_CHECKEDis used.
515-516: Good inclusion of docstring formotion_aware.
The docstring helps clarify the parameter usage. Consider briefly explaining the thresholds (like 0.5 m/s used in filtering) for future maintainers.
523-538: Refined lane-checking sequence.
Deferring the final lane state by returningTO_BE_CHECKEDlets you unify logic after lanemark or rectangle checks. This is a clean approach. However, if merging checks further reduces duplication, consider it.
554-591: Motion-aware filtering logic.
Filtering out entities moving away behind the hero or ahead in the same direction is beneficial. Consider documenting or parameterizing the 0.5 and 1.5 thresholds, and ensure side-motion or angled motion is handled if needed.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (4)
code/mapping/config/mapping.cfg(1 hunks)code/mapping/ext_modules/mapping_common/entity.py(2 hunks)code/mapping/ext_modules/mapping_common/map.py(6 hunks)code/planning/src/behavior_agent/behaviors/overtake.py(4 hunks)
🚧 Files skipped from review as they are similar to previous changes (2)
- code/mapping/config/mapping.cfg
- code/mapping/ext_modules/mapping_common/entity.py
🧰 Additional context used
🪛 Ruff (0.8.2)
code/mapping/ext_modules/mapping_common/map.py
551-551: Use lane_state != LaneFreeState.TO_BE_CHECKED instead of not lane_state == LaneFreeState.TO_BE_CHECKED
Replace with != operator
(SIM201)
🔇 Additional comments (6)
code/planning/src/behavior_agent/behaviors/overtake.py (3)
283-283: Confirm the extended clear distance.
Increasingself.clear_distanceto 50 makes the approach more conservative. Ensure this change aligns with desired overtaking behavior and doesn't introduce excessive caution.
343-343: Validate the reducedlane_transform.
Loweringlane_transformto 10.0 may affect the space checked for oncoming traffic. Verify if 10.0 is sufficient for safe overtaking in typical scenarios.
415-415: Maintain consistency between Approach and Wait.
Settingself.clear_distance = 50here matches the Approach class. Confirm both classes behave harmoniously under varied driving conditions with the updated distance.code/mapping/ext_modules/mapping_common/map.py (3)
487-488: Motion-aware parameter introduced.
Addingmotion_aware: bool = Trueexpands flexibility. Ensure tests cover both motion-aware and non-motion-aware scenarios for robust coverage.
642-644: Consistent sub-check return value.
ReturningLaneFreeState.TO_BE_CHECKEDis consistent with the new final evaluation flow. Tests should confirm the subsequent logic properly transitions this into FREE or BLOCKED.
734-734: Same approach forlanemarking.
This return mirrors the rectangle method to unify the final lane state logic.
There was a problem hiding this comment.
Actionable comments posted: 0
🧹 Nitpick comments (3)
code/mapping/ext_modules/mapping_common/map.py (3)
523-538: Potential confusion aroundTO_BE_CHECKEDnaming.
AssigningLANE_GEOMETRY_OKor a similar descriptive name instead ofTO_BE_CHECKEDcould improve clarity. Currently, the code usesTO_BE_CHECKEDto denote a successful geometry creation phase, which might be non-intuitive.- TO_BE_CHECKED = 2 + GEOMETRY_OK = 2
560-561: Consider logging a debug note when no hero is present.
Ifmotion_awareis enabled butherois missing, the motion-based filtering is effectively skipped. Logging a debug warning might help troubleshooting.if motion_aware and hero is not None: ... + else: + rospy.logdebug("Skipping motion-aware filtering because hero entity is None.")
581-585: Clarify the comment regarding direction of motion.
The inline comment mentions “car is moving in our direction,” but the conditionforward_velocity > 0.5suggests it’s actually moving away from the hero. Renaming or rephrasing might prevent confusion.- # car is moving in our direction and is in front of us + # car is moving away from us and is in front
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
code/mapping/ext_modules/mapping_common/map.py(6 hunks)
🔇 Additional comments (8)
code/mapping/ext_modules/mapping_common/map.py (8)
25-25: Enum addition is consistent.
AddingTO_BE_CHECKED = 2toLaneFreeStatealigns with the new workflow of deferring the final decision until after geometric and motion checks. This integration looks correct and poses no immediate issues.
487-516: Docstring formotion_awareis sufficient.
The newly introducedmotion_awareparameter has an accompanying docstring that adequately explains its purpose. It's generally clear how it filters objects based on relative motion. Good job!
548-552: Logic for returningSHAPE_ERRis coherent.
The flow ensures that the function only proceeds to collision checks if no errors occurred and a valid geometry is produced. The condition here is consistent with the new approach where “TO_BE_CHECKED” indicates a valid shape.
555-558: Use ofget_overlapping_entitiesfor collision determination is correct.
Delegating to this helper clarifies the logic and separates geometric handling from subsequent filtering.
562-579: Safe approach of iterating backward when removing items in-place.
This loop pattern avoids index shifting pitfalls and ensures correctness of entity removal.
587-591: Final collision logic is straightforward.
ReturningFREEorBLOCKEDbased on the presence of colliding entities is clear and fits the intended design.
642-644: Rectangle-based lane geometry.
ReturningTO_BE_CHECKEDupon successful creation of the rectangle shape is consistent with the final collision check logic.
734-735: Lanemark-based lane geometry outcome.
Similarly, returningTO_BE_CHECKEDupon successful lanemark geometry creation aligns with the overall pattern of deferring the final free/block conclusion until after collision checks.
Description
The lane free has a motion aware flag now. This results in ignoring objects which have a velocity and move away from the hero. This is especially useful in an overtake scenario and will be used for the intersection check.
Fixes #726
Type of change
Please delete options that are not relevant.
Does this PR introduce a breaking change?
No
Most important changes
The correct entities are filtered out.
Checklist:
Summary by CodeRabbit