Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

DO NOT MERGE: Trying pure pursuit only. #563

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
+23 −3
Open

DO NOT MERGE: Trying pure pursuit only. #563

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f700710
Trying pure pursuit only.
vinzenzm Dec 11, 2024
a031a75
Implemented the pure pursuit only, which wasnt pushed in last commit
vinzenzm Dec 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 24 additions & 2 deletions code/control/src/pure_pursuit_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
from std_msgs.msg import Float32
from acting.msg import Debug
import numpy as np

from typing import Tuple
from acting.helper_functions import vector_angle, points_to_vector

# Tuneable Values for PurePursuit-Algorithm
Expand Down Expand Up @@ -169,16 +169,38 @@ def __get_target_point_index(self, ld: float) -> int:
min_dist_idx = -1
# might be more elegant to only look at points
# _ahead_ of the closest point on the trajectory
for i in range(self.__tp_idx, len(self.__path.poses)):
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
Comment on lines +172 to +175
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue

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.

Comment on lines +174 to +175
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

💡 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:

  1. 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.
  2. 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:

  1. What issue this fix addresses
  2. 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

dist = self.__dist_to(pose.pose.position)
dist2ld = dist - ld
# can be optimized
if min_dist > dist2ld > 0:
min_dist = dist2ld
min_dist_idx = i

self.loginfo(str(min_dist_idx))
return min_dist_idx

def rotate_vector_2d(self, vector, angle_rad):
rotation_matrix = np.array(
[
[np.cos(angle_rad), -np.sin(angle_rad)],
[np.sin(angle_rad), np.cos(angle_rad)],
]
)

return rotation_matrix @ np.array(vector)

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
Comment on lines +194 to +200
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🛠️ 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.

Suggested change
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


def __dist_to(self, pos: Point) -> float:
"""
Distance between current position and target position (only (x,y))
Expand Down
Loading