532 feature remodel acc first iteration #603
Conversation
…ities was chosen by the minimum function
…ue instead of none
…ities was chosen by the minimum function
…ue instead of none
Merge branch '532-feature-remodel-acc-first-iteration' of https://github.com/una-auxme/paf into 532-feature-remodel-acc-first-iteration
…sholds implemented
WalkthroughThis pull request enhances the Adaptive Cruise Control (ACC) system by integrating radar data for more precise lead vehicle tracking. The changes introduce a new subscriber to receive radar information about the lead vehicle's distance and velocity. The ACC module now uses these radar-derived measurements to calculate safe speeds, replacing the previous obstacle-based approach. The motion planning component has also been updated to track and publish the source of target velocity. Changes
Sequence DiagramsequenceDiagram
participant Radar
participant ACC
participant MotionPlanning
Radar->>ACC: Publish lead vehicle data
ACC->>ACC: Process radar data
ACC->>ACC: Calculate safe speed
ACC->>MotionPlanning: Send target speed
MotionPlanning->>MotionPlanning: Update velocity selector
MotionPlanning->>ROS: Publish velocity selector
Possibly related issues
Possibly related PRs
Suggested labels
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: 3
🧹 Nitpick comments (4)
code/planning/src/local_planner/ACC_old.py (1)
215-219: Add validation for obstacle data before calculationsIn the control loop, there is no check to ensure that
self.obstacle_distanceandself.obstacle_speedare valid numerical values (not NaN or infinite). To enhance robustness, validate these variables before using them in calculations.Apply this diff to add validation:
if ( self.obstacle_distance is not None and self.obstacle_speed is not None and self.__current_velocity is not None + and not np.isnan(self.obstacle_distance) + and not np.isnan(self.obstacle_speed) + and not np.isinf(self.obstacle_distance) + and not np.isinf(self.obstacle_speed) ):code/planning/src/local_planner/ACC.py (1)
115-118: Consider removing obsolete obstacle variablesWith the integration of radar data,
self.obstacle_speedandself.obstacle_distanceappear to be unused. Removing these variables can clean up the code and reduce confusion.Apply this diff to remove unused variables:
-# Distance and speed from possible collision object -self.obstacle_speed: float = 10 -self.obstacle_distance: float = 50code/planning/src/local_planner/motion_planning.py (1)
492-498: Consider using a dictionary for speed-to-selector mapping.The current if-elif chain could be replaced with a more maintainable mapping solution.
- if self.target_speed == acc_speed: - self.target_velocity_selector = "acc_speed" - # be speed is sometimes equals acc speed (in case of cruise behaviour) - elif self.target_speed == be_speed: - self.target_velocity_selector = "be_speed" - elif self.target_speed == corner_speed: - self.target_velocity_selector = "corner_speed" + speed_to_selector = { + acc_speed: "acc_speed", + be_speed: "be_speed", + corner_speed: "corner_speed" + } + self.target_velocity_selector = speed_to_selector.get(self.target_speed, "not selected")doc/planning/ACC.md (1)
31-31: Enhance documentation clarity.
- Add a period at the end of the line for consistency.
- Consider adding more details about the data format and units for the range and velocity values.
-/paf/hero/Radar/lead_vehicle/range_velocity_array`: Distance to the vehicle in front, relative speed of the vehicle in front +/paf/hero/Radar/lead_vehicle/range_velocity_array`: Distance to the vehicle in front (meters) and relative speed of the vehicle in front (m/s).🧰 Tools
🪛 LanguageTool
[uncategorized] ~31-~31: Loose punctuation mark.
Context: ...Radar/lead_vehicle/range_velocity_array`: Distance to the vehicle in front, relat...(UNLIKELY_OPENING_PUNCTUATION)
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (6)
.vscode/launch.json(1 hunks)code/planning/launch/planning.launch(1 hunks)code/planning/src/local_planner/ACC.py(3 hunks)code/planning/src/local_planner/ACC_old.py(1 hunks)code/planning/src/local_planner/motion_planning.py(4 hunks)doc/planning/ACC.md(2 hunks)
🧰 Additional context used
🪛 LanguageTool
doc/planning/ACC.md
[uncategorized] ~31-~31: Loose punctuation mark.
Context: ...Radar/lead_vehicle/range_velocity_array`: Distance to the vehicle in front, relat...
(UNLIKELY_OPENING_PUNCTUATION)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (7)
code/planning/src/local_planner/ACC_old.py (1)
103-104:⚠️ Potential issueCorrect the type annotation for
self.obstacle_speedThe variable
self.obstacle_speedis annotated astuple, but it is assigned a single float value in the__collision_callbackmethod. Update the type annotation tofloatfor consistency and to prevent potential type errors.Apply this diff to correct the type:
-self.obstacle_speed: tuple = None +self.obstacle_speed: float = NoneLikely invalid or redundant comment.
code/planning/src/local_planner/ACC.py (2)
246-249: Verify the handling of negativeleading_vehicle_speedSetting
acc_speedto0.0whenself.leading_vehicle_speed < 0.0may not be appropriate. A negativeleading_vehicle_speedmight indicate that the lead vehicle is moving towards the ego vehicle. Ensure that this logic correctly reflects the intended behavior of the ACC system.
260-261: Review the calculation ofsafe_speedThe formula for
safe_speeduses the ratio ofleading_vehicle_distancetosafety_distance. Confirm that this calculation aligns with the desired ACC behavior and considers edge cases, such as whensafety_distanceis zero.code/planning/src/local_planner/motion_planning.py (2)
44-44: LGTM! Clean initialization of new velocity selector components.The initialization of the
target_velocity_selectorattribute and its corresponding publisher follows ROS conventions and best practices.Also applies to: 166-169
720-720: LGTM! Good error handling in control loop.Publishing a default value when conditions aren't met ensures the selector state is always communicated.
.vscode/launch.json (1)
22-35: LGTM! Well-structured debug configuration.The new debug configuration for port 53001 follows VS Code conventions and maintains consistency with the existing configuration.
doc/planning/ACC.md (1)
5-8: LGTM! Improved documentation structure.The reorganized section headers enhance readability and navigation.
There was a problem hiding this comment.
Overall code looks good, some things were noted to look again.
No test of functioning code was accomplished by me as it's still weekend.
@THDThieleT will go to the lab tomorrow morning and do a function check and also maybe furthermore review.
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (3)
code/planning/src/local_planner/motion_planning.py (1)
44-44: Consider using an enumeration for velocity selector values.Using string literals for velocity selector values could lead to maintenance issues and typos. Consider defining an enumeration to represent these values.
Example implementation:
from enum import Enum, auto class VelocitySelector(Enum): NOT_SELECTED = "not selected" ACC_SPEED = "acc_speed" BE_SPEED = "be_speed" CORNER_SPEED = "corner_speed" UNKNOWN = "unknown"Then use it like:
self.target_velocity_selector = VelocitySelector.NOT_SELECTED.value # For initialization # In update_target_speed: if self.target_speed == acc_speed: self.target_velocity_selector = VelocitySelector.ACC_SPEED.valueAlso applies to: 492-498
code/planning/src/local_planner/ACC.py (2)
110-110: Consider using Optional[float] for clarity.Since these variables can be None, using
Optional[float]would more accurately represent their type.- self.obstacle_speed: float = None - self.obstacle_distance: float = None + self.obstacle_speed: Optional[float] = None + self.obstacle_distance: Optional[float] = NoneDon't forget to add
from typing import Optionalat the top of the file.Also applies to: 112-112
238-249: Simplify condition checks and handle edge cases.The comment suggests timing issues with None checks. Consider restructuring to handle edge cases more gracefully:
- Move safety distance calculation outside the if block
- Add early returns for invalid states
+ # Calculate safety distance once + safety_distance = calculate_rule_of_thumb( + False, self.__current_velocity + ) if self.__current_velocity is not None else None + + # Early return for invalid states + if self.__current_velocity is None: + self.velocity_pub.publish(0) + return + if ( - # often none self.leading_vehicle_distance is not None - # often none -> often does elif even if if-case is necessary and self.leading_vehicle_speed is not None - and self.__current_velocity is not None ):
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
.vscode/launch.json(1 hunks)code/planning/src/local_planner/ACC.py(3 hunks)code/planning/src/local_planner/motion_planning.py(4 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- .vscode/launch.json
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (6)
code/planning/src/local_planner/motion_planning.py (3)
44-44: LGTM! Good initialization value.The
target_velocity_selectoris correctly initialized with a descriptive default value "not selected".
166-169: LGTM! Publisher initialization follows ROS conventions.The publisher is correctly initialized with appropriate topic name and QoS profile.
720-720: Consider using the initial value for consistency.The control loop publishes "0" when behavior, ACC speed, or corners are not set, but this differs from the initial value "not selected" set in line 44.
- self.velocity_selector_pub.publish("0") + self.velocity_selector_pub.publish("not selected")code/planning/src/local_planner/ACC.py (3)
76-82: LGTM! Well-structured addition of radar data handling.The new subscriber and attributes are properly initialized and follow the existing patterns in the code.
Also applies to: 115-118
122-135: 🛠️ Refactor suggestionEnhance radar data validation.
While the length check is good, consider adding these additional validations:
- Check for NaN/infinite values in radar data
- Validate self.__current_velocity before calculating leading_vehicle_speed
def __update_radar_data(self, data: Float32MultiArray): - if not data.data or len(data.data) < 2: + if (not data.data or len(data.data) < 2 or + np.isinf(data.data[0]) or np.isnan(data.data[0]) or + np.isinf(data.data[1]) or np.isnan(data.data[1])): # no distance and speed data of the leading vehicle is transferred # (leading vehicle is very far away) self.leading_vehicle_distance = None self.leading_vehicle_relative_speed = None self.leading_vehicle_speed = None else: self.leading_vehicle_distance = data.data[0] self.leading_vehicle_relative_speed = data.data[1] + if self.__current_velocity is not None: self.leading_vehicle_speed = ( self.__current_velocity + self.leading_vehicle_relative_speed ) + else: + self.leading_vehicle_speed = NoneLikely invalid or redundant comment.
252-261: 🛠️ Refactor suggestionConsider adding bounds checking for safety calculations.
The safety distance and speed calculations should include bounds checking to prevent potential issues:
- Ensure safety_distance is positive
- Add minimum threshold for leading_vehicle_distance
- safety_distance: float - safety_distance = calculate_rule_of_thumb( - False, self.__current_velocity - ) + if safety_distance <= 0: + self.velocity_pub.publish(0) + return + + # Ensure minimum safety distance + if self.leading_vehicle_distance < 0.5: # minimum 0.5m + self.velocity_pub.publish(0) + return + if self.leading_vehicle_distance < safety_distance:
There was a problem hiding this comment.
Actionable comments posted: 0
♻️ Duplicate comments (1)
code/planning/src/local_planner/motion_planning.py (1)
492-498:⚠️ Potential issueAdd a default case for target velocity selector.
The current implementation doesn't handle the case where
target_speeddoesn't match any of the compared speeds. This could lead to inconsistent selector values.Apply this diff to handle all cases:
if self.target_speed == acc_speed: self.target_velocity_selector = "acc_speed" - # be speed is sometimes equals acc speed (in case of cruise behaviour) elif self.target_speed == be_speed: self.target_velocity_selector = "be_speed" elif self.target_speed == corner_speed: self.target_velocity_selector = "corner_speed" + else: + self.target_velocity_selector = "unknown" # Handle unexpected cases
🧹 Nitpick comments (1)
code/planning/src/local_planner/motion_planning.py (1)
Line range hint
492-720: Consider using an Enum for selector values.The current implementation uses string literals for selector values, which could lead to typos and maintenance issues. Consider using an Enum to enforce type safety and maintainability.
Example implementation:
from enum import Enum, auto class VelocitySelector(Enum): NOT_SELECTED = "not selected" ACC_SPEED = "acc_speed" BE_SPEED = "be_speed" CORNER_SPEED = "corner_speed" UNKNOWN = "unknown"Then update the usage:
-self.target_velocity_selector = "not selected" +self.target_velocity_selector = VelocitySelector.NOT_SELECTED.value -if self.target_speed == acc_speed: - self.target_velocity_selector = "acc_speed" +if self.target_speed == acc_speed: + self.target_velocity_selector = VelocitySelector.ACC_SPEED.value
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
code/planning/src/local_planner/motion_planning.py(4 hunks)
⏰ Context from checks skipped due to timeout of 90000ms (1)
- GitHub Check: Build and push image
🔇 Additional comments (4)
code/planning/src/local_planner/motion_planning.py (4)
44-44: LGTM: Good initialization of the selector.The initial value "not selected" is appropriate and consistent with the usage throughout the code.
166-169: LGTM: Publisher initialization follows ROS conventions.The publisher is correctly initialized with appropriate topic name and QoS profile.
501-501: LGTM: Consistent publishing of the selector value.The selector value is published alongside the target speed, maintaining synchronization.
720-720: LGTM: Appropriate default selector value.When required conditions are not met, the selector is correctly set to "not selected".
Description
In this PR, the ACC was improved using the new radar data provided by #579. Apart from that, the old structure was kept up.
Fixes # 532
Type of change
Please delete options that are not relevant.
Does this PR introduce a breaking change?
No
Most important changes
Which files functionalities are most important in this PR. On which part should the reviewer be focussed on?
ACC.py
Checklist:
Summary by CodeRabbit
New Features
Documentation
Bug Fixes