Add lapscounter feature #5
Conversation
There was a problem hiding this comment.
Pull request overview
This PR adds a GPS-based lap counting feature for tracking race car laps using latitude/longitude coordinates. The implementation includes geometry helpers for coordinate conversion and a robust lap counter that handles GPS errors through tolerance-based crossing detection.
Key Changes:
- Implements geometry helper functions for coordinate conversion (lat/lon to x/y) and line intersection detection
- Adds LapCounter class with tolerance-based crossing detection, side-change validation, and cluster protection to prevent double-counting
- Includes configurable parameters for GPS error tolerance, minimum movement thresholds, and minimum lap distances
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| ab_len = math.hypot(abx, aby) | ||
| if ab_len == 0: | ||
| return math.hypot(p.x - a.x, p.y - a.y) | ||
| # take min of endpoints’ distances; good enough as a soft check | ||
| def point_to_line(pt: Point): | ||
| return abs((abx * (pt.y - a.y) - aby * (pt.x - a.x)) / ab_len) |
There was a problem hiding this comment.
Division by zero is not handled when ab_len equals zero on line 65. While line 65-66 attempts to handle this case, if ab_len is exactly zero (which would occur if line_start and line_end are the same point), the function returns early but the division on line 69 could still fail if ab_len is extremely small but non-zero (near machine epsilon). Consider using a small epsilon threshold instead of exact zero comparison.
| min_lap_distance_m: float = 50.0 | ||
| ): | ||
| self.line_start = line_start | ||
| self.line_end = line_end |
There was a problem hiding this comment.
The LapCounter class doesn't validate that line_start and line_end are different points during initialization. If they are the same point, this will cause division by zero errors in _segment_min_distance_to_line and undefined behavior in _signed_side calculations. Consider adding validation to ensure the finish line has non-zero length.
| self.line_end = line_end | |
| self.line_end = line_end | |
| # Validate that the finish line has non-zero length to avoid | |
| # division-by-zero and undefined behavior in geometric helpers. | |
| dx = self.line_end.x - self.line_start.x | |
| dy = self.line_end.y - self.line_start.y | |
| if dx == 0.0 and dy == 0.0: | |
| raise ValueError( | |
| "LapCounter finish line must have non-zero length: " | |
| "line_start and line_end must be different points." | |
| ) |
| tolerance_m: float = 20.0, | ||
| min_move_m: float = 0.5, | ||
| min_lap_distance_m: float = 50.0 | ||
| ): |
There was a problem hiding this comment.
Parameter documentation is missing for the LapCounter.init method. Based on the codebase pattern (e.g., can-snooper/can_utils/csv_writer.py:45-52, 62-74), methods should document their parameters with Args sections in the docstring. Consider adding documentation for line_start, line_end, tolerance_m, min_move_m, and min_lap_distance_m to clarify their purposes and units.
| ): | |
| ): | |
| """ | |
| Initialize a LapCounter with a finish line and configuration parameters. | |
| Args: | |
| line_start (Point): Start point of the finish line in local x/y | |
| coordinates (meters). | |
| line_end (Point): End point of the finish line in local x/y | |
| coordinates (meters). | |
| tolerance_m (float): Distance tolerance around the finish line, | |
| in meters, within which a position is considered close enough | |
| to count as crossing despite GPS error. | |
| min_move_m (float): Minimum movement between consecutive samples, | |
| in meters, required to treat a position update as significant. | |
| min_lap_distance_m (float): Minimum path distance between counted | |
| crossings, in meters, to avoid multiple lap counts during a | |
| single physical pass over the line. | |
| """ |
| return min(point_to_line(p), point_to_line(q)) | ||
|
|
||
|
|
||
| def _sign(x: float, eps: float = 1e-9) -> int: |
There was a problem hiding this comment.
The epsilon value 1e-9 is a magic number without explanation. While this is likely appropriate for floating-point comparisons in meter-scale GPS coordinates, consider adding a comment explaining why this specific value was chosen, or defining it as a named constant at the module level for better maintainability.
| self.lap_count: int = 0 | ||
|
|
||
| def update_position(self, lat: float, lon: float, ref_lat: float, ref_lon: float): | ||
| """Feed one GPS sample and update lap count if a valid crossing occurs.""" |
There was a problem hiding this comment.
Parameter documentation is missing for the update_position method. Based on the codebase pattern (e.g., can-snooper/can_utils/csv_writer.py:45-52, 62-74), methods should document their parameters with Args sections in the docstring. Consider adding documentation for lat, lon, ref_lat, and ref_lon to clarify their purposes and expected units (likely degrees).
| """Feed one GPS sample and update lap count if a valid crossing occurs.""" | |
| """ | |
| Feed one GPS sample and update lap count if a valid crossing occurs. | |
| Args: | |
| lat: Current latitude of the sample in degrees. | |
| lon: Current longitude of the sample in degrees. | |
| ref_lat: Reference latitude in degrees used as the origin for the local | |
| x/y conversion. | |
| ref_lon: Reference longitude in degrees used as the origin for the local | |
| x/y conversion. | |
| """ |
| def update_position(self, lat: float, lon: float, ref_lat: float, ref_lon: float): | ||
| """Feed one GPS sample and update lap count if a valid crossing occurs.""" |
There was a problem hiding this comment.
The update_position method requires passing ref_lat and ref_lon on every call, which is inefficient and error-prone since these reference coordinates should remain constant throughout the lap counter's lifetime. Consider accepting ref_lat and ref_lon in init and storing them as instance variables, then accepting only lat and lon in update_position. This would match the usage pattern seen in tester copy.py:60-61,64-65 where reference coordinates are computed once.
| # Soft near-line test (within tolerance of the infinite line) | ||
| near_line = ( | ||
| abs(s_prev_raw) <= self.tolerance_m or | ||
| abs(s_curr_raw) <= self.tolerance_m or |
There was a problem hiding this comment.
The signed side values (s_prev_raw and s_curr_raw) are being compared directly to tolerance_m, but _signed_side returns an area value (cross product), not a distance. The magnitude of this cross product depends on both the distance from the line AND the length of the line segment. This means the tolerance will behave inconsistently for different line lengths. Consider normalizing by the line length or using actual distance calculations consistently.
| # Soft near-line test (within tolerance of the infinite line) | |
| near_line = ( | |
| abs(s_prev_raw) <= self.tolerance_m or | |
| abs(s_curr_raw) <= self.tolerance_m or | |
| # Soft near-line test (within tolerance of the infinite line). | |
| # Normalize the signed side (cross product) by the line length to obtain | |
| # actual perpendicular distances before comparing to tolerance_m. | |
| line_dx = self.line_end.x - self.line_start.x | |
| line_dy = self.line_end.y - self.line_start.y | |
| line_len = math.hypot(line_dx, line_dy) | |
| if line_len > 0.0: | |
| prev_dist = abs(s_prev_raw) / line_len | |
| curr_dist = abs(s_curr_raw) / line_len | |
| else: | |
| # Degenerate line; fall back to only the segment-based distance test. | |
| prev_dist = float("inf") | |
| curr_dist = float("inf") | |
| near_line = ( | |
| prev_dist <= self.tolerance_m or | |
| curr_dist <= self.tolerance_m or |
| def seg_intersect_with_tolerance(p1, p2, q1, q2, tolerance=10.0): | ||
| """ | ||
| Legacy tolerant intersection. Kept for compatibility, | ||
| but LapCounter below does a stricter side-change check instead. | ||
| """ | ||
| if seg_intersect(p1, p2, q1, q2): | ||
| return True | ||
| for a in (p1, p2): | ||
| for b in (q1, q2): | ||
| if math.hypot(a[0]-b[0], a[1]-b[1]) <= tolerance: | ||
| return True | ||
| return False |
There was a problem hiding this comment.
The function seg_intersect_with_tolerance is marked as "legacy" and states that LapCounter uses a "stricter side-change check instead", but examining the LapCounter class shows it doesn't actually use this function. This dead code should be removed to improve maintainability, or if it's kept for backward compatibility, the comment should clarify what external code depends on it.
| def seg_intersect(p1, p2, q1, q2): | ||
| """Classic segment intersection (exact, no tolerance).""" | ||
| def ccw(A, B, C): | ||
| return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0]) | ||
| return ccw(p1, q1, q2) != ccw(p2, q1, q2) and ccw(p1, p2, q1) != ccw(p1, p2, q2) |
There was a problem hiding this comment.
The function seg_intersect appears to be unused within this module. The LapCounter class doesn't use classic segment intersection, relying instead on side-change checks. Consider removing this dead code or documenting which external code depends on it.
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
This is the code to count laps of the car and test it.