638 feature rework entering overtake #659
Conversation
…or-useage-in-behaviour-tree' into 638-feature-rework-entering-overtake
…into 638-feature-rework-entering-overtake
…nce-to-leftright-turn-in-intersections' into 638-feature-rework-entering-overtake
…nce-to-leftright-turn-in-intersections' into 638-feature-rework-entering-overtake
…ap-function-lane-free' into 638-feature-rework-entering-overtake
WalkthroughThe pull request introduces comprehensive modifications to the overtaking behavior in the robotic system. The changes span multiple files in the planning and local planner modules, focusing on enhancing the decision-making process for overtaking maneuvers. The modifications include restructuring the behavior tree, refining overtaking logic, adding distance and marker publishers, and updating motion planning calculations to support more sophisticated overtaking strategies. Changes
Possibly related issues
Poem
Tip 🌐 Web search-backed reviews and chat
✨ 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: 2
🧹 Nitpick comments (18)
code/planning/src/behavior_agent/behaviors/leave_parking_space.py (3)
7-10: New import usage and commented-out import.Good addition of the
Mapimport; it is indeed used below to check lane availability. Since# from mapping import msgis now commented out, consider removing it if it's no longer needed to keep the codebase clean.-# from mapping import msg
112-113: Clarifying comment about routine start.This comment helps readers understand the sequential flow of the logic. Keep the docstring updated so it aligns well with these in-line details.
114-128: Safe lane-free check before unparking.Invoking
Map.from_ros_msg(map_data)andmap.is_lane_free(...)is a good addition to ensure the left lane is actually free. Consider parameterizinglane_length=22.5andlane_transform=-5.0or referencing them in a config file to make adjustments easier.code/planning/src/behavior_agent/behaviors/overtake.py (12)
6-6: Unused importmath.According to static analysis,
mathis never used. Consider removing it to keep the codebase lean.-import math🧰 Tools
🪛 Ruff (0.8.2)
6-6:
mathimported but unusedRemove unused import:
math(F401)
7-7: Unused importRotation.
scipy.spatial.transform.Rotationis not used. Remove it to clean up.-from scipy.spatial.transform import Rotation🧰 Tools
🪛 Ruff (0.8.2)
7-7:
scipy.spatial.transform.Rotationimported but unusedRemove unused import:
scipy.spatial.transform.Rotation(F401)
12-12: Unused importconvert_to_ms.This function is never called. Consider removing it.
- convert_to_ms,🧰 Tools
🪛 Ruff (0.8.2)
12-12:
local_planner.utils.convert_to_msimported but unusedRemove unused import:
local_planner.utils.convert_to_ms(F401)
16-16: Unused importPolygon.
Polygonis not used. Remove it to adhere to standard linting guidelines.-from mapping_common.shape import Polygon🧰 Tools
🪛 Ruff (0.8.2)
16-16:
mapping_common.shape.Polygonimported but unusedRemove unused import:
mapping_common.shape.Polygon(F401)
100-105: Null-check for map data in Overtake.Failing immediately on missing map data has clear semantics. Though this ensures safety, consider fallback strategies if your system can operate in partial functionality without a full map.
145-149: Filtering out false positives.Relying on lateral position thresholds (
-2.5,2.5) to disregard irrelevant objects can be effective, but might lead to missing certain edge cases. Confirm these boundary values align with your lane width specifications or real-world constraints.
152-161: Marker publishing with short lifetime.The 0.3-second lifetime is good for ephemeral markers. If debugging or visualizing real-time data at lower framerates, consider whether a slightly longer lifetime might be more user-friendly.
162-170: Counter-based approach for positive overtake detection.Adding a tolerance (counter > 4) helps filter out transient obstacles or noise. Consider documenting these magic numbers or making them configurable to accommodate different traffic conditions.
241-242: Additional state tracking for approach.
self.ot_counterandself.clear_distancecontribute to the logic. Be sure to provide a clear definition or docstring about how these thresholds are derived.
265-269: Early failure on missing map data.As with the
Aheadclass, a graceful fallback approach or an alternative behavior might be worth considering if partial operation is acceptable.
307-326: Oncoming lane check before overtaking.Good logic to slow down if
is_lane_free(False, ...)returns false. If you see repeated patterns for obtaining map status in multiple behaviors, consider extracting them into a shared helper.
392-394: New publisher for overtake distance in Wait.Another instance of the same pattern. Reusing or centralizing these publisher setups across the three classes might reduce duplication.
I can help refactor this into a shared utility if you’d like. Let me know if you want a new issue created.
code/mapping/ext_modules/mapping_common/map.py (2)
109-172: Consider making lane dimensions configurable.The method is well-implemented with proper input validation and clear documentation. However, the lane width (1.5m) and offset (2.2m) are hardcoded, which might not be suitable for all vehicle types or road configurations.
Consider making these dimensions configurable:
def is_lane_free( self, right_lane: bool = False, lane_length: float = 20.0, lane_transform: float = 0.0, + lane_width: float = 1.5, + lane_offset: float = 2.2, ) -> bool: # ... lane_box_shape = Rectangle( length=lane_length, - width=1.5, + width=lane_width, offset=Transform2D.new_translation( - Vector2.new(lane_transform, lane_pos * 2.2) + Vector2.new(lane_transform, lane_pos * lane_offset) ), )🧰 Tools
🪛 Ruff (0.8.2)
170-172: Return the condition
bool(not lane_box_intersection_entities)directlyReplace with
return bool(not lane_box_intersection_entities)(SIM103)
Line range hint
175-186: Enhance return type documentation.While the parameter documentation has been improved, the return type documentation could be more detailed.
Consider updating the return type documentation:
Returns: - - Polygon: A Shapely Polygon representing the plane. + - shapely.geometry.Polygon: A rectangular polygon representing the projected + plane, with coordinates relative to the hero car's position.🧰 Tools
🪛 Ruff (0.8.2)
170-172: Return the condition
bool(not lane_box_intersection_entities)directlyReplace with
return bool(not lane_box_intersection_entities)(SIM103)
code/planning/src/local_planner/motion_planning.py (1)
530-533: Add validation in the setter method.The setter should validate the input data before assignment:
def __set_ot_distance(self, data: Float32): if data is not None: - self.__ot_distance = data.data + if data.data >= 0: + self.__ot_distance = data.data + else: + self.logwarn(f"Invalid overtake distance: {data.data}") + self.__ot_distance = float('inf')
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (8)
code/mapping/ext_modules/mapping_common/map.py(5 hunks)code/mapping/src/mapping_data_integration.py(1 hunks)code/planning/src/behavior_agent/behavior_tree.py(1 hunks)code/planning/src/behavior_agent/behaviors/leave_parking_space.py(4 hunks)code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)code/planning/src/behavior_agent/behaviors/topics2blackboard.py(2 hunks)code/planning/src/local_planner/motion_planning.py(9 hunks)code/planning/src/local_planner/utils.py(2 hunks)
✅ Files skipped from review due to trivial changes (1)
- code/mapping/src/mapping_data_integration.py
🧰 Additional context used
🪛 Ruff (0.8.2)
code/mapping/ext_modules/mapping_common/map.py
14-14: Redefinition of unused Transform2D from line 12
Remove definition: Transform2D
(F811)
170-172: Return the condition bool(not lane_box_intersection_entities) directly
Replace with return bool(not lane_box_intersection_entities)
(SIM103)
code/planning/src/behavior_agent/behaviors/overtake.py
6-6: math imported but unused
Remove unused import: math
(F401)
7-7: scipy.spatial.transform.Rotation imported but unused
Remove unused import: scipy.spatial.transform.Rotation
(F401)
12-12: local_planner.utils.convert_to_ms imported but unused
Remove unused import: local_planner.utils.convert_to_ms
(F401)
16-16: mapping_common.shape.Polygon imported but unused
Remove unused import: mapping_common.shape.Polygon
(F401)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (21)
code/planning/src/behavior_agent/behaviors/leave_parking_space.py (6)
28-29: More expressive state management.Replacing
self.calledwithself.startedandself.finishedis a good approach to better reflect the behavior’s lifecycle and improve clarity.
92-93: Retrieving map data from blackboard.Fetching
map_datafrom the blackboard aligns with the existing architecture. Confirm that the publisher for"/paf/hero/mapping/init_data"is reliably providing data in all scenarios to prevent potential blocking conditions downstream.
94-94: Conditional check for the ‘finished’ state.This additional guard prevents re-running the leaving-parking process after completion. Good defensive approach to avoid repeated actions.
100-100: Null-check for map data.Validating
map_dataensures correct usage of the mapping logic. This avoids potentialNoneTypeerrors. Good job on defensive coding.
129-136: Return status usage might be confusing.Marking the behavior as
FAILUREonce the vehicle moves sufficiently seems counterintuitive if “finished leaving” would normally imply success. Ensure that usingFAILUREhere won’t cause undesired logic in parent nodes (e.g., fallback or parallel behaviors).
138-139: Continuing in RUNNING state.This fallback to a
RUNNINGstate is clear, ensuring the behavior remains active. Confirm that once the lane is blocked again or the flow changes, further re-checks are triggered as intended.code/planning/src/behavior_agent/behaviors/overtake.py (7)
2-2: Useful imports for new messages.The addition of
Float32helps in publishing overtaking distance. Keep the usage of all new imports in sync with your data flows.
30-30: Clarification of global variable usage.
OVERTAKE_EXECUTINGis set to zero here as a placeholder. Ensure that usage across classes is thread-safe and consistent in a multithreaded environment, if applicable.
63-68: Publishing overtaking distance and markers.Great idea to have separate publishers for distance and visualization. Validate that the topics are properly handled in your ROS environment, and consider logging any exceptions if these publishers fail to connect.
81-81: Old obstacle distance tracking.
self.old_obstacle_distance = 200is used for referencing the previous distance. If you plan to handle incremental logic based on changes in distance, ensure there’s a plan to compare or reset it appropriately.
108-128: Trajectory-based obstacle detection.Using
map.get_obstacle_on_trajectory(...)significantly improves the decision-making process. Confirm that the friction model or environmental factors don’t require additional logic for edge cases (e.g., partial occlusion or static obstacles not on the exact path).
223-225: New publisher for overtaking distance.Consistent pattern with the
Aheadclass. Keep an eye on iteration or duplication across classes that might indicate a shared utility for publishing overtake data.
408-409: Clean separation of counters and distances.
self.clear_distanceandself.ot_counterkeep the wait logic consistent. Consider verifying these thresholds match real-world driving constraints (lane widths, safe distances, etc.).code/planning/src/behavior_agent/behaviors/topics2blackboard.py (1)
109-118: New topics for trajectory and current waypoint.Adding these topics extends the blackboard with essential planning data. This integration is important for advanced maneuvers. Ensure test coverage includes validating that these topics correctly populate the blackboard and remain in sync with the map data usage.
code/planning/src/behavior_agent/behavior_tree.py (1)
Line range hint
79-92: LGTM! Well-structured behavior tree modification.The new "Overtaking" Selector node follows the established pattern and maintains consistency with other behavior implementations like lane switching. This structure aligns well with the PR's objective of reworking the overtake routine.
code/planning/src/local_planner/utils.py (2)
16-16: Verify the shorter stopping distance for overtaking.The overtaking stopping distance (3.5m) is significantly shorter than the regular stopping distance (5m). While this allows for more aggressive overtaking maneuvers, ensure this shorter distance has been validated for safety across different speeds and conditions.
246-260: LGTM! Well-structured utility function.The implementation is clean and efficient:
- Good use of numpy for array operations
- Clear type hints and docstring
- Proper error handling through numpy's built-in checks
code/planning/src/local_planner/motion_planning.py (4)
117-122: LGTM! Well-structured subscriber setup.The new overtake distance subscriber follows the established pattern:
- Consistent topic naming convention
- Proper QoS profile
- Type safety with Float32
512-516: Verify overtake status handling.The code checks for infinite distance before trajectory change but doesn't validate other edge cases:
- What happens if distance is negative?
- What happens if distance is zero?
699-700: LGTM! Clear virtual overtake calculation.The change from collision point to overtake distance improves the clarity of the code's intent.
Line range hint
283-298: Verify the trajectory generation parameters.The function uses hardcoded values that might need adjustment:
normal_x_offset = 2for regular overtakingunstuck_x_offset = 3for unstuck scenariosRun the following script to check if these values are consistent with other trajectory calculations in the codebase:
✅ Verification successful
Trajectory generation parameters are consistently implemented
The offset values are used appropriately for trajectory generation:
- Values are consistently applied throughout the motion planning system
- Implementation properly handles both regular overtaking (2m) and unstuck scenarios (3m)
- Comment about potential adjustment with better steering shows awareness of the parameter's impact
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash # Search for other trajectory offset calculations rg -A 3 'x_offset.*=.*[23]' --type pyLength of output: 540
Script:
#!/bin/bash # Search for trajectory-related parameters and their usage echo "=== Searching for trajectory and offset parameters ===" rg -g '*.py' -g '*.yaml' -g '*.json' -i 'offset.*=|trajectory.*param|NUM_WAYPOINTS' -A 2 echo -e "\n=== Searching for configuration files ===" fd -e yaml -e json echo -e "\n=== Checking how these offsets are used ===" ast-grep --pattern 'normal_x_offset|unstuck_x_offset' -l pythonLength of output: 10839
….com/una-auxme/paf into 638-feature-rework-entering-overtake
There was a problem hiding this comment.
Actionable comments posted: 5
🧹 Nitpick comments (9)
code/planning/src/local_planner/motion_planning.py (4)
Line range hint
283-297: Improve readability of trajectory selection logic.The selection of trajectory points for overtaking could be made more readable by extracting the index calculations into named variables.
Consider refactoring like this:
- selection = pose_list[ - int(currentwp) - + min(2, int(distance / 2)) : int(currentwp) - + int(distance) - + NUM_WAYPOINTS - + 2 - ] + start_idx = int(currentwp) + min(2, int(distance / 2)) + end_idx = int(currentwp) + int(distance) + NUM_WAYPOINTS + 2 + selection = pose_list[start_idx:end_idx]
513-517: Add error logging for invalid overtake distance.When the overtake distance is infinite, it would be helpful to log this condition for debugging purposes.
Add error logging:
if np.isinf(self.__ot_distance): + self.logwarn("Overtake distance is infinite, aborting trajectory change") self.__overtake_status = -1 self.overtake_success_pub.publish(self.__overtake_status) return
675-676: Document the speed calculation constants.The magic numbers
9.0and1.4in the speed calculation should be documented to explain their significance.Add comments explaining the constants:
- v_stop = min(convert_to_ms(9.0), convert_to_ms(stopline / 1.4)) + # Maximum speed during overtake approach is 9.0 m/s + # Deceleration factor of 1.4 provides smooth braking + v_stop = min(convert_to_ms(9.0), convert_to_ms(stopline / 1.4))
700-701: Consider adding a buffer constant.The magic number
2.5used as a buffer distance should be defined as a constant at the module level.Define the buffer as a constant:
+# Buffer distance in meters to maintain from the obstacle during overtake +OVERTAKE_BUFFER_DISTANCE = 2.5 def __calc_virtual_overtake(self) -> float: if (self.__ot_distance is not None) and self.__ot_distance != np.inf: - return self.__ot_distance - 2.5 + return self.__ot_distance - OVERTAKE_BUFFER_DISTANCEcode/planning/src/behavior_agent/behaviors/overtake.py (5)
26-28: Consider replacing global variables with class-level state management.Using global variables for state management can lead to thread-safety issues and make the code harder to test. Consider encapsulating these states within a dedicated class or using the behavior tree's blackboard for state management.
-# Variable to determine the distance to overtake the object -OVERTAKE_EXECUTING = 0 -OVERTAKE_FREE = False +class OvertakeState: + def __init__(self): + self.executing_distance = 0 + self.is_free = False + + @classmethod + def get_instance(cls): + if not hasattr(cls, '_instance'): + cls._instance = cls() + return cls._instance
167-168: Extract magic numbers into named constants.The counter threshold of 4 is hardcoded. Consider extracting it into a named constant for better maintainability and documentation.
+# Number of consecutive detections required to confirm overtake situation +OVERTAKE_CONFIRMATION_THRESHOLD = 4 - if self.counter_overtake > 4: + if self.counter_overtake > OVERTAKE_CONFIRMATION_THRESHOLD:
246-246: Extract magic number into a named constant.The clear_distance value of 40 is hardcoded. Consider extracting it into a named constant at the module level.
+# Distance to check for clear path during overtake approach +CLEAR_PATH_CHECK_DISTANCE = 40 - self.clear_distance = 40 + self.clear_distance = CLEAR_PATH_CHECK_DISTANCE
429-430: Extract magic number into a named constant.The clear_distance value of 35 is hardcoded. Consider extracting it into a named constant at the module level.
+# Distance to check for clear path during wait phase +WAIT_CLEAR_PATH_CHECK_DISTANCE = 35 - self.clear_distance = 35 + self.clear_distance = WAIT_CLEAR_PATH_CHECK_DISTANCE
Line range hint
606-685: LGTM with a minor note.The Leave class implementation is clean, but it still relies on the global OVERTAKE_EXECUTING variable. Consider refactoring this as suggested earlier to use class-level state management.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)code/planning/src/local_planner/motion_planning.py(9 hunks)code/planning/src/local_planner/utils.py(1 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- code/planning/src/local_planner/utils.py
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (2)
code/planning/src/local_planner/motion_planning.py (1)
22-22: LGTM! Import added for new constant.The addition of
TARGET_DISTANCE_TO_STOP_OVERTAKEis consistent with the PR's objective to enhance overtaking behavior.code/planning/src/behavior_agent/behaviors/overtake.py (1)
Line range hint
531-605: LGTM!The Enter class implementation is clean and well-structured.
There was a problem hiding this comment.
Actionable comments posted: 0
🧹 Nitpick comments (3)
code/planning/src/behavior_agent/behaviors/overtake.py (3)
28-30: Consider encapsulating global state in a dedicated class.Global variables
OVERTAKE_EXECUTINGandOVERTAKE_FREEcould lead to thread safety issues and make state management difficult to track. Consider encapsulating these in a dedicated state management class.-OVERTAKE_EXECUTING = 0 -OVERTAKE_FREE = False +class OvertakeState: + def __init__(self): + self._executing = 0 + self._free = False + + @property + def executing(self): + return self._executing + + @executing.setter + def executing(self, value): + self._executing = value + + @property + def free(self): + return self._free + + @free.setter + def free(self, value): + self._free = value + +overtake_state = OvertakeState()
324-341: Simplify lane clearance logic and extract counter mechanism.The lane clearance logic with counter could be simplified and extracted into a helper method for better readability and reusability.
+ def _is_lane_consistently_free(self, map_instance, clear_distance=15): + """Check if the lane is consistently free over multiple checks.""" + if map_instance.is_lane_free(False, self.clear_distance, clear_distance): + self.ot_counter += 1 + return self.ot_counter > 3 + self.ot_counter = 0 + return False - if self.ot_distance < 15.0: - rospy.loginfo(f"Overtake Approach Distance: {self.ot_distance}") - ot_free = map.is_lane_free(False, self.clear_distance, 15) - rospy.loginfo(f"Overtake is free: {ot_free}") - if ot_free: - self.ot_counter += 1 - # using a counter to account for inconsistencies - if self.ot_counter > 3: - rospy.loginfo("Overtake is free not slowing down!") - self.ot_distance_pub.publish(self.ot_distance) - self.curr_behavior_pub.publish(bs.ot_app_free.name) - # bool to skip Wait since oncoming is free - OVERTAKE_FREE = True - return py_trees.common.Status.SUCCESS - else: - self.ot_distance_pub.publish(self.ot_distance) - self.curr_behavior_pub.publish(bs.ot_app_blocked.name) - return py_trees.common.Status.RUNNING + if self.ot_distance < 15.0: + rospy.loginfo(f"Overtake Approach Distance: {self.ot_distance}") + if self._is_lane_consistently_free(map): + rospy.loginfo("Overtake is free not slowing down!") + self.ot_distance_pub.publish(self.ot_distance) + self.curr_behavior_pub.publish(bs.ot_app_free.name) + OVERTAKE_FREE = True + return py_trees.common.Status.SUCCESS + self.ot_distance_pub.publish(self.ot_distance) + self.curr_behavior_pub.publish(bs.ot_app_blocked.name) + return py_trees.common.Status.RUNNING
Line range hint
31-627: Create a base behavior class for common functionality.Multiple behavior classes share common functionality such as:
- Map integration
- Data preparation
- Counter mechanisms
- Publisher setup
- Marker visualization
Consider creating a base behavior class to reduce code duplication and improve maintainability.
class BaseOvertakeBehavior(py_trees.behaviour.Behaviour): """Base class for overtaking behaviors.""" def setup(self, timeout): """Common setup for all overtaking behaviors.""" self.blackboard = py_trees.blackboard.Blackboard() self._setup_publishers() return True def _setup_publishers(self): """Setup common publishers.""" self.curr_behavior_pub = rospy.Publisher( "/paf/hero/curr_behavior", String, queue_size=1 ) self.ot_distance_pub = rospy.Publisher( "/paf/hero/overtake_distance", Float32, queue_size=1 ) self.ot_marker_pub = rospy.Publisher( "/paf/hero/overtake_marker", Marker, queue_size=1 ) def _prepare_data(self): """Common data preparation logic.""" trajectory = self.blackboard.get("/paf/hero/trajectory") current_wp = self.blackboard.get("/paf/hero/current_wp") hero_pos = self.blackboard.get("/paf/hero/current_pos") hero_heading = self.blackboard.get("/paf/hero/current_heading") if all(x is not None for x in [trajectory, current_wp, hero_pos, hero_heading]): return self._process_trajectory_data(trajectory, current_wp, hero_pos, hero_heading) return None, None, None, None def _process_trajectory_data(self, trajectory, current_wp, hero_pos, hero_heading): """Process trajectory data.""" hero_heading = hero_heading.data current_wp = current_wp.data hero_pos = (hero_pos.pose.position.x, hero_pos.pose.position.y) return trajectory, current_wp, hero_pos, hero_heading def _publish_marker(self, entity): """Common marker visualization logic.""" if entity is not None: ot_marker = entity.to_marker() ot_marker.color.r = 1.0 ot_marker.color.g = 0 ot_marker.color.b = 0 ot_marker.header.frame_id = "hero" ot_marker.header.stamp = rospy.Time.now() ot_marker.lifetime = rospy.Duration(0.3) self.ot_marker_pub.publish(ot_marker)
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (2)
code/planning/src/behavior_agent/behaviors/overtake.py (2)
153-161: Add null check before accessing entity for visualization.The code assumes
entityis not None when generating the visualization marker. Add a null check to prevent potential crashes.
462-489: Extract duplicate data preparation logic into a helper function.This data preparation code is duplicated across multiple classes. Consider extracting it into a helper function to improve maintainability.
There was a problem hiding this comment.
Actionable comments posted: 2
♻️ Duplicate comments (1)
code/planning/src/local_planner/motion_planning.py (1)
117-122:⚠️ Potential issueFix the topic name format.
The topic name is missing a leading forward slash which is inconsistent with other topic names in the file.
Apply this diff to fix the topic name format:
- f"paf/{self.role_name}/overtake_distance", + f"/paf/{self.role_name}/overtake_distance",
🧹 Nitpick comments (10)
code/planning/src/local_planner/utils.py (2)
15-15: Consider using float for distance constants.While functionally equivalent, using
5.0instead of5would be more consistent with the floating-point nature of distance measurements.-TARGET_DISTANCE_TO_STOP = 5 +TARGET_DISTANCE_TO_STOP = 5.0
246-260: Consider optimizing the pose conversion.While the implementation is correct, it could be improved in terms of efficiency and robustness.
Consider these improvements:
- Add input validation
- Use numpy vectorization for better performance
def convert_pose_to_array(poses: np.ndarray) -> np.ndarray: """Convert an array of PoseStamped objects to a numpy array of positions. Args: poses (np.ndarray): Array of PoseStamped objects. Returns: np.ndarray: Numpy array of shape (n, 2) containing the x and y positions. """ + if not poses or len(poses) == 0: + return np.empty((0, 2)) + - result_array = np.empty((len(poses), 2)) - for pose in range(len(poses)): - result_array[pose] = np.array( - [poses[pose].pose.position.x, poses[pose].pose.position.y] - ) - return result_array + return np.array([[pose.pose.position.x, pose.pose.position.y] for pose in poses])code/planning/src/behavior_agent/behaviors/behavior_speed.py (1)
71-71: Consider using the conversion function for consistency.The speed value should use the
convert_to_ms()function for consistency with other behaviors. Also, consider adding a comment explaining the rationale for choosing 3.0 m/s.-ot_wait_stopped = Behavior("ot_wait_stopped", 3.0) +# Maintain a slow speed while waiting to check for overtaking opportunities +ot_wait_stopped = Behavior("ot_wait_stopped", convert_to_ms(10.8)) # 3.0 m/s = 10.8 km/hcode/planning/src/behavior_agent/behaviors/behavior_utils.py (1)
32-43: Complete the return type in docstring.The docstring is missing the return type specification.
Returns: - _type_: _description_ + MarkerArray: Visualization marker array containing text and shape markerscode/planning/src/behavior_agent/behaviors/unstuck_routine.py (1)
157-163: Add a comment explaining the wait behaviors list expansion.Consider adding a comment explaining why
int_app_initandint_app_to_stopare considered wait behaviors, as this affects the stuck timer reset logic.wait_behaviors = [ bs.int_wait.name, bs.lc_wait.name, bs.ot_wait_stopped, + # Include initialization and stopping behaviors as they represent intentional waiting states bs.int_app_init, bs.int_app_to_stop, ]code/planning/src/behavior_agent/behaviors/overtake.py (4)
2-26: Consider organizing imports by category.The imports could be better organized by grouping standard library, ROS, and local imports.
-import sys -import os -from std_msgs.msg import String, Float32 - -import rospy -import numpy as np - -from behaviors import behavior_speed as bs -import mapping_common.map -import mapping_common.mask -import mapping_common.entity -from mapping_common.map import Map -import shapely -from mapping_common.entity import FlagFilter -from mapping_common.entity import Pedestrian -from visualization_msgs.msg import Marker, MarkerArray -import behaviors.behavior_utils - -sys.path.append(os.path.abspath(sys.path[0] + "/..")) -# from behavior_utils import get_hero_width, get_marker_arr_in_front -from local_planner.utils import ( - NUM_WAYPOINTS, - TARGET_DISTANCE_TO_STOP_OVERTAKE, -) # type: ignore # noqa: E402 +# Standard library imports +import os +import sys + +# ROS imports +import rospy +import numpy as np +from std_msgs.msg import String, Float32 +from visualization_msgs.msg import Marker, MarkerArray + +# Local imports +from behaviors import behavior_speed as bs +import behaviors.behavior_utils +import mapping_common.map +import mapping_common.mask +import mapping_common.entity +from mapping_common.map import Map +import shapely +from mapping_common.entity import FlagFilter, Pedestrian + +# Add local planner to path +sys.path.append(os.path.abspath(sys.path[0] + "/..")) +from local_planner.utils import ( + NUM_WAYPOINTS, + TARGET_DISTANCE_TO_STOP_OVERTAKE, +) # type: ignore # noqa: E402
107-111: Combine nested if statements.The nested if statements can be combined using
andfor better readability.- wp = self.blackboard.get("/paf/hero/waypoint_distance") - if wp is not None: - if wp.isStopLine and wp.distance < 20: - rospy.logerr("Not overtaking in intersection!") - return py_trees.common.Status.FAILURE + wp = self.blackboard.get("/paf/hero/waypoint_distance") + if wp is not None and wp.isStopLine and wp.distance < 20: + rospy.logerr("Not overtaking in intersection!") + return py_trees.common.Status.FAILURE🧰 Tools
🪛 Ruff (0.8.2)
107-108: Use a single
ifstatement instead of nestedifstatementsCombine
ifstatements usingand(SIM102)
206-214: Add a constant for the speed and counter thresholds.The speed (2.5) and counter (4) thresholds are hardcoded. Consider extracting them as named constants.
+# Constants for overtake detection +OBSTACLE_SPEED_THRESHOLD = 2.5 # m/s +OBSTACLE_DISTANCE_THRESHOLD = 15.0 # meters +OVERTAKE_COUNTER_THRESHOLD = 4 # number of consecutive detections + - if obstacle_speed < 2.5 and obstacle_distance < 15: + if obstacle_speed < OBSTACLE_SPEED_THRESHOLD and obstacle_distance < OBSTACLE_DISTANCE_THRESHOLD: self.counter_overtake += 1 rospy.loginfo(f"Obstacle distance: {obstacle_distance}") rospy.loginfo("Overtake counter: " + str(self.counter_overtake)) - if self.counter_overtake > 4: + if self.counter_overtake > OVERTAKE_COUNTER_THRESHOLD: self.ot_distance_pub.publish(obstacle_distance) return py_trees.common.Status.SUCCESS self.old_obstacle_distance = obstacle_distance
619-623: Consider extracting lane check parameters to constants.The lane check parameters (0.5, 15.0, -4.0) are hardcoded. Consider extracting them as named constants for better maintainability.
+# Constants for lane checks +OBSTACLE_STATIONARY_SPEED = 0.5 # m/s +LANE_CHECK_DISTANCE_STATIONARY = 15.0 # meters +LANE_CHECK_DISTANCE_MOVING = -4.0 # meters + - if obstacle_speed < 0.5: - ot_free = tree.is_lane_free(False, self.clear_distance, 15.0) + if obstacle_speed < OBSTACLE_STATIONARY_SPEED: + ot_free = tree.is_lane_free(False, self.clear_distance, LANE_CHECK_DISTANCE_STATIONARY) else: - ot_free = tree.is_lane_free(False, 15.0, -4.0) + ot_free = tree.is_lane_free(False, LANE_CHECK_DISTANCE_STATIONARY, LANE_CHECK_DISTANCE_MOVING)code/planning/src/local_planner/motion_planning.py (1)
513-520: Improve error handling for overtake initialization.The code checks for infinite distance but doesn't validate other edge cases.
Consider adding validation for:
- Negative distances
- Zero distances
- NaN values
if data.data == bs.ot_enter_init.name: - if np.isinf(self.__ot_distance): + if np.isinf(self.__ot_distance) or np.isnan(self.__ot_distance) or self.__ot_distance <= 0: self.__overtake_status = -1 self.overtake_success_pub.publish(self.__overtake_status) return self.change_trajectory(self.__ot_distance)
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (6)
code/planning/src/behavior_agent/behaviors/behavior_speed.py(1 hunks)code/planning/src/behavior_agent/behaviors/behavior_utils.py(1 hunks)code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)code/planning/src/behavior_agent/behaviors/unstuck_routine.py(1 hunks)code/planning/src/local_planner/motion_planning.py(9 hunks)code/planning/src/local_planner/utils.py(1 hunks)
🧰 Additional context used
🪛 Ruff (0.8.2)
code/planning/src/behavior_agent/behaviors/overtake.py
107-108: Use a single if statement instead of nested if statements
Combine if statements using and
(SIM102)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (7)
code/planning/src/local_planner/utils.py (2)
18-18: LGTM: Increased waypoints for smoother trajectory.Increasing the number of waypoints from 7 to 9 should provide better trajectory control during overtaking maneuvers.
15-16: Verify the stopping distance difference for overtaking.The overtaking stopping distance (7m) is larger than the regular stopping distance (5m). While this makes sense to give more room for maneuvering during overtaking, please verify that this difference is sufficient for safe overtaking at various speeds.
✅ Verification successful
🏁 Script executed:
#!/bin/bash # Description: Search for speed-related calculations in overtaking code # to verify if the stopping distances are appropriate. # Look for speed calculations in overtaking-related code rg -A 5 "speed.*overtake|overtake.*speed" --type pyLength of output: 3226
Verification of Stopping Distance Constants and Their Use in Overtaking Logic
- The regular stopping distance is defined as 5 m and the overtaking stopping distance is defined as 7 m in
code/planning/src/local_planner/utils.py.- In the overtaking speed calculation (
__calc_speed_to_stop_overtakeincode/planning/src/local_planner/motion_planning.py), the code checks if the computed stopline is less thanTARGET_DISTANCE_TO_STOP_OVERTAKE(7 m) and, if so, sets the target speed to 2.5 m/s. This indicates that a larger buffer is enforced during overtaking maneuvers.- The logic shows a deliberate design choice to allow an extra 2 m of stopping distance for overtaking, which is intended to provide a safety margin for maneuvering at varying speeds.
- While the static analysis confirms that the constants are correctly used, assessing whether a 7 m threshold is physically sufficient for safe overtaking at all speeds would require empirical validation (e.g., simulation or real-world testing).
code/planning/src/behavior_agent/behaviors/behavior_utils.py (1)
14-29: LGTM! Good error handling.The function properly handles cases where hero or motion data is unavailable, and provides a sensible minimum width of 1.0.
code/planning/src/behavior_agent/behaviors/overtake.py (1)
195-203: Add null check before accessing entity for visualization.The code assumes
entityis not None when generating the visualization marker.code/planning/src/local_planner/motion_planning.py (3)
22-22: LGTM! New constant for overtake stopping distance.The addition of
TARGET_DISTANCE_TO_STOP_OVERTAKEaligns with the PR objectives to rework the overtaking routine.
701-704: LGTM! Improved distance calculation for overtaking.The changes to virtual overtake calculation:
- Use new
__ot_distanceinstead of collision point- Add a 2.5m buffer for safety
283-300: Verify the trajectory generation parameters.The changes to waypoint selection in the overtake trajectory generation include:
- Dynamic offset based on distance/2
- Additional buffer points (+2)
These changes could affect the smoothness of the overtaking maneuver.
Run this script to analyze the trajectory generation parameters:
There was a problem hiding this comment.
Actionable comments posted: 0
🧹 Nitpick comments (3)
code/planning/src/behavior_agent/behaviors/overtake.py (3)
34-34: Consider replacing global variable with class property.Using global variables for state management can lead to maintainability issues and make the code harder to test. Consider encapsulating
OVERTAKE_FREEwithin a class or using a shared state object.
197-198: Extract magic numbers into named constants.The speed threshold (2.5) and distance threshold (15) should be defined as named constants at the module level for better maintainability.
+# Constants for overtake conditions +OBSTACLE_SPEED_THRESHOLD = 2.5 # m/s +OBSTACLE_DISTANCE_THRESHOLD = 15.0 # meters - if obstacle_speed < 2.5 and obstacle_distance < 15: + if obstacle_speed < OBSTACLE_SPEED_THRESHOLD and obstacle_distance < OBSTACLE_DISTANCE_THRESHOLD:
618-621: Document bicycle speed thresholds and extract magic numbers.The bicycle speed check uses undocumented magic numbers. Add comments explaining the speed thresholds and extract them into named constants.
+# Constants for bicycle handling +BICYCLE_MIN_SPEED = 1.7 # m/s +BICYCLE_MAX_SPEED = 2.4 # m/s +BICYCLE_CLEAR_DISTANCE = 15.0 # meters +BICYCLE_CLEAR_WIDTH = -4.0 # meters - if obstacle_speed > 1.7 and obstacle_speed < 2.4: - ot_free = tree.is_lane_free(False, 15.0, -4.0) + # Special handling for bicycles based on typical speed range + if obstacle_speed > BICYCLE_MIN_SPEED and obstacle_speed < BICYCLE_MAX_SPEED: + ot_free = tree.is_lane_free(False, BICYCLE_CLEAR_DISTANCE, BICYCLE_CLEAR_WIDTH)
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (4)
code/planning/src/behavior_agent/behaviors/overtake.py (4)
186-194: Add null check before accessing entity for visualization.The code assumes
entityis not None when generating the visualization marker.
387-395: Extract duplicate marker visualization logic into a helper function.
314-364: Extract duplicate data preparation logic into a helper function.
107-113: Great improvements in error handling and logging!The addition of detailed error logging and proper handling of edge cases (missing map data, invalid trajectory masks) improves the system's robustness and debuggability.
Also applies to: 146-148, 345-347, 572-574
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (1)
code/planning/src/behavior_agent/behaviors/overtake.py (1)
16-16: Remove unused import.The
Pedestrianimport is not used in the code.Apply this diff to remove the unused import:
-from mapping_common.entity import Pedestrian🧰 Tools
🪛 Ruff (0.8.2)
16-16:
mapping_common.entity.Pedestrianimported but unusedRemove unused import:
mapping_common.entity.Pedestrian(F401)
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)code/planning/src/local_planner/motion_planning.py(9 hunks)
🧰 Additional context used
📓 Learnings (1)
code/planning/src/behavior_agent/behaviors/overtake.py (1)
Learnt from: Zelberor
PR: una-auxme/paf#659
File: code/planning/src/behavior_agent/behaviors/overtake.py:171-174
Timestamp: 2025-02-05T17:35:21.384Z
Learning: In the overtake behavior, marker generation for detected entities should be placed outside motion data checks to ensure visualization of all entities, regardless of their motion data availability.
🪛 Ruff (0.8.2)
code/planning/src/behavior_agent/behaviors/overtake.py
16-16: mapping_common.entity.Pedestrian imported but unused
Remove unused import: mapping_common.entity.Pedestrian
(F401)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (3)
code/planning/src/local_planner/motion_planning.py (1)
117-122:⚠️ Potential issueInitialize
__ot_distancein constructor.The
__ot_distanceattribute is used but not initialized in the constructor.Add initialization in the constructor:
def __init__(self): # ... existing code ... self.unstuck_distance = None self.unstuck_overtake_flag = False self.init_overtake_pos = None + self.__ot_distance = np.inf # Initialize with infinityLikely invalid or redundant comment.
code/planning/src/behavior_agent/behaviors/overtake.py (2)
184-192: Extract duplicate marker visualization logic into a helper function.This marker visualization code is duplicated across multiple classes.
Extract the marker generation code into a helper function:
+def publish_obstacle_marker(entity, marker_pub): + """Publish visualization marker for detected obstacle.""" + if entity is not None: + ot_marker = entity.to_marker() + ot_marker.color.r = 1.0 + ot_marker.color.g = 0 + ot_marker.color.b = 0 + ot_marker.header.frame_id = "hero" + ot_marker.header.stamp = rospy.Time.now() + ot_marker.lifetime = rospy.Duration(0.3) + marker_pub.publish(ot_marker) - # generate visualization marker for obstacle - ot_marker = entity.to_marker() - ot_marker.color.r = 1.0 - ot_marker.color.g = 0 - ot_marker.color.b = 0 - ot_marker.header.frame_id = "hero" - ot_marker.header.stamp = rospy.Time.now() - ot_marker.lifetime = rospy.Duration(0.3) - self.ot_marker_pub.publish(ot_marker) + publish_obstacle_marker(entity, self.ot_marker_pub)
616-619: Verify bicycle handling logic.The code uses a different distance check for bicycles (speed between 1.7 and 2.4). This seems like a special case that needs verification.
Could you confirm if this speed range is appropriate for bicycles and whether they should be handled differently from other obstacles?
| def __calc_speed_to_stop_overtake(self) -> float: | ||
| stopline = self.__calc_virtual_overtake() | ||
| v_stop = max(convert_to_ms(10.0), convert_to_ms(stopline / 0.8)) | ||
| if stopline < TARGET_DISTANCE_TO_STOP: | ||
| v_stop = 0.0 | ||
| v_stop = min(convert_to_ms(9.0), convert_to_ms(stopline / 1.4)) | ||
| if stopline < TARGET_DISTANCE_TO_STOP_OVERTAKE: | ||
| v_stop = 2.5 | ||
|
|
There was a problem hiding this comment.
💡 Verification agent
🧩 Analysis chain
Add test coverage for overtaking speed parameters.
The changes to speed calculation include:
- Modified speed scaling (1.4 instead of 0.8)
- Different minimum speed (2.5 instead of 0)
- New maximum speed limit (9.0)
These parameters are crucial for safe overtaking but lack test coverage.
Run this script to check for any test coverage of these parameters:
🏁 Script executed:
#!/bin/bash
# Description: Check test coverage for overtaking speed parameters
# Search for test files that verify overtaking speeds
rg -A 5 "calc_speed_to_stop_overtake|TARGET_DISTANCE_TO_STOP_OVERTAKE" --type pythonLength of output: 117
🏁 Script executed:
#!/bin/bash
# Description: Check test coverage for overtaking speed parameters in Python files using the correct file type.
rg -A 5 "calc_speed_to_stop_overtake|TARGET_DISTANCE_TO_STOP_OVERTAKE" --type pyLength of output: 4167
Missing test coverage for overtaking speed parameters.
The search results indicate that while the constant TARGET_DISTANCE_TO_STOP_OVERTAKE and the __calc_speed_to_stop_overtake method are referenced across multiple production files (e.g., in motion_planning.py and overtake.py), there is no dedicated test case or test file providing validation for these parameters. It's important to add tests that:
- Validate the speed scaling method (using a factor of 1.4 and ensuring the min speed of 2.5 is applied properly).
- Check that the computed speed does not exceed the converted maximum speed limit (9.0 m/s).
- Ensure that the condition involving
TARGET_DISTANCE_TO_STOP_OVERTAKEbehaves as expected.
There was a problem hiding this comment.
Actionable comments posted: 0
♻️ Duplicate comments (1)
code/planning/src/behavior_agent/behaviors/overtake.py (1)
183-191:⚠️ Potential issueAdd null check before accessing entity for visualization.
The code assumes
entityis not None when generating the visualization marker. Add a null check to prevent potential crashes.
🧹 Nitpick comments (4)
code/planning/src/behavior_agent/behaviors/overtake.py (4)
33-33: Consider using a state manager instead of global variables.Using global variables for state management can lead to maintainability issues and make the code harder to test. Consider implementing a state manager class to handle the overtaking state.
Example implementation:
class OvertakeStateManager: def __init__(self): self._is_free = False self._executing = 0 @property def is_free(self): return self._is_free @is_free.setter def is_free(self, value): self._is_free = value @property def executing(self): return self._executing @executing.setter def executing(self, value): self._executing = value
194-202: Consider parameterizing the counter threshold.The counter threshold for overtake initiation is hardcoded. Consider making it configurable through ROS parameters for easier tuning.
Example implementation:
- if self.counter_overtake > 4: + self.counter_threshold = rospy.get_param('~overtake_counter_threshold', 4) + if self.counter_overtake > self.counter_threshold:
280-281: Parameterize distance thresholds.The clear distance threshold is hardcoded. Consider making it configurable through ROS parameters.
Example implementation:
- self.clear_distance = 35 + self.clear_distance = rospy.get_param('~overtake_clear_distance', 35.0)
615-618: Parameterize bicycle speed thresholds.The bicycle speed range (1.7-2.4) is hardcoded. Consider making these thresholds configurable through ROS parameters and adding comments explaining the chosen values.
Example implementation:
- if obstacle_speed > 1.7 and obstacle_speed < 2.4: + min_bicycle_speed = rospy.get_param('~min_bicycle_speed', 1.7) + max_bicycle_speed = rospy.get_param('~max_bicycle_speed', 2.4) + # Speed range typical for bicycles in urban environments + if obstacle_speed > min_bicycle_speed and obstacle_speed < max_bicycle_speed:
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
code/planning/src/behavior_agent/behaviors/overtake.py(11 hunks)
🧰 Additional context used
📓 Learnings (1)
code/planning/src/behavior_agent/behaviors/overtake.py (1)
Learnt from: Zelberor
PR: una-auxme/paf#659
File: code/planning/src/behavior_agent/behaviors/overtake.py:171-174
Timestamp: 2025-02-05T17:35:21.384Z
Learning: In the overtake behavior, marker generation for detected entities should be placed outside motion data checks to ensure visualization of all entities, regardless of their motion data availability.
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (2)
code/planning/src/behavior_agent/behaviors/overtake.py (2)
65-73: LGTM! Publishers are well-organized.The publishers for distance and markers are properly initialized with appropriate topics and queue sizes.
394-421: LGTM! Robust lane availability check.The implementation uses a counter to handle inconsistencies in lane availability data, which is a good practice for dealing with sensor noise and intermittent detection issues.
Description
Overtake routine has been reworked to make use of intermediate layer.
Overtake obstacles are now detected based on the trajectory. If the nearest obstacle in x-direction is not moving,
a counter gets incremented. Once this exceeds a threshold, the Overtake behaviour starts.
Executing the overtake is now only allowed if left lane is free. Also based on a counter.
While in Approach the car gets slowed down and checks for a free lane. If it's free before we reach the
obstacle, wait gets skipped and the Overtake execution begins.
Wait handles standing in front of the obstacle and wating for a free lane.
A marker for visualizing the nearest entity on the trajectory in Overtake Ahead has been added as well.
Fixes # 638
Type of change
Please delete options that are not relevant.
Does this PR introduce a breaking change?
No
Most important changes
Overtake Ahead, Approach and Wait were mainly changed by this.
Checklist:
Summary by CodeRabbit
New Features
Bug Fixes
Chores