Faithful implementation of the nuPlan at-fault collision metric#440
Faithful implementation of the nuPlan at-fault collision metric#440WaelDLZ wants to merge 2 commits into
Conversation
There was a problem hiding this comment.
Pull request overview
This PR updates PufferDrive’s at-fault collision metric implementation to more closely mirror nuPlan’s collision-fault classification logic, replacing the prior simplified heuristic with geometry-based collision typing and lane-position checks.
Changes:
- Added nuPlan-oriented collision type constants and a behind-angle threshold (
NUPLAN_BEHIND_COS_THRESHOLD). - Introduced helper geometry routines to detect whether the ego’s front bumper intersects another agent’s box.
- Reworked
is_at_fault_collisionto use a collision-type classifier (stopped-track, active-front, active-rear, active-lateral) and lane cleanliness logic.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| } | ||
| } | ||
|
|
||
| // helpers for nuPlan's at-fault collision, you need to know if the front bumper was involed |
| static bool is_at_fault_collision(Drive *env, int agent_idx, int other_idx) { | ||
| Agent *agent = &env->agents[agent_idx]; | ||
| Agent *other = &env->agents[other_idx]; | ||
| int collision_type = classify_collision_type(agent, other); | ||
|
|
| @@ -2280,30 +2320,68 @@ static int collision_check(Drive *env, int agent_idx) { | |||
| return car_collided_with_index; | |||
| } | |||
|
|
|||
| static bool is_agent_behind_rear_axle(const Agent *ego, const Agent *other) { | |||
| float rear_x = ego->sim_x - 0.5f * ego->sim_length * ego->cos_heading; | |||
| float rear_y = ego->sim_y - 0.5f * ego->sim_length * ego->sin_heading; | |||
| float dx = other->sim_x - rear_x; | |||
| float dy = other->sim_y - rear_y; | |||
| float dist = sqrtf(dx * dx + dy * dy); | |||
| if (dist < 1e-6f) { | |||
| return false; | |||
| } | |||
|
|
|||
| float dot = (ego->cos_heading * dx + ego->sin_heading * dy) / dist; | |||
| return dot < NUPLAN_BEHIND_COS_THRESHOLD; | |||
| } | |||
|
|
|||
| static bool is_agent_cleanly_in_lane(const Agent *agent) { | |||
| if (agent->current_lane_idx == -1) { | |||
| return false; | |||
| } | |||
|
|
|||
| float edge_dist = fabsf(agent->metrics_array[LANE_DIST_IDX]) + 0.5f * agent->sim_width; | |||
| return edge_dist <= MULTI_LANE_THRESHOLD; | |||
| } | |||
|
|
|||
| static int classify_collision_type(const Agent *ego, const Agent *other) { | |||
| if (ego->sim_speed <= AGENT_STOPPED_SPEED_THRESHOLD) { | |||
| return COLLISION_TYPE_STOPPED_EGO; | |||
| } | |||
|
|
|||
| if (other->sim_speed <= AGENT_STOPPED_SPEED_THRESHOLD) { | |||
| return COLLISION_TYPE_STOPPED_TRACK; | |||
| } | |||
|
|
|||
| if (is_agent_behind_rear_axle(ego, other)) { | |||
| return COLLISION_TYPE_ACTIVE_REAR; | |||
| } | |||
|
|
|||
| if (front_bumper_intersects_agent(ego, other)) { | |||
| return COLLISION_TYPE_ACTIVE_FRONT; | |||
| } | |||
|
|
|||
| return COLLISION_TYPE_ACTIVE_LATERAL; | |||
| } | |||
|
|
|||
There was a problem hiding this comment.
Since all this functions are used only once inside is_at_fault_collision, directly place the code
There was a problem hiding this comment.
sorry, I meant place the code where it used, so in is_at_fault_collision
| } | ||
|
|
||
| static void compute_agent_corners(Agent *agent, float corners[4][2]) { | ||
| static void compute_agent_corners(const Agent *agent, float corners[4][2]) { |
There was a problem hiding this comment.
De toute façon soit je le met là, soit je l'enlève à tous les autres endroits du code ou y a const Agent, en soit osef c'est juste pour les fonctions qui ne mutent pas agent la convention c'est de mettre const, contrairement au fonctions de type "update_agent_...."
Bref c'est une convention j'te laisse choisir si tu la veux ou pas
The previous implementation was more a rough simplification while this one is designate to be as faithful as i could to nuPlan's original code.