DO NOT MERGE: Trying pure pursuit only. #563
Conversation
This commit was created in order to run full leaderboard test in the git action. Car is only steering with pure pursuit controller. I also added a fix for a known indexing Bug in the pure pursuit contoller. The fix is commented out, because it introduces a more severe Bug.
WalkthroughThe changes in the pull request enhance the Changes
Sequence Diagram(s)sequenceDiagram
participant Vehicle
participant Controller
participant Vector
Vehicle->>Controller: Request target point
Controller->>Controller: Calculate minimum distance
Controller->>Vector: Call rotate_vector_2d(vector, angle_rad)
Vector-->>Controller: Return rotated vector
Controller->>Controller: Check if position is ahead
Controller-->>Vehicle: Return target point index
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
🧹 Outside diff range and nitpick comments (3)
code/control/src/pure_pursuit_controller.py (3)
183-183: Use appropriate log level
loginfois being used for debug information. Consider usinglogdebugfor diagnostic output.- self.loginfo(str(min_dist_idx)) + self.logdebug(f"Selected target point index: {min_dist_idx}")
186-194: Add documentation and type hintsThe rotation implementation is correct, but would benefit from proper documentation and type hints.
- def rotate_vector_2d(self, vector, angle_rad): + def rotate_vector_2d(self, vector: np.ndarray, angle_rad: float) -> np.ndarray: + """Rotates a 2D vector by a given angle in radians. + + Args: + vector: A 2D vector as numpy array + angle_rad: Rotation angle in radians + + Returns: + The rotated vector as numpy array + """
186-202: Consider extracting geometric utilitiesThe geometric operations (
rotate_vector_2d,__is_ahead) could be moved to a separate utility class or module. This would:
- Improve testability
- Make the code more reusable
- Separate concerns between geometric operations and controller logic
Consider creating a
geometric_utils.pymodule with these operations as standalone functions or a utility class.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
📒 Files selected for processing (1)
code/control/src/pure_pursuit_controller.py(2 hunks)
🔇 Additional comments (1)
code/control/src/pure_pursuit_controller.py (1)
13-13: LGTM: Type import addition
The addition of Tuple import supports proper type hinting in the codebase.
| for i in range(self.__tp_idx, len(self.__path.poses)): # self.__tp_idx, | ||
| pose: PoseStamped = self.__path.poses[i] | ||
| # if not self.__is_ahead((pose.pose.position.x, pose.pose.position.y)): | ||
| # continue |
There was a problem hiding this comment.
Potential issue with loop range starting from self.__tp_idx
Starting the loop from self.__tp_idx could skip valid target points if tp_idx becomes incorrect. Consider adding a fallback to search the entire path if no suitable point is found in the forward range.
- for i in range(self.__tp_idx, len(self.__path.poses)): # self.__tp_idx,
+ # First try from current target point
+ min_dist_idx = self.__find_target_point(self.__tp_idx, len(self.__path.poses), ld)
+ # If no point found, search entire path
+ if min_dist_idx == -1:
+ min_dist_idx = self.__find_target_point(0, len(self.__path.poses), ld)Committable suggestion skipped: line range outside the PR's diff.
| def __is_ahead(self, pos: Tuple[float, float]): | ||
| x, y = pos | ||
| c_x, c_y = self.__position | ||
| to_car = np.array([x - c_x, y - c_y]) | ||
| heading = self.rotate_vector_2d(np.array([1.0, 0.0]), self.__heading) | ||
|
|
||
| return np.dot(to_car, heading) > 1 |
There was a problem hiding this comment.
🛠️ Refactor suggestion
Improve robustness and documentation of position check
The implementation uses a fixed threshold of 1 for the dot product check, which might not be appropriate for all scenarios.
- def __is_ahead(self, pos: Tuple[float, float]):
+ def __is_ahead(self, pos: Tuple[float, float]) -> bool:
+ """Determines if a point is ahead of the vehicle's current position.
+
+ The point is considered ahead if the dot product between the vector to the point
+ and the vehicle's heading vector is positive (angle < 90 degrees).
+
+ Args:
+ pos: Target position as (x, y) tuple
+
+ Returns:
+ bool: True if the point is ahead of the vehicle
+ """
x, y = pos
c_x, c_y = self.__position
to_car = np.array([x - c_x, y - c_y])
heading = self.rotate_vector_2d(np.array([1.0, 0.0]), self.__heading)
- return np.dot(to_car, heading) > 1
+ # Normalize vectors to ensure consistent comparison
+ to_car = to_car / np.linalg.norm(to_car)
+ heading = heading / np.linalg.norm(heading)
+ return np.dot(to_car, heading) > 0📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| def __is_ahead(self, pos: Tuple[float, float]): | |
| x, y = pos | |
| c_x, c_y = self.__position | |
| to_car = np.array([x - c_x, y - c_y]) | |
| heading = self.rotate_vector_2d(np.array([1.0, 0.0]), self.__heading) | |
| return np.dot(to_car, heading) > 1 | |
| def __is_ahead(self, pos: Tuple[float, float]) -> bool: | |
| """Determines if a point is ahead of the vehicle's current position. | |
| The point is considered ahead if the dot product between the vector to the point | |
| and the vehicle's heading vector is positive (angle < 90 degrees). | |
| Args: | |
| pos: Target position as (x, y) tuple | |
| Returns: | |
| bool: True if the point is ahead of the vehicle | |
| """ | |
| x, y = pos | |
| c_x, c_y = self.__position | |
| to_car = np.array([x - c_x, y - c_y]) | |
| heading = self.rotate_vector_2d(np.array([1.0, 0.0]), self.__heading) | |
| # Normalize vectors to ensure consistent comparison | |
| to_car = to_car / np.linalg.norm(to_car) | |
| heading = heading / np.linalg.norm(heading) | |
| return np.dot(to_car, heading) > 0 |
| # if not self.__is_ahead((pose.pose.position.x, pose.pose.position.y)): | ||
| # continue |
There was a problem hiding this comment.
💡 Codebase verification
Commenting out position check introduces potential path following issues
The commented-out __is_ahead check was a safety mechanism that ensured the controller only considered path points that are in front of the vehicle (with a 1-unit threshold). By removing this check:
- The original bug it fixed: Likely prevented the controller from selecting points behind the vehicle, which could cause erratic behavior when the vehicle needs to reverse or handle sharp turns.
- The new bug it introduces: The controller now considers all path points after the current target point index (
self.__tp_idx), regardless of their position relative to the vehicle. This could lead to:- Selection of points behind the vehicle as target points
- Incorrect steering calculations when the path crosses itself
- Potential oscillations in vehicle control
• Consider implementing a more robust position check instead of removing it entirely, perhaps with a configurable threshold or different geometric approach.
🔗 Analysis chain
Review commented-out position check
The PR description mentions this is a bug fix that introduces a more severe bug. We need to understand:
- What issue this fix addresses
- What new bug it introduces
Let's analyze the impact:
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for related issues or PRs discussing the position check bug
gh issue list --search "pure pursuit position check"
gh pr list --search "pure pursuit position check"
# Look for tests or test failures related to this
rg -l "test.*pure.*pursuit" -g "test_*.py"
Length of output: 151
Script:
#!/bin/bash
# Let's analyze the pure pursuit controller implementation and its usage
rg -A 5 -B 5 "__is_ahead"
# Look for the implementation of the pure pursuit controller
ast-grep --pattern 'class PurePursuitController:
$$$
'
# Search for test files related to pure pursuit
fd test.*pure.*pursuit
# Look for commit messages that might give context
git log --oneline -S "__is_ahead"
Length of output: 1927
Script:
#!/bin/bash
# Let's look at the complete implementation of __is_ahead method
rg -A 10 "def __is_ahead" code/control/src/pure_pursuit_controller.py
# Look for any comments or documentation about this controller
rg -A 5 "class PurePursuitController" code/control/src/pure_pursuit_controller.py
# Check if there are any related configuration parameters
rg "lookahead|look_ahead|pursuit" code/control/src/pure_pursuit_controller.py
Length of output: 1647
There was a problem hiding this comment.
Actionable comments posted: 0
🧹 Outside diff range and nitpick comments (2)
code/control/src/vehicle_controller.py (2)
148-151: Update comment to reflect current behaviorThe comment "Velocities over 5 m/s = use Stanley, else use PurePuresuit" is now misleading as it doesn't reflect the actual behavior. Since this is a test change, consider:
- Updating the comment to indicate this is a temporary test configuration
- Using a configuration flag instead of hardcoding
if False- # Velocities over 5 m/s = use Stanley, else use PurePuresuit - if False: # self.__velocity > 5: + # TEMPORARY TEST: Using Pure Pursuit controller only + if False: # Disabled velocity-based controller selection for testing
Line range hint
148-156: Consider using configuration parameter for controller selectionInstead of hardcoding
if False, consider introducing a configuration parameter to control this behavior. This would make testing more maintainable and prevent accidental merges of test-only code.def __init__(self): super(VehicleController, self).__init__("vehicle_controller") self.loginfo("VehicleController node started") self.control_loop_rate = self.get_param("control_loop_rate", 0.05) self.role_name = self.get_param("role_name", "ego_vehicle") + self.force_pure_pursuit = self.get_param("force_pure_pursuit", False) # ... rest of init ... def run(self): # ... in the loop function ... - if False: # self.__velocity > 5: + if not self.force_pure_pursuit and self.__velocity > 5:
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
code/control/src/pure_pursuit_controller.py(3 hunks)code/control/src/vehicle_controller.py(1 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- code/control/src/pure_pursuit_controller.py
🔇 Additional comments (1)
code/control/src/vehicle_controller.py (1)
148-148:
Breaking Change: Forcing Pure Pursuit controller usage
This change forces the use of Pure Pursuit controller by hardcoding if False, effectively disabling the velocity-based controller selection. While this aligns with the PR's testing objectives, it represents a significant breaking change that could affect vehicle behavior, especially at higher speeds where the Stanley controller was previously used.
Let's verify the impact of this change:
This commit was created in order to run full leaderboard test in the git action. Car is only steering with pure pursuit controller. I also added a fix for a known indexing Bug in the pure pursuit contoller. The fix is commented out, because it introduces a more severe Bug.
Description
Testing out different steering modes in the github action.
Fixes # (issue)
Type of change
Does this PR introduce a breaking change?
Yes, do not merge. For Testing purposes only.
Summary by CodeRabbit