Cone Ordering ============= Cone ordering is the process of organizing scattered cone observations into sorted left and right track boundaries. This is a critical preprocessing step that transforms unordered cone detections into a structured representation suitable for trajectory optimization. Problem Statement ----------------- **Input**: Unordered cone positions with color labels (blue = left, yellow = right) **Output**: Two sorted lists of cones representing the left and right track boundaries, ordered by distance along the track **Challenge**: Cones may be observed in any order, and observations may have: - Missing cones (occlusions, limited sensor range) - Noisy positions (sensor measurement errors) - Ambiguous pairings (wide track sections, complex geometries) Distance-Based Ordering ------------------------ The current implementation uses a greedy nearest-neighbor algorithm: Algorithm Description ~~~~~~~~~~~~~~~~~~~~~ .. code-block:: python def distance_cone_order(cones, vehicle_state): 1. Filter out cones behind the vehicle 2. Start from vehicle position 3. Repeat until all cones processed: a. Find nearest cone to current position b. Add cone to ordered list c. Move current position to that cone 4. Interpolate to get N evenly-spaced points 5. Return ordered left and right boundaries **Step 1: Filter Cones Behind Vehicle** Transform cones to vehicle frame and reject those with negative x-coordinates: .. code-block:: python # Rotation matrix to vehicle frame R = [[cos(-heading), -sin(-heading)], [sin(-heading), cos(-heading)]] cone_vehicle_frame = R @ (cone_position - vehicle_position) if cone_vehicle_frame[0] > 0: # Forward of vehicle keep_cone() This prevents the algorithm from ordering cones in the wrong direction. **Step 2: Greedy Nearest Neighbor** Starting from the vehicle position, iteratively select the closest unprocessed cone: .. code-block:: python current_position = vehicle_position ordered_cones = [] while unprocessed_cones: distances = [||cone - current_position|| for cone in unprocessed_cones] nearest_idx = argmin(distances) nearest_cone = unprocessed_cones[nearest_idx] ordered_cones.append(nearest_cone) unprocessed_cones.remove(nearest_cone) current_position = nearest_cone This produces a path that follows the cone boundary. **Step 3: Interpolation** The ordered cones are not evenly spaced, but the trajectory optimizer expects N uniformly distributed points. Linear interpolation creates this uniform distribution: .. code-block:: python # Compute arc length parameterization t = [0] for i in range(1, len(ordered_cones)): dx = ordered_cones[i][0] - ordered_cones[i-1][0] dy = ordered_cones[i][1] - ordered_cones[i-1][1] t.append(t[-1] + sqrt(dx² + dy²)) # Interpolate at uniform spacing t_uniform = linspace(0, t[-1], N) interpolated_cones = interp1d(t, ordered_cones)(t_uniform) This ensures the optimization has evenly distributed boundary points. **See:** ``local_path/local_path/distance_cone_order.py:9-102`` Implementation Details ---------------------- Filtering ~~~~~~~~~ The filtering step is critical for correct operation: .. code-block:: python heading = state[2] # Vehicle heading angle rotation_matrix = rotation(-heading) for cone in cones: cone_rel = cone - vehicle_position cone_in_car_frame = rotation_matrix @ cone_rel if cone_in_car_frame[0] > 0: # x > 0 means forward filtered_cones.append(cone) Without this filter, the algorithm might order cones backwards or create loops. Separate Left/Right Processing ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Left and right boundaries are ordered independently: - Blue cones (left boundary) processed separately from yellow cones (right) - Each boundary ordered using the same greedy algorithm - Both start from the vehicle position This prevents incorrect pairing between left and right sides. Minimum Cone Requirements ~~~~~~~~~~~~~~~~~~~~~~~~~~ The algorithm requires at least 2 cones per side: .. code-block:: python if len(left_sorted) < 2 or len(right_sorted) < 2: return None, None # Insufficient cones With only 1 cone per side, interpolation is not possible. Algorithm Characteristics ------------------------- This is a simple greedy nearest-neighbor implementation that provides fast computation (~5-20ms) and is easy to understand. The algorithm works well for the majority of typical race track scenarios, handling straight sections and moderate curves effectively. **Known Limitations:** We acknowledge that this distance-based approach does not handle all edge cases perfectly. The greedy nature of the algorithm can fail on complex track geometries such as sharp hairpins, very wide sections, or ambiguous cone pairings. We are currently working on developing a more robust cone ordering solution. For the 2025 competition, this distance-based algorithm was selected as the primary approach due to its simplicity and consistent performance on typical tracks, despite its known limitations. Edge Case Failures ----------- **Greedy Approach** ~~~~~~~~~~~~~~~~~~~ The nearest-neighbor strategy can fail when: - Track boundaries cross or converge (e.g., sharp hairpins) - Wide track sections with ambiguous nearest cone - Cones form complex patterns not captured by simple distance **Example failure case:** .. code-block:: text Left boundary: A -------- B | Right boundary: C D If starting at A, algorithm might go A → C (nearest) instead of A → B (correct track direction) **No Global Optimization** ~~~~~~~~~~~~~~~~~~~~~~~~~~~ The algorithm processes cones sequentially without considering the entire set: - Cannot backtrack if early decisions were suboptimal - May create zigzag patterns in ambiguous situations - Doesn't enforce smoothness or consistency **Edge Case Failures** ~~~~~~~~~~~~~~~~~~~~~~~ Known problematic scenarios: - **Very wide sections**: Distance ambiguity between left/right sides - **Hairpin turns**: Cones may be geometrically close but far along track - **Missing cones**: Gaps in observations can break the ordering - **Dense cone placement**: Multiple cones at similar distances Alternative Algorithm --------------------- An alternative cone ordering algorithm (``funny_cone_ordering``) is under development to address these limitations. It uses more sophisticated logic for pairing and ordering cones, but has not been fully validated on all edge cases. **Note:** The system is configured to use distance-based ordering (``distance_cone_order``) as the primary algorithm. While the distance-based approach has known limitations, it provides consistent performance on typical race tracks. We acknowledge that neither the current distance-based algorithm nor the alternative algorithm handles all edge cases perfectly. Visualization ------------- For debugging cone ordering, the local planner publishes visualization markers: **Topic**: ``/path/local/vizconeorder`` **Type**: ``visualization_msgs/Marker`` (LINE_LIST) **Content**: Red lines connecting paired left-right boundary points This allows visual inspection of the cone ordering result in RViz: .. code-block:: python # Each pair of left-right cones shown as a line points = [] for left_cone, right_cone in zip(ordered_left, ordered_right): points.append(Point(x=left_cone[0], y=left_cone[1])) points.append(Point(x=right_cone[0], y=right_cone[1])) If the lines are smooth and roughly perpendicular to the track, ordering is good. If lines cross or zigzag, ordering has failed. Configuration ------------- From ``all_settings.py``: .. code-block:: python class LocalOptSettings(metaclass=Settings): N: int = 10 # Number of interpolated points distance_cone_order: bool = True # Use distance-based ordering To disable distance-based ordering and use the alternative algorithm: .. code-block:: python distance_cone_order: bool = False However, this is not recommended for production use until the alternative is fully validated.