Skip to content

Detect red-light stop-line crossings at the front bumper #603

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

Closed
kawaeeeee wants to merge 1 commit into from
Closed

Detect red-light stop-line crossings at the front bumper #603

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 4 additions & 0 deletions AGENTIC_PR
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
Devant la ligne, un instant de repos,
Le feu veille au passage des autos.
Quand vient le vert, la route se dessine,
Et chaque trajet poursuit sa ligne.
91 changes: 67 additions & 24 deletions pufferlib/ocean/drive/drive.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1774,9 +1774,74 @@ static bool check_corner_boxes_overlap(float corners_a[4][2], float corners_b[4]
return true;
}

static bool check_front_bumper_crossing(Agent *agent, TrafficControlElement *tc) {
float line_dx = tc->stop_line[3] - tc->stop_line[0];
float line_dy = tc->stop_line[4] - tc->stop_line[1];
float line_len = sqrtf(line_dx * line_dx + line_dy * line_dy);
if (line_len <= 0.0f) {
return false;
}
float mid_x = (tc->stop_line[0] + tc->stop_line[3]) * 0.5f;
float mid_y = (tc->stop_line[1] + tc->stop_line[4]) * 0.5f;
float line_ux = line_dx / line_len, line_uy = line_dy / line_len;
// Normal oriented along travel direction, so s < 0 is before the line.
float normal_x = -line_uy, normal_y = line_ux;
if (normal_x * cosf(tc->heading) + normal_y * sinf(tc->heading) < 0.0f) {
normal_x = -normal_x;
normal_y = -normal_y;
}

// Use physical dimensions, not the observation-only perceived size margin.
float prev_corners[4][2], cur_corners[4][2];
compute_bounding_box_corners(
agent->prev_x,
agent->prev_y,
agent->prev_cos_heading,
agent->prev_sin_heading,
agent->sim_length / 2.0f,
agent->sim_width / 2.0f,
prev_corners);
compute_bounding_box_corners(
agent->sim_x,
agent->sim_y,
agent->cos_heading,
agent->sin_heading,
agent->sim_length / 2.0f,
agent->sim_width / 2.0f,
cur_corners);

// Corners 0 and 1 form the front bumper. Both must start before the line:
// the trailing corner, center, and rear must not report the same entry again.
float s_prev[2], s_cur[2];
for (int corner_idx = 0; corner_idx < 2; corner_idx++) {
s_prev[corner_idx]
= (prev_corners[corner_idx][0] - mid_x) * normal_x + (prev_corners[corner_idx][1] - mid_y) * normal_y;
s_cur[corner_idx]
= (cur_corners[corner_idx][0] - mid_x) * normal_x + (cur_corners[corner_idx][1] - mid_y) * normal_y;
}
if (fmaxf(s_prev[0], s_prev[1]) >= 0.0f) {
return false;
}
for (int corner_idx = 0; corner_idx < 2; corner_idx++) {
if (s_cur[corner_idx] < 0.0f) {
continue;
}
float crossing_frac = s_prev[corner_idx] / (s_prev[corner_idx] - s_cur[corner_idx]);
float cross_x
= prev_corners[corner_idx][0] + crossing_frac * (cur_corners[corner_idx][0] - prev_corners[corner_idx][0]);
float cross_y
= prev_corners[corner_idx][1] + crossing_frac * (cur_corners[corner_idx][1] - prev_corners[corner_idx][1]);
float lateral = (cross_x - mid_x) * line_ux + (cross_y - mid_y) * line_uy;
if (fabsf(lateral) <= 0.5f * line_len + RED_LIGHT_LATERAL_EXTENSION_M) {
return true;
}
}
return false;
}

static bool check_red_light_violation(Drive *env, int agent_idx) {
Agent *agent = &env->agents[agent_idx];
// Vehicle center crosses on red; the laterally extended line cannot be driven
// Front bumper crosses on red; the laterally extended line cannot be driven
// around, and the heading gate exempts exiting/opposing traffic.
for (int i = 0; i < env->num_traffic_elements; i++) {
TrafficControlElement *tc = &env->traffic_elements[i];
Expand Down Expand Up @@ -1804,29 +1869,7 @@ static bool check_red_light_violation(Drive *env, int agent_idx) {
continue;
}

float line_dx = tc->stop_line[3] - tc->stop_line[0];
float line_dy = tc->stop_line[4] - tc->stop_line[1];
float line_len = sqrtf(line_dx * line_dx + line_dy * line_dy);
if (line_len <= 0.0f) {
continue;
}
float line_ux = line_dx / line_len, line_uy = line_dy / line_len;
// normal oriented along travel direction, so s < 0 is before the line
float normal_x = -line_uy, normal_y = line_ux;
if (normal_x * cosf(tc->heading) + normal_y * sinf(tc->heading) < 0.0f) {
normal_x = -normal_x;
normal_y = -normal_y;
}
float s_prev = (agent->prev_x - mid_x) * normal_x + (agent->prev_y - mid_y) * normal_y;
float s_cur = (agent->sim_x - mid_x) * normal_x + (agent->sim_y - mid_y) * normal_y;
if (!(s_prev < 0.0f && s_cur >= 0.0f)) {
continue;
}
float crossing_frac = s_prev / (s_prev - s_cur);
float cross_x = agent->prev_x + crossing_frac * (agent->sim_x - agent->prev_x);
float cross_y = agent->prev_y + crossing_frac * (agent->sim_y - agent->prev_y);
float lateral = (cross_x - mid_x) * line_ux + (cross_y - mid_y) * line_uy;
if (fabsf(lateral) <= 0.5f * line_len + RED_LIGHT_LATERAL_EXTENSION_M) {
if (check_front_bumper_crossing(agent, tc)) {
return true;
}
}
Expand Down
161 changes: 145 additions & 16 deletions tests/drive/test_drive_red_light.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ static Drive drive_test_light_env(TrafficControlElement *tc) {
return env;
}

// Violation = vehicle center crossing the line; stationary/backward motion never flags.
// Agent length 4: the front bumper is 2 m ahead of the center.
static Agent drive_test_driving_agent(float prev_x, float cur_x, float y) {
Agent agent = drive_test_agent(prev_x, y, 0.0f);
agent.sim_x = cur_x;
Expand All @@ -47,33 +47,33 @@ static int test_red_crossing_flags(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 0.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_straddle_then_proceed_on_red_flags(void) {
static int test_front_already_across_no_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
// nose already over the line when the light turned red; proceeding completes entry
// The entry event has already happened; crossing with the center must not fire again.
Agent agent = drive_test_driving_agent(-1.5f, 0.5f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
}

static int test_green_crossing_then_red_inside_no_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_GREEN);
Agent agent = drive_test_driving_agent(1.5f, 2.5f, 0.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
// light turns red once the agent is fully inside the junction
// The front entered on green; red must not penalize the later center crossing.
set_light(TRAFFIC_CONTROL_STATE_RED);
agent = drive_test_driving_agent(4.0f, 5.0f, 0.0f);
agent = drive_test_driving_agent(-1.5f, 0.5f, 0.0f);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
}
Expand All @@ -94,8 +94,12 @@ static int test_crossing_fires_once(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(2.5f, 3.5f, 0.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
agent = drive_test_driving_agent(-1.5f, 0.5f, 0.0f);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
agent = drive_test_driving_agent(0.5f, 2.5f, 0.0f);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
}
Expand All @@ -105,7 +109,7 @@ static int test_opposing_lane_bypass_flags(void) {
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
// crossing 5 m beside the painted line (opposing lane) is still a violation
Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 5.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 5.0f);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
Expand All @@ -115,7 +119,7 @@ static int test_crossing_beyond_virtual_extension_no_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 18.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 18.1f);
env.agents = &agent;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
Expand All @@ -138,8 +142,9 @@ static int test_diagonal_entry_flags(void) {
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
// 1.2 rad off the light heading is still entering (< pi/2 gate)
Agent agent = drive_test_agent(-0.5f, 0.0f, 1.2f);
agent.sim_x = 0.5f;
// Only the leading corner crosses: even the bumper midpoint stays before x = 0.
Agent agent = drive_test_agent(-2.0f, 0.0f, 1.2f);
agent.sim_x = -1.5f;
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
Expand All @@ -161,7 +166,7 @@ static int test_different_z_level_no_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 0.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f);
agent.sim_z = 6.0f;
env.agents = &agent;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
Expand All @@ -172,18 +177,134 @@ static int test_no_lane_assignment_still_flags(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 0.0f);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f);
agent.current_lane_idx = -1;
agent.previous_lane_idx = -1;
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_approach_and_reverse_no_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-3.0f, -2.1f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
agent = drive_test_driving_agent(-2.1f, -2.1f, 0.0f);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
agent = drive_test_driving_agent(-1.5f, -2.5f, 0.0f);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
}

static int test_front_touch_and_large_step_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-2.5f, -2.0f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
agent = drive_test_driving_agent(-10.0f, 10.0f, 0.0f);
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_heading_change_uses_previous_pose(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_agent(-1.8f, 0.0f, 1.2f);
agent.sim_heading = 0.4f;
agent.cos_heading = cosf(agent.sim_heading);
agent.sin_heading = sinf(agent.sim_heading);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_trailing_front_corner_does_not_retrigger(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_agent(-2.0f, 0.0f, 1.2f);
agent.sim_x = -1.5f;
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
copy_pose_to_prev(&agent);
agent.sim_x = 1.0f;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
}

static int test_front_edge_overlaps_lateral_limit(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
// Center is outside the 17 m half-span, but the right front corner is inside.
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 17.5f);
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_translated_rotated_line_and_reversed_endpoints(void) {
TrafficControlElement tc = drive_test_light();
tc.stop_line[0] = 8.0f;
tc.stop_line[1] = 20.0f;
tc.stop_line[3] = 12.0f;
tc.stop_line[4] = 20.0f;
tc.heading = (float) M_PI / 2.0f;
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_agent(10.0f, 17.5f, tc.heading);
agent.sim_y = 18.5f;
env.agents = &agent;
EXPECT_TRUE(check_red_light_violation(&env, 0));
tc.stop_line[0] = 12.0f;
tc.stop_line[3] = 8.0f;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_physical_length_and_perceived_margin(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
set_light(TRAFFIC_CONTROL_STATE_RED);
Agent agent = drive_test_driving_agent(-3.5f, -2.5f, 0.0f);
env.agents = &agent;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
env.eval_mode = 1;
env.control_mode = CONTROL_MODE_SDC_ONLY;
env.eval_perceived_size_margin_m = 2.0f;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
agent.sim_length = 6.0f;
EXPECT_TRUE(check_red_light_violation(&env, 0));
env.eval_perceived_size_margin_m = 0.0f;
EXPECT_TRUE(check_red_light_violation(&env, 0));
return 0;
}

static int test_non_red_or_missing_state_no_flag(void) {
TrafficControlElement tc = drive_test_light();
Drive env = drive_test_light_env(&tc);
Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f);
env.agents = &agent;
set_light(TRAFFIC_CONTROL_STATE_GREEN);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
set_light(TRAFFIC_CONTROL_STATE_YELLOW);
EXPECT_TRUE(!check_red_light_violation(&env, 0));
set_light(TRAFFIC_CONTROL_STATE_RED);
env.timestep = tc.state_size;
EXPECT_TRUE(!check_red_light_violation(&env, 0));
return 0;
}

int main(void) {
int failures = 0;
RUN_TEST(test_red_crossing_flags);
RUN_TEST(test_straddle_then_proceed_on_red_flags);
RUN_TEST(test_front_already_across_no_flag);
RUN_TEST(test_green_crossing_then_red_inside_no_flag);
RUN_TEST(test_stationary_straddle_no_flag);
RUN_TEST(test_crossing_fires_once);
Expand All @@ -194,5 +315,13 @@ int main(void) {
RUN_TEST(test_perpendicular_crossing_no_flag);
RUN_TEST(test_different_z_level_no_flag);
RUN_TEST(test_no_lane_assignment_still_flags);
RUN_TEST(test_approach_and_reverse_no_flag);
RUN_TEST(test_front_touch_and_large_step_flag);
RUN_TEST(test_heading_change_uses_previous_pose);
RUN_TEST(test_trailing_front_corner_does_not_retrigger);
RUN_TEST(test_front_edge_overlaps_lateral_limit);
RUN_TEST(test_translated_rotated_line_and_reversed_endpoints);
RUN_TEST(test_physical_length_and_perceived_margin);
RUN_TEST(test_non_red_or_missing_state_no_flag);
return test_summary(failures);
}