Selector - tier4/new_planning_framework GitHub Wiki
Overview
A selector improves design flexibility and overall system performance by choosing the best trajectory -generated by diverse methods such as rule‑based logic or machine learning - according to the deployment environment and requirements. It also reflects human preferences in inherently multimodal decisions (e.g., when to change lanes) while guaranteeing a minimum safety level by filtering out unsafe trajectories produced by the generator.
Architecture
Below is a brief diagram of the selector architechture: rectangles are ROS nodes and arrows indicate ROS topics. The feasible trajectory filter node and valid trajectory filter node could be combined as they both consider safety of the candidate trajectory. A brief overview of each node will be given in the next section, but for detailed information, please refer to the README of each package.
Overview of each component
Trajectory concatenator
Concatenates candidate trajectories from multiple generators into a unified trajectories message for downstream selection. It maintains the latest input per generator and periodically merges them, optionally incorporating trimmed feedback trajectories for re-evaluation. Designed as an intermediate node between generators and the selector/ranker. The concatenator can be placed within the generator framework, but for this case, it was placed within the selector framework.
Feasible trajectory filter
Evaluates each trajectory candidate before final ranking, discarding those that are physically infeasible or unsafe, then republishes only the viable trajectories for further processing. It is important to keep the design minimal, as implementing too many diverse functions may lead to a rule-based system, potentially undermining the strengths of generators powered by end-to-end models.
Valid trajectory filter
Traffic Rule Validator serves as a safety gate, filtering out trajectories that violate immediate traffic rules. In addition to perception results, it is acceptable to use map information that defines traffic rules. The map format—whether HD map, SD map, or otherwise—does not matter.
Trajectory ranker
Assigns a scalar score to each candidate path, converting them into a ranked list to guide motion control. It standardizes trajectories through resampling, evaluates using a evaluation function with multiple metrics, and aggregates the results with weighted scoring. Metrics that are assumed to be used are written below with some example values. Some sample figures are provided for each metric, and in all the figures it is preferable that the blue trajectory is selected over the red one.
Safety
Meaning: How low the collision risk is.
- Time-to-Collision (TTC) at each point
Red trajectory collides with parked vehicle, whereas blue trajectory avoids.
Comfortability
Meaning: Ride comfort
- Lateral acceleration at each point
- Longitudinal jerk at each point
- Longitudinal acceleration at each point
Though the endpoint of the trajectory is equivalent, the red trajectory continuously changes the acceleration, whereas the blue trajectory has a constant acceleration.
Efficiency
Meaning: Propulsive effectiveness
- Total travel distance over time steps
Red trajectory is shorter than blue trajectory.
Achievability
Meaning: How well the trajectory follows the navigation route
- Lateral deviation to route information (lane level)
- Navigation compliance (turn instructions)
Red trajectory ignores lane level navigation, whereas blue trajectory changes lane to obey the lane level navigation.
Consistency
Meaning: A metric to avoid frequent and erratic decision switching
- Lateral deviation to previous selected trajectory
- Steering consistency
Red trajectory has a larger lateral deviation with the previously selected (black) trajectory in average than the blue trajectory.
Trajectory adaptor
Extracts the highest-scoring trajectory from the ranked candidates and converts it into a final trajectory message. If no valid candidate exists, an empty path is published. The selected trajectory is refined to ensure compatibility with downstream control modules.
Current implementation
Feasible trajectory filter
It checks for data validity, proximity to ego, lane adherence, and collision risk. These functions are not implemented as plugins and this would be a future task.
Valid trajectory filter
Using the HD map information and current traffic light states, it ensures only lawful paths—those stopping at red lights and stop lines—are passed on for ranking. These functions are not implemented as plugins and this would be a future task.
Trajectory ranker
The implemented evaluation function is as below:
The key points of this evaluation function is:
- Time-weighted scoring: Each metric value at time t is scaled by a temporal weight w_t, which incorporates a decay function—giving higher importance to near-term behavior and gradually reducing the influence of distant future points.
- Per-metric aggregation: The time-weighted values are summed for each metric and then scaled by a metric-specific weight W_metric, reflecting the relative importance of each criterion.
- Total score computation: The final score is obtained by summing the weighted scores of all metrics, producing a single scalar that ranks trajectory candidates.
This formulation allows the system to prioritize certain time horizons and evaluation metrics, enabling robust and tunable trajectory selection in autonomous driving. The evaluation is based on five conceptual metrics, with six specific implementations in total.
- Safety: Implemented as Time-to-Collision (TTC) at each trajectory point.
- Comfortability: Implemented as lateral acceleration and longitudinal jerk at each point.
- Efficiency: Implemented as the cumulative travel distance from the starting point to each point.
- Achievability: Implemented as lateral deviation from the centerline of the route’s lanelets.
- Consistency: Implemented as steering consistency. Steering angles are calculated at each point using a pure pursuit model, and consistency is evaluated by comparing steering angles between nearby points of the previous trajectory and the candidate trajectory.
Weight adjustment
Implemented an offline tool for tuning the Trajectory Ranker’s metric and time-decay weights. It loads driving logs that pair ground-truth trajectories with generated candidates, converts them into tensors of metric traces and poses, and feeds them to a PyTorch model. This model applies learnable temporal-decay weights, folds the result into six per-metric scalars, scales those by learnable metric-importance weights, and sums everything into a single trajectory score. All weights are re-parametrised through sigmoids so they remain bounded and non-negative throughout training. It then minimizes adaptive margin or triplet ranking losses that force the model to score the ground-truth path higher than competing candidates, with the margin automatically expanding on difficult examples - as measured by ADE (average displacement error)/FDE (final displacement error). During each epoch the script prints the evolving metric and temporal weights, evaluates ranking accuracy on a held-out set, and finally saves the tuned parameters to a checkpoint file..
Trajectory adaptor
Selects the optimal trajectory and align to the trajectory point format for the current Autoware controller. (ex. Remove overlapping point)
Tests
Unit tests
Unit tests have been written for each function to verify that they behave as expected. In particular, several basic tests are implemented for the ranker to ensure that weights are properly set after tuning the metric weights. These tests are designed to prevent the selection of clearly inappropriate trajectories - for example, ensuring that a trajectory following the lane center is preferred over one that sways.
Simulation
Planning simulator tests were conducted using the Motion Transformer. Please refer for further information.
Future work
- Define the safety responsibilities of the selector framework. Care must be taken to avoid overloading it with rules, which could shift the system toward a rule-based architecture.
- Finalize the evaluation function used in ranking, including the selection of appropriate metrics. While a machine learning model can be used as the ranker, using a hand-crafted evaluation function is preferable for interpretability. In that case, a method for tuning the weights assigned to each metric must be devised.
- Modularize safety rules by implementing them as plugins.