383 planning evaluate and document motion planning #418
Conversation
WalkthroughThe changes include significant modifications to the Changes
Suggested labels
Suggested reviewers
Poem
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: 8
🧹 Outside diff range and nitpick comments (9)
doc/planning/motion_planning.md (4)
3-4: Fix typo and improve file path reference.There are a few minor issues in the summary:
- "fowarded" should be "forwarded"
- The file path uses ellipsis which makes it harder to navigate. Consider using the full relative path.
-**Summary:** [motion_planning.py](.../code/planning/local_planner/src/motion_planning.py): +**Summary:** [motion_planning.py](../../../code/planning/local_planner/src/motion_planning.py): -The motion planning is responsible for collecting all the velocity recommendations from the different components and choosing the optimal one to be fowarded into the acting. +The motion planning is responsible for collecting all the velocity recommendations from the different components and choosing the optimal one to be forwarded into the acting.
16-16: Fix grammar and hyphenation.A few grammatical improvements are needed:
- "wether" should be "whether"
- "decision making" should be hyphenated as "decision-making"
-It also publishes a topic that indicates wether an overtake maneuver was successful or not. +It also publishes a topic that indicates whether an overtake maneuver was successful or not. -When the decision making node decides that an overtake maneuver is necessary, +When the decision-making node decides that an overtake maneuver is necessary,Also applies to: 21-21
🧰 Tools
🪛 LanguageTool
[uncategorized] ~16-~16: The preposition “whether” seems more likely in this position.
Context: ...t also publishes a topic that indicates wether an overtake maneuver was successful or ...(AI_EN_LECTOR_REPLACEMENT_PREPOSITION)
30-42: Add message types for subscribed topics.For consistency and completeness, please add message type information with documentation links for all subscribed topics, similar to how it's done in the published topics section. This helps developers understand the interface better.
Example format:
-`/carla/hero/Speed`: Subscribes to the current speed. +`/carla/hero/Speed`: Subscribes to the current speed. ([std_msgs/Float32](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html))Would you like me to help generate the complete list with proper message types?
🧰 Tools
🪛 LanguageTool
[uncategorized] ~30-~30: Loose punctuation mark.
Context: ...following topics: -/carla/hero/Speed: Subscribes to the current speed. - `/pa...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~31-~31: Loose punctuation mark.
Context: ...urrent speed. -/paf/hero/acc_velocity: Subscribes to the speed published by th...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~33-~33: Loose punctuation mark.
Context: ...bound in pixels. -/paf/hero/collision: Subscribes to the collision published b...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~34-~34: Loose punctuation mark.
Context: ...ision Check. -/paf/hero/curr_behavior: Subscribes to Current Behavior pubished...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~34-~34: The noun “Decision-Making” (= the process of deciding something) is spelled with a hyphen.
Context: ...bes to Current Behavior pubished by the Decision Making. -/paf/hero/current_heading: Subscri...(DECISION_MAKING)
[uncategorized] ~35-~35: Loose punctuation mark.
Context: ...on Making. -/paf/hero/current_heading: Subscribes to the filtered heading of t...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~36-~36: Loose punctuation mark.
Context: ...e ego vehicle. -/paf/hero/current_pos: Subscribes to the filtered position of ...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~37-~37: Loose punctuation mark.
Context: ...ion of our car. -/paf/hero/current_wp: Subscribes to the current waypoint. - `...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~38-~38: Loose punctuation mark.
Context: ...oint. -/paf/hero/lane_change_distance: Subscribes to the Carla Waypoint to che...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~39-~39: Loose punctuation mark.
Context: ...a lane change. -/paf/hero/speed_limit: Subscribes to the speed limit. - `/paf/...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~40-~40: Loose punctuation mark.
Context: ...d limit. -/paf/hero/trajectory_global: Subscribes to the global trajectory, wh...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~41-~41: Loose punctuation mark.
Context: ...mulation. -/paf/hero/unstuck_distance: Subscribes to the distance travelled by...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~42-~42: Loose punctuation mark.
Context: ...aneuver. -/paf/hero/waypoint_distance: Subscribes to the Carla Waypoint to get...(UNLIKELY_OPENING_PUNCTUATION)
63-65: Improve debugging section structure and fix punctuation.The debugging section could be improved:
- Remove the unnecessary comma before "if"
- Consider adding more details about the debugging functionality
-Can spawn a car on the first straight in the dev environment, if this message is manually published. +Can spawn a car on the first straight in the dev environment if this message is manually published.Consider adding:
- The message type for the debugging topic
- Example usage or command to publish the message
- Expected behavior when the message is received
🧰 Tools
🪛 LanguageTool
[uncategorized] ~65-~65: Loose punctuation mark.
Context: ...bugging topics: -/paf/hero/Spawn_car: Can spawn a car on the first straight i...(UNLIKELY_OPENING_PUNCTUATION)
[typographical] ~65-~65: Usually, there’s no comma before “if”.
Context: ...he first straight in the dev environment, if this message is manually published.(IF_NO_COMMA)
doc/general/architecture.md (1)
Line range hint
1-24: Documentation enhancement suggestionsThe architecture documentation is well-structured, but could benefit from the following improvements:
Message Type Documentation:
- Consider adding links to ROS message documentation consistently across all message types
- Example: Add links for custom messages like
perception/Waypointandperception/LanecChangeComponent Interactions:
- Consider adding sequence diagrams or flow charts to illustrate the interaction between components
- This would be particularly helpful for understanding the relationship between Motion Planning and other Planning components
Visualization Section:
- Consider expanding the Visualization section with specific examples of what is being visualized
- Add details about visualization tools and interfaces used
Would you like assistance in implementing any of these suggestions?
Also applies to: 201-201
code/planning/src/local_planner/motion_planning.py (4)
163-165: Movewp_subssubscription to the subscribers section.The
wp_subssubscription is currently placed separately from the other subscribers. According to the TODO comment and for better code organization and readability, consider moving it up to the subscribers section where other subscription initializations are defined.
Line range hint
398-404: Simplifyeuclid_distfunction using NumPy'slinalg.norm.As per the TODO comment, replace the custom Euclidean distance calculation with NumPy's built-in function for better efficiency and readability.
Apply this diff:
def euclid_dist(vector1, vector2): - # TODO replace with numpy function - point1 = np.array(vector1) - point2 = np.array(vector2) - - diff = point2 - point1 - sum_sqrt = np.dot(diff.T, diff) - return np.sqrt(sum_sqrt) + return np.linalg.norm(np.array(vector1) - np.array(vector2))
440-447: Add type hints toconvert_pose_to_arrayparameters for clarity.Currently, the method's parameter
poseslacks a specific type hint for the elements it contains. Clarify the type of elements withinposesto enhance code maintainability and help static type checkers.Update the method signature:
- def convert_pose_to_array(poses: np.ndarray) -> np.ndarray: + def convert_pose_to_array(self, poses: np.ndarray[np.ndarray]) -> np.ndarray:
685-687: Improve the docstring of therunmethod for clarity.The current docstring can be updated to follow PEP 257 conventions by providing a one-line summary and a more detailed description if necessary. This enhances readability and helps maintain consistent documentation.
Here's a suggested docstring:
def run(self): - """ - Control loop that updates the target speed and publishes the target trajectory and speed over ROS topics. - """ + """Start the control loop to update and publish target speed and trajectory. + This method initializes a timer that periodically updates the target speed based on current behavior and ACC speed, + and publishes the target trajectory and speed over ROS topics. + """
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
📒 Files selected for processing (3)
code/planning/src/local_planner/motion_planning.py(15 hunks)doc/general/architecture.md(1 hunks)doc/planning/motion_planning.md(1 hunks)
🧰 Additional context used
🪛 Ruff
code/planning/src/local_planner/motion_planning.py
414-414: Found useless expression. Either assign it to a variable or remove it.
(B018)
🪛 LanguageTool
doc/planning/motion_planning.md
[uncategorized] ~16-~16: The preposition “whether” seems more likely in this position.
Context: ...t also publishes a topic that indicates wether an overtake maneuver was successful or ...
(AI_EN_LECTOR_REPLACEMENT_PREPOSITION)
[uncategorized] ~21-~21: The noun “decision-making” (= the process of deciding something) is spelled with a hyphen.
Context: ...d by the motion planning node. When the decision making node decides that an overtake maneuver ...
(DECISION_MAKING)
[uncategorized] ~21-~21: A comma may be missing after the conjunctive/linking adverb ‘Otherwise’.
Context: ...will adjust the trajectory accordingly. Otherwise the received trajectory is published wi...
(SENT_START_CONJUNCTIVE_LINKING_ADVERB_COMMA)
[uncategorized] ~30-~30: Loose punctuation mark.
Context: ...following topics: - /carla/hero/Speed: Subscribes to the current speed. - `/pa...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~31-~31: Loose punctuation mark.
Context: ...urrent speed. - /paf/hero/acc_velocity: Subscribes to the speed published by th...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~33-~33: Loose punctuation mark.
Context: ...bound in pixels. - /paf/hero/collision: Subscribes to the collision published b...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~34-~34: Loose punctuation mark.
Context: ...ision Check. - /paf/hero/curr_behavior: Subscribes to Current Behavior pubished...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~34-~34: The noun “Decision-Making” (= the process of deciding something) is spelled with a hyphen.
Context: ...bes to Current Behavior pubished by the Decision Making. - /paf/hero/current_heading: Subscri...
(DECISION_MAKING)
[uncategorized] ~35-~35: Loose punctuation mark.
Context: ...on Making. - /paf/hero/current_heading: Subscribes to the filtered heading of t...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~36-~36: Loose punctuation mark.
Context: ...e ego vehicle. - /paf/hero/current_pos: Subscribes to the filtered position of ...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~37-~37: Loose punctuation mark.
Context: ...ion of our car. - /paf/hero/current_wp: Subscribes to the current waypoint. - `...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~38-~38: Loose punctuation mark.
Context: ...oint. - /paf/hero/lane_change_distance: Subscribes to the Carla Waypoint to che...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~39-~39: Loose punctuation mark.
Context: ...a lane change. - /paf/hero/speed_limit: Subscribes to the speed limit. - `/paf/...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~40-~40: Loose punctuation mark.
Context: ...d limit. - /paf/hero/trajectory_global: Subscribes to the global trajectory, wh...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~41-~41: Loose punctuation mark.
Context: ...mulation. - /paf/hero/unstuck_distance: Subscribes to the distance travelled by...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~42-~42: Loose punctuation mark.
Context: ...aneuver. - /paf/hero/waypoint_distance: Subscribes to the Carla Waypoint to get...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~48-~48: Loose punctuation mark.
Context: ... topics: - /paf/hero/overtake_success: Publishes if an overtake was successful...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~65-~65: Loose punctuation mark.
Context: ...bugging topics: - /paf/hero/Spawn_car: Can spawn a car on the first straight i...
(UNLIKELY_OPENING_PUNCTUATION)
[typographical] ~65-~65: Usually, there’s no comma before “if”.
Context: ...he first straight in the dev environment, if this message is manually published.
(IF_NO_COMMA)
🔇 Additional comments (4)
doc/general/architecture.md (1)
201-201: LGTM: New subscription properly documented
The addition of the current_wp subscription is well-documented and consistent with the Motion Planning module's responsibilities.
code/planning/src/local_planner/motion_planning.py (3)
388-388: Verify if filtering sublists with only one point is intended.
The TODO comment suggests checking whether it's intended to filter out sublists with only one point. Ensure that this behavior is correct and doesn't exclude necessary data for subsequent processing.
562-564: Confirm correct handling of unstuck flag in trajectory generation.
In the __get_speed_unstuck method, when generating the overtake trajectory with unstuck=True, ensure that the generate_overtake_trajectory method properly adjusts the trajectory for the unstuck scenario.
489-492: Ensure overtake trajectory is correctly initiated in __set_curr_behavior.
When the current behavior is ot_enter_init, the method change_trajectory is called. Verify that self.__collision_point is properly set and that change_trajectory effectively generates the required overtake trajectory.
Run the following script to check the assignments of self.__collision_point:
✅ Verification successful
Overtake trajectory initialization is properly handled in __set_curr_behavior
The verification shows that the overtake trajectory system is correctly implemented:
-
self.__collision_pointis properly managed:- Initialized as
None - Updated via
__set_collision_pointcallback when collision data is received - Checked for validity (
np.isinf) before trajectory generation
- Initialized as
-
The overtake trajectory flow is correct:
- When
ot_enter_initbehavior is set, it checks collision point validity - If valid, calls
change_trajectorywith the collision point change_trajectorythen callsgenerate_overtake_trajectoryand updates status- Status is properly published via
overtake_success_pub
- When
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Find all assignments to `self.__collision_point`
# Test: Search for assignments. Expect to find where `self.__collision_point` is set.
rg --python --multiline-dotall 'self\.__collision_point\s*=.*'
Length of output: 475
Script:
#!/bin/bash
# Search for assignments to collision_point
rg "self\.__collision_point\s*="
# Search for the implementation of __set_curr_behavior method
ast-grep --pattern 'def __set_curr_behavior(self, $_):
$$$'
# Search for change_trajectory method implementation
ast-grep --pattern 'def change_trajectory(self, $_):
$$$'
# Search for overtake behavior related code
rg -A 5 "ot_enter_init"
Length of output: 5023
Script:
#!/bin/bash
# Search for generate_overtake_trajectory implementation
ast-grep --pattern 'def generate_overtake_trajectory(self, $_):
$$$'
# Search for collision point initialization and updates
rg -B 3 -A 3 "collision_point"
# Search for where collision point data is received
rg -B 3 -A 3 "data\.data\[0\]"
Length of output: 6161
Script:
#!/bin/bash
# Search for generate_overtake_trajectory implementation which was missed in the previous run
rg -A 10 "def generate_overtake_trajectory"
# Search for overtake_success_pub initialization and usage
rg -B 3 -A 3 "overtake_success_pub"
Length of output: 3214
niklasr22
requested review from
bilinskaA,
victor-42,
johannaschwarz,
Vroni27 and
fedor98
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Outside diff range and nitpick comments (2)
code/planning/src/local_planner/motion_planning.py (2)
164-168: Add type hints to class attributes.The TODO comment indicates missing type hints. Consider adding type hints to improve code maintainability and IDE support.
- # TODO: add type hints - self.target_speed = 0.0 - self.__curr_behavior = None - self.__acc_speed = 0.0 + self.target_speed: float = 0.0 + self.__curr_behavior: Optional[str] = None + self.__acc_speed: float = 0.0
Line range hint
347-358: Optimize corner point calculations using numpy vectorized operations.The TODO comment suggests using numpy functions. The current implementation using list comprehensions can be optimized.
- # TODO: refactor by using numpy functions - x_values = np.array([point[0] for point in coords]) - y_values = np.array([point[1] for point in coords]) - - angles = np.arctan2(np.diff(y_values), np.diff(x_values)) + differences = np.diff(coords, axis=0) + angles = np.arctan2(differences[:, 1], differences[:, 0])
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
📒 Files selected for processing (1)
code/planning/src/local_planner/motion_planning.py(15 hunks)
🧰 Additional context used
🪛 Ruff
code/planning/src/local_planner/motion_planning.py
421-421: Found useless expression. Either assign it to a variable or remove it.
(B018)
🔇 Additional comments (3)
code/planning/src/local_planner/motion_planning.py (3)
Line range hint 3-33: LGTM! Import organization and docstring improvements.
The reorganized imports and enhanced class docstring provide better clarity about the module's purpose and dependencies.
239-241: Verify trajectory generation before updating overtake status.
The overtake status is set to 1 immediately after calling generate_overtake_trajectory, without verifying if the trajectory generation was successful.
#!/bin/bash
# Search for error handling patterns in trajectory generation
rg -A 5 "generate_overtake_trajectory.*\{" | rg "if.*error|try.*except|raise"332-344: LGTM! Improved documentation and type hints.
The method's documentation and type hints have been significantly improved, making the code more maintainable.
niklasr22
deleted the
383-planning-evaluate-and-document-motion-planning
branch
Description
This updates the markdown documentation of the motion planning component as well as the actual code (doc strings). (Solves issue #383)
Checklist:
Summary by CodeRabbit
New Features
Documentation
Bug Fixes