feat: Add reverse mode in pure pursuit #1194
Conversation
![github-advanced-security[bot]](https://avatars.githubusercontent.com/in/57789?s=60&v=4){kind=small}
|
@AtsushiSakai some error not caused by this PR ocurred |
|
@yangtzech Thank you for your PR. Could you please rebase on the master branch? |
There was a problem hiding this comment.
Pull Request Overview
This pull request adds reverse mode support to the pure pursuit algorithm and enhances vehicle visualization.
- Adds a new unit test for reverse mode (test_backward)
- Updates the State class and pure pursuit steering control to account for reverse movement
- Introduces an improved vehicle plotting function with a reverse mode indicator
Reviewed Changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 1 comment.
| File | Description |
|---|---|
| tests/test_pure_pursuit.py | Adds a new test case to validate reverse driving mode |
| PathTracking/pure_pursuit/pure_pursuit.py | Updates algorithm logic for reverse mode and enhances vehicle visualization |
Comments suppressed due to low confidence (2)
PathTracking/pure_pursuit/pure_pursuit.py:34
- [nitpick] Global simulation control variables are declared at the module level; consider centralizing these related settings in a dedicated configuration block to improve maintainability.
show_animation = True // pause_simulation = False // is_reverse_mode = False
PathTracking/pure_pursuit/pure_pursuit.py:183
- The numpy module is referenced via 'np' in plot_vehicle but there is no corresponding import (e.g., 'import numpy as np') in this module.
wheel = np.array([
There was a problem hiding this comment.
@yangtzech Thank you for the PR, and sorry for the delayed review.
I’ve added a few comments, so please take a look. Also, it would be great if you could add some diagrams or equations explaining the simulation to this document (though it’s not mandatory).
https://atsushisakai.github.io/PythonRobotics/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking.html
| @@ -141,19 +163,119 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): | |||
| plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), | |||
| fc=fc, ec=ec, head_width=width, head_length=width) | |||
| plt.plot(x, y) | |||
|
|
|||
| def plot_vehicle(x, y, yaw, steer=0.0, color='blue', is_reverse=False): | |||
There was a problem hiding this comment.
Please add an empty line here.
| """ | ||
| # Adjust heading angle in reverse mode | ||
| if is_reverse: | ||
| yaw = angle_mod(yaw + math.pi) # Rotate heading by 180 degrees |
There was a problem hiding this comment.
Why is it necessary to rotate the heading by 180 degrees? I thought the heading would stay the same even when moving backward, and that it wouldn’t affect the rendering.
| delta = state.direction * math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0) | ||
|
|
||
| # Limit steering angle to max value | ||
| delta = max(min(delta, MAX_STEER), -MAX_STEER) |
There was a problem hiding this comment.
Numpy's clip function can be used. https://numpy.org/doc/2.1/reference/generated/numpy.clip.html
| self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw)) | ||
| self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw)) | ||
| self.direction = -1 if is_reverse else 1 # Direction based on reverse flag | ||
| self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw)) |
There was a problem hiding this comment.
What is the rear_x, rear_y for?
Reference issue
What does this implement/fix?
Additional information
CheckList