diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5927bf2f08..aa676bc2a9 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -40,6 +40,39 @@ jobs: - name: Unit tests run: python -m pytest -v tests/unit_tests + c-drive-tests: + name: C Drive Tests + runs-on: ubuntu-latest + env: + PUFFER_CPU: 1 + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: "3.11" + + - name: Free up disk space + run: | + sudo rm -rf /usr/share/dotnet /usr/local/lib/android /opt/ghc + sudo apt-get clean + sudo rm -rf ~/.cache/pip /tmp/* /var/tmp/* + + - name: Install pufferlib + env: + TMPDIR: ${{ runner.temp }}/build + PIP_NO_CACHE_DIR: 1 + run: | + sudo apt-get update && sudo apt-get install -y build-essential cmake xvfb libgl1-mesa-dev libegl1-mesa-dev libx11-dev libxcursor-dev libxrandr-dev libxinerama-dev libxi-dev libxxf86vm-dev + python -m pip install -U pip + pip install -e . --no-cache-dir + python setup.py build_ext --inplace --force + + - name: C drive unit and render tests + run: make -C tests/drive test render + notebook-tests: name: Notebook Execution runs-on: ubuntu-latest @@ -147,11 +180,11 @@ jobs: TMPDIR: ${{ runner.temp }}/build PIP_NO_CACHE_DIR: 1 run: | - sudo apt-get update && sudo apt-get install -y build-essential cmake + sudo apt-get update && sudo apt-get install -y build-essential cmake libgl1-mesa-dev libegl1-mesa-dev libx11-dev libxcursor-dev libxrandr-dev libxinerama-dev libxi-dev libxxf86vm-dev python -m pip install -U pip pip install -e . --no-cache-dir python setup.py build_ext --inplace --force - name: Test simulator performance - run: python tests/smoke_tests/test_simulator_perf.py + run: make -C tests/drive perf timeout-minutes: 20 diff --git a/pufferlib/ocean/drive/datatypes.h b/pufferlib/ocean/drive/datatypes.h index 404ca1b52a..0d1ad0ee90 100644 --- a/pufferlib/ocean/drive/datatypes.h +++ b/pufferlib/ocean/drive/datatypes.h @@ -181,21 +181,26 @@ struct Agent { float *log_height; int *log_valid; - // Simulation state (always center-based, even when trajectory control uses the rear axle) - float sim_x; // Vehicle center x-coordinate - float sim_y; // Vehicle center y-coordinate - float sim_z; - float sim_heading; - float cos_heading; // Cached cosf(sim_heading) - updated in move_dynamics - float sin_heading; // Cached sinf(sim_heading) - updated in move_dynamics - float sim_vx; // Vehicle center velocity x-component - float sim_vy; // Vehicle center velocity y-component - float yaw_rate; // Angular velocity used to derive rear-axle velocity - float sim_speed; - float sim_speed_signed; - float sim_length; - float sim_width; - float sim_height; + // Simulation state + float sim_x; // Bounding box center x + float sim_y; // Bounding box center y + float sim_z; // Bounding box center z + float sim_heading; // Bounding box heading + float cos_heading; + float sin_heading; + float sim_vx; // Bounding box velocity x + float sim_vy; // Bounding box velocity y + float yaw_rate; // Angular velocity used to convert between rear and center velocity + float sim_speed; // Bounding box center speed magnitude + float sim_speed_signed; // Bounding box center signed longitudinal speed + float sim_length; // Bounding box length + float sim_width; // Bounding box width + float sim_height; // Bounding box height + float radius; // 0.5 * sqrt(L^2 + W^2); refreshed when L/W change + float prev_x; + float prev_y; + float prev_cos_heading; + float prev_sin_heading; int sim_valid; // Route information diff --git a/pufferlib/ocean/drive/drive.h b/pufferlib/ocean/drive/drive.h index 15a7e28a74..0ccfec7c69 100644 --- a/pufferlib/ocean/drive/drive.h +++ b/pufferlib/ocean/drive/drive.h @@ -82,8 +82,9 @@ #define LANE_ALIGN_COS_THRESHOLD 0.5f // Collision and distance thresholds +#define COLLISION_SKIP_DISP_M 0.1f +#define COLLISION_PAIR_MARGIN_M 0.5f // Extra slack on the radius+displacement quick-check before OBB SAT #define MAX_CHECKED_LANES 32 -#define COLLISION_QUICK_CHECK_DIST 15.0f // Quick distance check before OBB SAT #define AGENT_STOPPED_SPEED_THRESHOLD 0.2f #define MAX_STOPPED_SECONDS 60.0f #define DTC_FRONT_CONE_COS_THRESHOLD -0.90f // 340 degree cone centered on ego heading @@ -488,6 +489,20 @@ static float compute_euclidean_distance(float x1, float y1, float x2, float y2) return sqrtf(dx * dx + dy * dy); } +// Distance from point (px, py) to segment (x0, y0)->(x1, y1). +// Degenerate (zero-length) segment reduces to point-to-point distance. +static float compute_point_to_segment_distance(float px, float py, float x0, float y0, float x1, float y1) { + float dx = x1 - x0; + float dy = y1 - y0; + float seg_len_sq = dx * dx + dy * dy; + float t = 0.0f; + if (seg_len_sq > 1e-6f) { + t = ((px - x0) * dx + (py - y0) * dy) / seg_len_sq; + t = fmaxf(0.0f, fminf(1.0f, t)); + } + return compute_euclidean_distance(px, py, x0 + t * dx, y0 + t * dy); +} + static int compare_depthpoint(const void *a, const void *b) { float diff = ((const DepthPoint *) a)->euclidean_dis - ((const DepthPoint *) b)->euclidean_dis; return (diff > 0.0f) - (diff < 0.0f); @@ -577,6 +592,17 @@ static void invalidate_agent(Agent *agent) { agent->sim_valid = 0; } +static void copy_pose_to_prev(Agent *agent) { + agent->prev_x = agent->sim_x; + agent->prev_y = agent->sim_y; + agent->prev_cos_heading = agent->cos_heading; + agent->prev_sin_heading = agent->sin_heading; +} + +static inline void update_agent_radius(Agent *agent) { + agent->radius = 0.5f * sqrtf(agent->sim_length * agent->sim_length + agent->sim_width * agent->sim_width); +} + static inline void apply_infraction_behavior(Agent *agent, int behavior) { if (behavior == STOP_AGENT && !agent->stopped) { agent->stopped = 1; @@ -614,23 +640,42 @@ static inline float compute_log_yaw_rate(Agent *agent, int timestep, float dt) { return 0.0f; } -static inline void project_vector_to_ego_frame( +static inline void project_vector_to_local(float vx, float vy, float cos_h, float sin_h, float *lx, float *ly) { + // Rotate a world-frame vector into a local frame with heading (cos_h, sin_h). Rotation only. + *lx = vx * cos_h + vy * sin_h; + *ly = -vx * sin_h + vy * cos_h; +} + +static inline void project_point_to_local( + float px, + float py, + float cx, + float cy, + float cos_h, + float sin_h, + float *lx, + float *ly) { + // Transform a world point into a local frame centered at (cx,cy) with heading (cos_h,sin_h). + // Translate to the center, then rotate. + project_vector_to_local(px - cx, py - cy, cos_h, sin_h, lx, ly); +} + +static inline void project_point_to_ego_frame( const Agent *ego, float world_x, float world_y, float *ego_x, float *ego_y) { - *ego_x = world_x * ego->cos_heading + world_y * ego->sin_heading; - *ego_y = -world_x * ego->sin_heading + world_y * ego->cos_heading; + project_point_to_local(world_x, world_y, ego->sim_x, ego->sim_y, ego->cos_heading, ego->sin_heading, ego_x, ego_y); } -static inline void project_point_to_ego_frame( +static inline void project_vector_to_ego_frame( const Agent *ego, float world_x, float world_y, float *ego_x, float *ego_y) { - project_vector_to_ego_frame(ego, world_x - ego->sim_x, world_y - ego->sim_y, ego_x, ego_y); + project_vector_to_local(world_x, world_y, ego->cos_heading, ego->sin_heading, ego_x, ego_y); } // ======================================== @@ -2081,72 +2126,130 @@ static float compute_displacement_error(Agent *agent, int timestep) { return displacement; } -static bool check_line_intersection(float p1[2], float p2[2], float q1[2], float q2[2]) { - if (fmaxf(p1[0], p2[0]) < fminf(q1[0], q2[0]) || fminf(p1[0], p2[0]) > fmaxf(q1[0], q2[0]) - || fmaxf(p1[1], p2[1]) < fminf(q1[1], q2[1]) || fminf(p1[1], p2[1]) > fmaxf(q1[1], q2[1])) { - return false; - } - - // Calculate vectors - float dx1 = p2[0] - p1[0]; - float dy1 = p2[1] - p1[1]; - float dx2 = q2[0] - q1[0]; - float dy2 = q2[1] - q1[1]; - - // Calculate cross products - float cross = dx1 * dy2 - dy1 * dx2; - - // If lines are parallel - if (cross == 0) { - return false; - } - - // Calculate relative vectors between start points - float dx3 = p1[0] - q1[0]; - float dy3 = p1[1] - q1[1]; - - // Calculate parameters for intersection point - float s = (dx1 * dy3 - dy1 * dx3) / cross; - float t = (dx2 * dy3 - dy2 * dx3) / cross; - - // Check if intersection point lies within both line segments - return (s >= 0 && s <= 1 && t >= 0 && t <= 1); -} - -static void compute_agent_corners(const Agent *agent, float corners[4][2]) { +static void compute_bounding_box_corners( + float x, + float y, + float cos_h, + float sin_h, + float half_l, + float half_w, + float corners[4][2]) { static const float offsets[4][2] = {{1, 1}, {1, -1}, {-1, -1}, {-1, 1}}; - float half_length = agent->sim_length / 2.0f; - float half_width = agent->sim_width / 2.0f; - for (int i = 0; i < 4; i++) { - corners[i][0] = agent->sim_x - + (offsets[i][0] * half_length * agent->cos_heading - offsets[i][1] * half_width * agent->sin_heading); - corners[i][1] = agent->sim_y - + (offsets[i][0] * half_length * agent->sin_heading + offsets[i][1] * half_width * agent->cos_heading); + corners[i][0] = x + (offsets[i][0] * half_l * cos_h - offsets[i][1] * half_w * sin_h); + corners[i][1] = y + (offsets[i][0] * half_l * sin_h + offsets[i][1] * half_w * cos_h); } } -static bool check_agent_corners_cross_stop_line(float corners[4][2], TrafficControlElement *traffic) { - float sl_dx = traffic->stop_line[3] - traffic->stop_line[0]; - float sl_dy = traffic->stop_line[4] - traffic->stop_line[1]; - float ext = (STOP_LINE_EXTENSION_FACTOR - 1.0f) * 0.5f; - float ext_p1[2] = {traffic->stop_line[0] - ext * sl_dx, traffic->stop_line[1] - ext * sl_dy}; - float ext_p2[2] = {traffic->stop_line[3] + ext * sl_dx, traffic->stop_line[4] + ext * sl_dy}; - - for (int k = 0; k < 4; k++) { - if (k == 2) { +static bool check_segment_intersects_aabb(float p0[2], float p1[2], float half_l, float half_w) { + // Liang-Barsky slab clip of segment p0->p1 against the origin-centered AABB + // [-half_l,half_l] x [-half_w,half_w]. True if the segment crosses the box or starts inside it. + // A degenerate (zero-length) segment reduces to a point-in-box test. + float dx = p1[0] - p0[0]; + float dy = p1[1] - p0[1]; + float t0 = 0.0f; + float t1 = 1.0f; + float p[4] = {-dx, dx, -dy, dy}; + float q[4] = {p0[0] + half_l, half_l - p0[0], p0[1] + half_w, half_w - p0[1]}; + for (int i = 0; i < 4; i++) { + if (p[i] == 0.0f) { + if (q[i] < 0.0f) { + return false; // parallel to this slab and outside it + } continue; } - int next = (k + 1) % 4; - if (check_line_intersection(corners[k], corners[next], ext_p1, ext_p2)) { - return true; + float r = q[i] / p[i]; + if (p[i] < 0.0f) { + if (r > t1) { + return false; + } + if (r > t0) { + t0 = r; + } + } else { + if (r < t0) { + return false; + } + if (r < t1) { + t1 = r; + } } } + return true; +} + +static bool check_segment_crosses_moving_box(float ax, float ay, float bx, float by, Agent *agent) { + // Transform both endpoints into the box-local frame at prev and cur, then test the 4 boundary + // segments of the swept quad {line@t, line@t+1, A-traj, B-traj} vs the origin-centered AABB. + float a_prev[2], b_prev[2], a_cur[2], b_cur[2]; + float half_l = agent->sim_length / 2.0f, half_w = agent->sim_width / 2.0f; + project_point_to_local( + ax, + ay, + agent->prev_x, + agent->prev_y, + agent->prev_cos_heading, + agent->prev_sin_heading, + &a_prev[0], + &a_prev[1]); + project_point_to_local( + bx, + by, + agent->prev_x, + agent->prev_y, + agent->prev_cos_heading, + agent->prev_sin_heading, + &b_prev[0], + &b_prev[1]); + project_point_to_local( + ax, + ay, + agent->sim_x, + agent->sim_y, + agent->cos_heading, + agent->sin_heading, + &a_cur[0], + &a_cur[1]); + project_point_to_local( + bx, + by, + agent->sim_x, + agent->sim_y, + agent->cos_heading, + agent->sin_heading, + &b_cur[0], + &b_cur[1]); + if (check_segment_intersects_aabb(a_prev, b_prev, half_l, half_w) + || check_segment_intersects_aabb(a_cur, b_cur, half_l, half_w) + || check_segment_intersects_aabb(a_prev, a_cur, half_l, half_w) + || check_segment_intersects_aabb(b_prev, b_cur, half_l, half_w)) { + return true; + } - return false; + // Boundary miss can still mean the swept segment quad contains the AABB. + // Same-sign test assumes the quad is convex; a self-intersecting (bowtie) quad — large per-step + // relative rotation — can report a contained AABB as outside. Degenerate prev == cur collapses + // to a line: opposite signs => false, and the boundary tests above already caught any overlap. + float swept_quad[4][2] = { + {a_prev[0], a_prev[1]}, + {b_prev[0], b_prev[1]}, + {b_cur[0], b_cur[1]}, + {a_cur[0], a_cur[1]}, + }; + bool has_positive = false; + bool has_negative = false; + for (int i = 0; i < 4; i++) { + int j = (i + 1) % 4; + float edge_x = swept_quad[j][0] - swept_quad[i][0]; + float edge_y = swept_quad[j][1] - swept_quad[i][1]; + float cross = edge_x * -swept_quad[i][1] - edge_y * -swept_quad[i][0]; + has_positive = has_positive || cross > 0.0f; + has_negative = has_negative || cross < 0.0f; + } + return has_positive != has_negative; } -static bool check_stop_line_crossing(Drive *env, Agent *agent, float corners[4][2]) { +static bool check_stop_line_crossing(Drive *env, Agent *agent) { for (int i = 0; i < env->num_traffic_elements; i++) { TrafficControlElement *tc = &env->traffic_elements[i]; @@ -2189,7 +2292,13 @@ static bool check_stop_line_crossing(Drive *env, Agent *agent, float corners[4][ continue; } - if (check_agent_corners_cross_stop_line(corners, tc)) { + float sl_dx = tc->stop_line[3] - tc->stop_line[0]; + float sl_dy = tc->stop_line[4] - tc->stop_line[1]; + float ext = (STOP_LINE_EXTENSION_FACTOR - 1.0f) * 0.5f; + float ext_p1[2] = {tc->stop_line[0] - ext * sl_dx, tc->stop_line[1] - ext * sl_dy}; + float ext_p2[2] = {tc->stop_line[3] + ext * sl_dx, tc->stop_line[4] + ext * sl_dy}; + + if (check_segment_crosses_moving_box(ext_p1[0], ext_p1[1], ext_p2[0], ext_p2[1], agent)) { return true; } } @@ -2248,10 +2357,7 @@ static bool check_red_light_violation(Drive *env, int agent_idx) { return false; } - float corners[4][2]; - compute_agent_corners(agent, corners); - - if (check_stop_line_crossing(env, agent, corners)) { + if (check_stop_line_crossing(env, agent)) { return true; } @@ -2274,19 +2380,30 @@ static bool check_obb_collision(Agent *car1, Agent *car2) { return false; } - // Get car corners in world space + float cos1 = car1->cos_heading; + float sin1 = car1->sin_heading; + float cos2 = car2->cos_heading; + float sin2 = car2->sin_heading; float car1_corners[4][2]; + compute_bounding_box_corners( + car1->sim_x, + car1->sim_y, + cos1, + sin1, + car1->sim_length / 2.0f, + car1->sim_width / 2.0f, + car1_corners); float car2_corners[4][2]; - compute_agent_corners(car1, car1_corners); - compute_agent_corners(car2, car2_corners); - - // Get the axes to check (normalized vectors perpendicular to each edge) - float axes[4][2] = { - {car1->cos_heading, car1->sin_heading}, // Car1's length axis - {-car1->sin_heading, car1->cos_heading}, // Car1's width axis - {car2->cos_heading, car2->sin_heading}, // Car2's length axis - {-car2->sin_heading, car2->cos_heading} // Car2's width axis - }; + compute_bounding_box_corners( + car2->sim_x, + car2->sim_y, + cos2, + sin2, + car2->sim_length / 2.0f, + car2->sim_width / 2.0f, + car2_corners); + + float axes[4][2] = {{cos1, sin1}, {-sin1, cos1}, {cos2, sin2}, {-sin2, cos2}}; for (int i = 0; i < 4; i++) { float min1 = INFINITY, max1 = -INFINITY; @@ -2306,6 +2423,77 @@ static bool check_obb_collision(Agent *car1, Agent *car2) { return true; } +static bool check_moving_obb_collision(Agent *a, Agent *b, float a_disp, float b_disp) { + // Each box's corner trajectories (prev->cur, transformed into the other box's prev/cur local frame) + // tested vs the other's origin-centered AABB, both directions. + // Final-pose SAT catches cross/plus overlaps where no corner trajectory enters the other box. + + // Early z-axis rejection + float a_top = a->sim_z + a->sim_height; + float b_top = b->sim_z + b->sim_height; + if (a_top < b->sim_z || b_top < a->sim_z) { + return false; + } + + // Final-pose overlap (incl. cross/plus with no corner inside) is the complete static-overlap test. + if (check_obb_collision(a, b)) { + return true; + } + + // Both nearly static: a sub-threshold step can't open a tunnelling gap the sweep would catch. + if (a_disp < COLLISION_SKIP_DISP_M && b_disp < COLLISION_SKIP_DISP_M) { + return false; + } + + for (int d = 0; d < 2; d++) { + Agent *ego = d ? a : b; + Agent *other = d ? b : a; + float oc_prev[4][2], oc_cur[4][2]; + compute_bounding_box_corners( + other->prev_x, + other->prev_y, + other->prev_cos_heading, + other->prev_sin_heading, + other->sim_length / 2.0f, + other->sim_width / 2.0f, + oc_prev); + compute_bounding_box_corners( + other->sim_x, + other->sim_y, + other->cos_heading, + other->sin_heading, + other->sim_length / 2.0f, + other->sim_width / 2.0f, + oc_cur); + float ego_half_l = ego->sim_length / 2.0f, ego_half_w = ego->sim_width / 2.0f; + for (int k = 0; k < 4; k++) { + float q_prev[2], q_cur[2]; + project_point_to_local( + oc_prev[k][0], + oc_prev[k][1], + ego->prev_x, + ego->prev_y, + ego->prev_cos_heading, + ego->prev_sin_heading, + &q_prev[0], + &q_prev[1]); + project_point_to_local( + oc_cur[k][0], + oc_cur[k][1], + ego->sim_x, + ego->sim_y, + ego->cos_heading, + ego->sin_heading, + &q_cur[0], + &q_cur[1]); + if (check_segment_intersects_aabb(q_prev, q_cur, ego_half_l, ego_half_w)) { + return true; + } + } + } + return false; +} + static int collision_check(Drive *env, int agent_idx) { Agent *agent = &env->agents[agent_idx]; @@ -2314,10 +2502,9 @@ static int collision_check(Drive *env, int agent_idx) { } int car_collided_with_index = -1; + float ego_disp = compute_euclidean_distance(agent->sim_x, agent->sim_y, agent->prev_x, agent->prev_y); - // O(N) linear scan over all agents (active + static); no spatial grid used here. - // COLLISION_QUICK_CHECK_DIST (15m) is the real bottleneck — the 5m-cell grid - // neighborhood covers ~50m but this quick check prunes at 15m. + // Linear over all actors; pair radius quick-check prunes before OBB SAT. for (int i = 0; i < env->num_agents; i++) { int index = -1; if (i < env->active_agent_count) { @@ -2330,14 +2517,22 @@ static int collision_check(Drive *env, int agent_idx) { } Agent *other_agent = &env->agents[index]; + if (other_agent->sim_x == INVALID_POSITION || other_agent->removed || other_agent->sim_valid != 1) { + continue; + } - float dist_sq - = ((other_agent->sim_x - agent->sim_x) * (other_agent->sim_x - agent->sim_x) - + (other_agent->sim_y - agent->sim_y) * (other_agent->sim_y - agent->sim_y)); - if (dist_sq > COLLISION_QUICK_CHECK_DIST * COLLISION_QUICK_CHECK_DIST) { + float other_disp = compute_euclidean_distance( + other_agent->sim_x, + other_agent->sim_y, + other_agent->prev_x, + other_agent->prev_y); + float threshold = agent->radius + other_agent->radius + COLLISION_PAIR_MARGIN_M + ego_disp + other_disp; + float ddx = other_agent->sim_x - agent->sim_x; + float ddy = other_agent->sim_y - agent->sim_y; + if (ddx * ddx + ddy * ddy > threshold * threshold) { continue; } - if (check_obb_collision(agent, other_agent)) { + if (check_moving_obb_collision(agent, other_agent, ego_disp, other_disp)) { car_collided_with_index = index; break; } @@ -2372,33 +2567,42 @@ static bool is_at_fault_collision(Drive *env, int agent_idx, int other_idx) { } float agent_corners[4][2]; - compute_agent_corners(agent, agent_corners); - - float other_half_length = 0.5f * other->sim_length; - float other_half_width = 0.5f * other->sim_width; - bool front_bumper_intersects = false; - for (int i = 0; i < 2; i++) { - float corner_dx = agent_corners[i][0] - other->sim_x; - float corner_dy = agent_corners[i][1] - other->sim_y; - float local_long = corner_dx * other->cos_heading + corner_dy * other->sin_heading; - float local_lat = -corner_dx * other->sin_heading + corner_dy * other->cos_heading; - if (fabsf(local_long) <= other_half_length && fabsf(local_lat) <= other_half_width) { - front_bumper_intersects = true; - break; - } - } - - if (!front_bumper_intersects) { - float other_corners[4][2]; - compute_agent_corners(other, other_corners); - for (int i = 0; i < 4; i++) { - int next = (i + 1) % 4; - if (check_line_intersection(agent_corners[0], agent_corners[1], other_corners[i], other_corners[next])) { - front_bumper_intersects = true; - break; - } - } - } + 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, + agent_corners); + + // Front bumper = segment between front-left (corner 0) and front-right (corner 1). + // Transform into other's local frame and test vs its origin-centered AABB: this catches both + // a bumper corner inside other and the bumper edge crossing other's boundary. + float front_left_local[2], front_right_local[2]; + project_point_to_local( + agent_corners[0][0], + agent_corners[0][1], + other->sim_x, + other->sim_y, + other->cos_heading, + other->sin_heading, + &front_left_local[0], + &front_left_local[1]); + project_point_to_local( + agent_corners[1][0], + agent_corners[1][1], + other->sim_x, + other->sim_y, + other->cos_heading, + other->sin_heading, + &front_right_local[0], + &front_right_local[1]); + bool front_bumper_intersects = check_segment_intersects_aabb( + front_left_local, + front_right_local, + other->sim_length / 2.0f, + other->sim_width / 2.0f); if (front_bumper_intersects) { return true; @@ -2440,52 +2644,30 @@ static inline void compute_pairwise_ttc(Agent *ego, Agent *other) { return; } - float ego_front_x = ego_x + 0.5f * ego->sim_length * ego_heading_x; - float ego_front_y = ego_y + 0.5f * ego->sim_length * ego_heading_y; - float ego_rear_x = ego_x - 0.5f * ego->sim_length * ego_heading_x; - float ego_rear_y = ego_y - 0.5f * ego->sim_length * ego_heading_y; - float ego_left_x = -ego_heading_y; - float ego_left_y = ego_heading_x; - float ego_half_width = 0.5f * ego->sim_width; - - float other_front_x = other_x + 0.5f * other->sim_length * other_heading_x; - float other_front_y = other_y + 0.5f * other->sim_length * other_heading_y; - float other_rear_x = other_x - 0.5f * other->sim_length * other_heading_x; - float other_rear_y = other_y - 0.5f * other->sim_length * other_heading_y; - float other_left_x = -other_heading_y; - float other_left_y = other_heading_x; - float other_half_width = 0.5f * other->sim_width; - - float ego_corners_x[4] = { - ego_front_x + ego_half_width * ego_left_x, - ego_front_x - ego_half_width * ego_left_x, - ego_rear_x + ego_half_width * ego_left_x, - ego_rear_x - ego_half_width * ego_left_x, - }; - float ego_corners_y[4] = { - ego_front_y + ego_half_width * ego_left_y, - ego_front_y - ego_half_width * ego_left_y, - ego_rear_y + ego_half_width * ego_left_y, - ego_rear_y - ego_half_width * ego_left_y, - }; - float other_corners_x[4] = { - other_front_x + other_half_width * other_left_x, - other_front_x - other_half_width * other_left_x, - other_rear_x + other_half_width * other_left_x, - other_rear_x - other_half_width * other_left_x, - }; - float other_corners_y[4] = { - other_front_y + other_half_width * other_left_y, - other_front_y - other_half_width * other_left_y, - other_rear_y + other_half_width * other_left_y, - other_rear_y - other_half_width * other_left_y, - }; + float ego_corners[4][2]; + float other_corners[4][2]; + compute_bounding_box_corners( + ego->sim_x, + ego->sim_y, + ego->cos_heading, + ego->sin_heading, + ego->sim_length / 2.0f, + ego->sim_width / 2.0f, + ego_corners); + compute_bounding_box_corners( + other->sim_x, + other->sim_y, + other->cos_heading, + other->sin_heading, + other->sim_length / 2.0f, + other->sim_width / 2.0f, + other_corners); float min_dtc_sq = DEFAULT_DTC * DEFAULT_DTC; for (int ego_corner = 0; ego_corner < 4; ego_corner++) { for (int other_corner = 0; other_corner < 4; other_corner++) { - float dx = ego_corners_x[ego_corner] - other_corners_x[other_corner]; - float dy = ego_corners_y[ego_corner] - other_corners_y[other_corner]; + float dx = ego_corners[ego_corner][0] - other_corners[other_corner][0]; + float dy = ego_corners[ego_corner][1] - other_corners[other_corner][1]; min_dtc_sq = fminf(min_dtc_sq, dx * dx + dy * dy); } } @@ -2969,9 +3151,6 @@ static bool check_spawn_offroad(Drive *env, Agent *tmp_agent) { scaled.sim_length *= 1.1f; scaled.sim_width *= 1.1f; - float corners[4][2]; - compute_agent_corners(&scaled, corners); - GridMapEntity entity_list[ROAD_QUERY_ENTITY_COUNT]; int list_size = get_neighbors_entities( env, @@ -2992,25 +3171,19 @@ static bool check_spawn_offroad(Drive *env, Agent *tmp_agent) { if (abs_dz > Z_BUFFER) { continue; } - float start[2] = {element->x[geometry_idx], element->y[geometry_idx]}; - float end[2] = {element->x[geometry_idx + 1], element->y[geometry_idx + 1]}; - for (int k = 0; k < 4; k++) { - int next = (k + 1) % 4; - if (check_line_intersection(corners[k], corners[next], start, end)) { - return true; - } + if (check_segment_crosses_moving_box( + element->x[geometry_idx], + element->y[geometry_idx], + element->x[geometry_idx + 1], + element->y[geometry_idx + 1], + &scaled)) { + return true; } } } return false; } -static bool check_spawn_red_light_violation(Drive *env, Agent *tmp_agent) { - float corners[4][2]; - compute_agent_corners(tmp_agent, corners); - return check_stop_line_crossing(env, tmp_agent, corners); -} - static bool spawn_agent(Drive *env, int agent_idx, int num_agents) { Agent *agent = &env->agents[agent_idx]; @@ -3094,10 +3267,16 @@ static bool spawn_agent(Drive *env, int agent_idx, int num_agents) { tmp_agent.sim_heading = spawn_heading; tmp_agent.cos_heading = cosf(spawn_heading); tmp_agent.sin_heading = sinf(spawn_heading); + // Spawn pose is static: prev == curr makes the moving-box checks degenerate to static. + tmp_agent.prev_x = spawn_x; + tmp_agent.prev_y = spawn_y; + tmp_agent.prev_cos_heading = tmp_agent.cos_heading; + tmp_agent.prev_sin_heading = tmp_agent.sin_heading; tmp_agent.yaw_rate = 0.0f; tmp_agent.sim_length = spawn_length; tmp_agent.sim_width = spawn_width; tmp_agent.sim_height = spawn_height; + update_agent_radius(&tmp_agent); tmp_agent.current_lane_idx = start_lane_idx; if (check_spawn_collision(env, num_agents, &tmp_agent)) { @@ -3108,7 +3287,7 @@ static bool spawn_agent(Drive *env, int agent_idx, int num_agents) { continue; } - if (check_spawn_red_light_violation(env, &tmp_agent)) { + if (check_stop_line_crossing(env, &tmp_agent)) { continue; } @@ -3128,9 +3307,11 @@ static bool spawn_agent(Drive *env, int agent_idx, int num_agents) { agent->sim_heading = spawn_heading; agent->cos_heading = cosf(spawn_heading); agent->sin_heading = sinf(spawn_heading); + copy_pose_to_prev(agent); agent->sim_length = spawn_length; agent->sim_width = spawn_width; agent->sim_height = spawn_height; + update_agent_radius(agent); agent->sim_valid = 1; agent->wheelbase = 0.6f * spawn_length; agent->current_lane_idx = start_lane_idx; @@ -3194,8 +3375,9 @@ static void set_start_position(Drive *env) { agent->sim_length = agent->log_length[step]; agent->sim_width = agent->log_width[step]; agent->sim_height = agent->log_height[step]; - // Estimate wheelbase as 60% of length + update_agent_radius(agent); agent->wheelbase = 0.6f * agent->sim_length; + copy_pose_to_prev(agent); if (agent->type == UNKNOWN) { continue; @@ -3445,6 +3627,7 @@ void move_expert(Drive *env, int agent_idx) { agent->sim_length = agent->log_length[t]; agent->sim_width = agent->log_width[t]; agent->sim_height = agent->log_height[t]; + update_agent_radius(agent); agent->wheelbase = 0.6f * agent->sim_length; } agent->yaw_rate = compute_log_yaw_rate(agent, t, env->dt); @@ -3452,6 +3635,15 @@ void move_expert(Drive *env, int agent_idx) { agent->sim_vy = agent->log_velocity_y[t]; update_agent_speed(agent); agent->sim_valid = agent->log_valid[t]; + + if (t == 0 || agent->log_valid[t - 1] == 0) { + copy_pose_to_prev(agent); + } else { + agent->prev_x = agent->log_trajectory_x[t - 1]; + agent->prev_y = agent->log_trajectory_y[t - 1]; + agent->prev_cos_heading = cosf(agent->log_heading[t - 1]); + agent->prev_sin_heading = sinf(agent->log_heading[t - 1]); + } } void remove_bad_trajectories(Drive *env) { @@ -3931,9 +4123,6 @@ static void compute_metrics(Drive *env, int agent_idx, int log_idx) { float best_candidate_signed_lane_distance = 0.0f; float best_candidate_lane_heading = 0.0f; - float corners[4][2]; - compute_agent_corners(agent, corners); - GridMapEntity entity_list[ROAD_QUERY_ENTITY_COUNT]; int list_size = get_neighbors_entities( env, @@ -3961,21 +4150,18 @@ static void compute_metrics(Drive *env, int agent_idx, int log_idx) { int geometry_idx = entity_list[i].geometry_idx; RoadMapElement *element = &env->road_elements[entity_idx]; - // Check for offroad collision with road edges + // Check for offroad crossing with road edges if (is_road_edge(element->type)) { - float start[2] = {element->x[geometry_idx], element->y[geometry_idx]}; - float end[2] = {element->x[geometry_idx + 1], element->y[geometry_idx + 1]}; float abs_dz = fabsf(element->z[geometry_idx] - agent->sim_z); if (abs_dz > Z_BUFFER) { continue; } - for (int k = 0; k < 4; k++) { // Check each edge of the bounding box - int next = (k + 1) % 4; - if (check_line_intersection(corners[k], corners[next], start, end)) { - is_offroad = true; - break; - } - } + is_offroad = check_segment_crosses_moving_box( + element->x[geometry_idx], + element->y[geometry_idx], + element->x[geometry_idx + 1], + element->y[geometry_idx + 1], + agent); } if (is_offroad) { @@ -4178,11 +4364,18 @@ static void compute_metrics(Drive *env, int agent_idx, int log_idx) { return; } - float distance_to_goal - = compute_euclidean_distance(agent->sim_x, agent->sim_y, agent->goal_position_x, agent->goal_position_y); + // Goal reaching: swept check against the step's motion segment (prev -> sim), + // so a high dt cannot jump over the goal disc between two states. + float distance_to_goal = compute_point_to_segment_distance( + agent->goal_position_x, + agent->goal_position_y, + agent->prev_x, + agent->prev_y, + agent->sim_x, + agent->sim_y); float goal_z_dist = fabsf(agent->sim_z - agent->goal_position_z); - - // Goal reaching — guard against incrementing past num_target_waypoints + // Guard against incrementing past num_target_waypoints: replay mode leaves current_goal_idx + // saturated at num_target_waypoints, and the reached-goal condition must not fire again. if (agent->current_goal_idx < env->num_target_waypoints && distance_to_goal < agent->reward_coefs[REWARD_COEF_GOAL_RADIUS] && goal_z_dist < Z_BUFFER) { agent->metrics_array[REACHED_GOAL_IDX] = 1.0f; @@ -4454,8 +4647,6 @@ static int write_partner_obs(Drive *env, Agent *ego, int agent_idx, float *obs, typedef struct { int index; float dist_sq; - float dx; - float dy; float dz; } AgentDistance; AgentDistance nearby_agents[env->num_agents]; @@ -4480,8 +4671,6 @@ static int write_partner_obs(Drive *env, Agent *ego, int agent_idx, float *obs, } nearby_agents[nearby_count].index = index; nearby_agents[nearby_count].dist_sq = dist_sq; - nearby_agents[nearby_count].dx = dx; - nearby_agents[nearby_count].dy = dy; nearby_agents[nearby_count].dz = dz; nearby_count++; } @@ -4504,8 +4693,17 @@ static int write_partner_obs(Drive *env, Agent *ego, int agent_idx, float *obs, for (int j = 0; j < partners_to_write; j++) { Agent *other = &env->agents[nearby_agents[j].index]; - float rel_x, rel_y, rel_heading_x, rel_heading_y; - project_vector_to_ego_frame(ego, nearby_agents[j].dx, nearby_agents[j].dy, &rel_x, &rel_y); + float rel_x, rel_y, rel_heading_x, rel_heading_y, rel_vx, rel_vy; + project_point_to_ego_frame(ego, other->sim_x, other->sim_y, &rel_x, &rel_y); + project_point_to_local( + other->sim_vx, + other->sim_vy, + ego->sim_vx, + ego->sim_vy, + ego->cos_heading, + ego->sin_heading, + &rel_vx, + &rel_vy); project_vector_to_ego_frame(ego, other->cos_heading, other->sin_heading, &rel_heading_x, &rel_heading_y); obs[obs_idx++] = rel_x / env->obs_norm_xy_offset_m; obs[obs_idx++] = rel_y / env->obs_norm_xy_offset_m; @@ -4745,6 +4943,7 @@ static void compute_observations(Drive *env) { static void move_dynamics(Drive *env, int action_idx, int agent_idx) { Agent *agent = &env->agents[agent_idx]; + copy_pose_to_prev(agent); if (agent->removed) { invalidate_agent(agent); @@ -4996,6 +5195,9 @@ static void move_dynamics(Drive *env, int action_idx, int agent_idx) { void c_reset(Drive *env) { if (env->timestep == 0) { + for (int i = 0; i < env->num_total_agents; i++) { + copy_pose_to_prev(&env->agents[i]); + } for (int x = 0; x < env->active_agent_count; x++) { env->logs[x] = (Log) {0}; int agent_idx = env->active_agent_indices[x]; diff --git a/pufferlib/ocean/drive/idm.h b/pufferlib/ocean/drive/idm.h index 68e004cf21..4fa595f81d 100644 --- a/pufferlib/ocean/drive/idm.h +++ b/pufferlib/ocean/drive/idm.h @@ -135,6 +135,11 @@ static inline Agent idm_make_sample_agent(const Agent *ego, float x, float y, fl sample.sim_heading = normalize_heading(heading); sample.cos_heading = cosf(sample.sim_heading); sample.sin_heading = sinf(sample.sim_heading); + // Static probe: prev == cur so any swept geometry check degenerates to a static box test. + sample.prev_x = x; + sample.prev_y = y; + sample.prev_cos_heading = sample.cos_heading; + sample.prev_sin_heading = sample.sin_heading; sample.sim_length = ego->sim_length + 2.0f * IDM_BBOX_MARGIN; sample.sim_width = ego->sim_width + 2.0f * IDM_BBOX_MARGIN; sample.removed = 0; @@ -162,9 +167,6 @@ static int idm_boxes_overlap(const Agent *sample, const Agent *other) { return check_obb_collision(&sample_expanded, &other_expanded); } static int idm_sample_hits_red_light(Drive *env, Agent *sample, int lane_idx) { - float corners[4][2]; - compute_agent_corners(sample, corners); - for (int i = 0; i < env->num_traffic_elements; i++) { TrafficControlElement *traffic = &env->traffic_elements[i]; if (traffic->type != TRAFFIC_CONTROL_TYPE_TRAFFIC_LIGHT) { @@ -199,14 +201,8 @@ static int idm_sample_hits_red_light(Drive *env, Agent *sample, int lane_idx) { float ext_p1[2] = {traffic->stop_line[0] - ext * sl_dx, traffic->stop_line[1] - ext * sl_dy}; float ext_p2[2] = {traffic->stop_line[3] + ext * sl_dx, traffic->stop_line[4] + ext * sl_dy}; - for (int k = 0; k < 4; k++) { - if (k == 2) { - continue; - } - int next = (k + 1) % 4; - if (check_line_intersection(corners[k], corners[next], ext_p1, ext_p2)) { - return 1; - } + if (check_segment_crosses_moving_box(ext_p1[0], ext_p1[1], ext_p2[0], ext_p2[1], sample)) { + return 1; } } @@ -220,6 +216,11 @@ static inline void idm_update_sample_agent_pose(Agent *sample, RoadMapElement *l sample->sim_heading = normalize_heading(lane->headings[seg_idx]); sample->cos_heading = cosf(sample->sim_heading); sample->sin_heading = sinf(sample->sim_heading); + // Keep prev == cur so swept geometry checks stay static for the static probe. + sample->prev_x = sample->sim_x; + sample->prev_y = sample->sim_y; + sample->prev_cos_heading = sample->cos_heading; + sample->prev_sin_heading = sample->sin_heading; } static int idm_set_projected_agent_pose(Drive *env, Agent *agent, IDMLaneProjection projection, float distance) { diff --git a/pufferlib/ocean/drive/render.h b/pufferlib/ocean/drive/render.h index 51125da7e2..d50ad5fce1 100644 --- a/pufferlib/ocean/drive/render.h +++ b/pufferlib/ocean/drive/render.h @@ -30,7 +30,6 @@ const Color LIGHTGREEN = (Color) {152, 255, 152, 255}; typedef struct Client { float width; float height; - Texture2D puffers; Vector3 camera_target; float camera_zoom; Camera3D camera; @@ -349,17 +348,16 @@ Client *make_client(Drive *env) { client->egl_mode = 1; } #endif - client->puffers = LoadTexture("resources/puffers_128.png"); - client->cars[0] = LoadModel("resources/drive/RedCar.glb"); - client->cars[1] = LoadModel("resources/drive/WhiteCar.glb"); - client->cars[2] = LoadModel("resources/drive/BlueCar.glb"); - client->cars[3] = LoadModel("resources/drive/YellowCar.glb"); - client->cars[4] = LoadModel("resources/drive/GreenCar.glb"); - client->cars[5] = LoadModel("resources/drive/GreyCar.glb"); - client->cyclist = LoadModel("resources/drive/cyclist.glb"); - client->pedestrian = LoadModel("resources/drive/pedestrian.glb"); + client->cars[0] = LoadModel("pufferlib/resources/drive/RedCar.glb"); + client->cars[1] = LoadModel("pufferlib/resources/drive/WhiteCar.glb"); + client->cars[2] = LoadModel("pufferlib/resources/drive/BlueCar.glb"); + client->cars[3] = LoadModel("pufferlib/resources/drive/YellowCar.glb"); + client->cars[4] = LoadModel("pufferlib/resources/drive/GreenCar.glb"); + client->cars[5] = LoadModel("pufferlib/resources/drive/GreyCar.glb"); + client->cyclist = LoadModel("pufferlib/resources/drive/cyclist.glb"); + client->pedestrian = LoadModel("pufferlib/resources/drive/pedestrian.glb"); int animCountCyc = 0; - client->cycle_anim = LoadModelAnimations("resources/drive/cyclist.glb", &animCountCyc); + client->cycle_anim = LoadModelAnimations("pufferlib/resources/drive/cyclist.glb", &animCountCyc); for (int i = 0; i < MAX_AGENTS; i++) { client->car_assignments[i] = (rand() % 4) + 1; } @@ -1771,7 +1769,6 @@ void close_client(Client *client) { } UnloadModel(client->cyclist); UnloadModel(client->pedestrian); - UnloadTexture(client->puffers); // CRITICAL: skip CloseWindow + egl_headless_cleanup in egl_mode. // diff --git a/tests/drive/Makefile b/tests/drive/Makefile index a42602e3b2..106ba6d2cb 100644 --- a/tests/drive/Makefile +++ b/tests/drive/Makefile @@ -1,15 +1,56 @@ -CC = gcc -CFLAGS = -Wall -Wextra -g -std=c99 -I../../ -TARGET = test_error -SOURCE = test_error.c +CC ?= gcc +ROOT := ../.. +UNAME_S := $(shell uname -s) -$(TARGET): $(SOURCE) - $(CC) $(CFLAGS) -o $(TARGET) $(SOURCE) +ifeq ($(UNAME_S),Darwin) +RAYLIB_NAME := raylib-5.5_macos +LDLIBS_DRIVE := $(ROOT)/extern/$(RAYLIB_NAME)/lib/libraylib.a -framework Cocoa -framework OpenGL -framework IOKit -lm +else +RAYLIB_NAME := raylib-5.5_linux_amd64 +LDLIBS_DRIVE := $(ROOT)/extern/$(RAYLIB_NAME)/lib/libraylib.a -lGL -ldl -lpthread -lrt -lX11 -lm +ifneq ($(wildcard /usr/include/EGL/egl.h),) +LDLIBS_DRIVE += -lEGL +endif +endif -test: $(TARGET) - ./$(TARGET) +BUILD_DIR := build +CFLAGS := -O0 -g -std=c99 -DPLATFORM_DESKTOP -D_GNU_SOURCE -I$(ROOT) -I$(ROOT)/extern/$(RAYLIB_NAME)/include \ + -Wall -Wextra -Wno-unused-function -Wno-unused-variable -Wno-unused-parameter -Wno-implicit-function-declaration + +UNIT_SRCS := \ + test_error.c \ + test_drive_geometry.c \ + test_drive_observations_rewards.c \ + test_drive_map_cache.c \ + test_drive_idm.c +SMOKE_SRCS := test_drive_env_smoke.c +RENDER_SRCS := test_render_smoke.c +PERF_SRCS := test_perf.c + +UNIT_BINS := $(UNIT_SRCS:%.c=$(BUILD_DIR)/%) +SMOKE_BINS := $(SMOKE_SRCS:%.c=$(BUILD_DIR)/%) +RENDER_BINS := $(RENDER_SRCS:%.c=$(BUILD_DIR)/%) +PERF_BINS := $(PERF_SRCS:%.c=$(BUILD_DIR)/%) + +$(BUILD_DIR)/%: %.c include/test.h include/drive_fixture.h + mkdir -p $(BUILD_DIR) + $(CC) $(CFLAGS) $< $(LDLIBS_DRIVE) -o $@ + +unit: $(UNIT_BINS) + @for bin in $(UNIT_BINS); do (cd $(ROOT) && tests/drive/$$bin) || exit $$?; done + +smoke: $(SMOKE_BINS) + @for bin in $(SMOKE_BINS); do (cd $(ROOT) && tests/drive/$$bin) || exit $$?; done + +test: unit smoke + +render: $(RENDER_BINS) + @cd $(ROOT) && xvfb-run -a tests/drive/$(RENDER_BINS) + +perf: $(PERF_BINS) + @cd $(ROOT) && tests/drive/$(PERF_BINS) clean: - rm -f $(TARGET) + rm -rf $(BUILD_DIR) -.PHONY: test clean +.PHONY: unit smoke test render perf clean diff --git a/tests/drive/include/drive_fixture.h b/tests/drive/include/drive_fixture.h new file mode 100644 index 0000000000..b1bb368ec7 --- /dev/null +++ b/tests/drive/include/drive_fixture.h @@ -0,0 +1,181 @@ +#ifndef DRIVE_FIXTURE_H +#define DRIVE_FIXTURE_H + +#include "pufferlib/ocean/drive/drive.h" + +#ifndef DRIVE_TEST_REPO_ROOT +#define DRIVE_TEST_REPO_ROOT "." +#endif + +static inline const char *drive_carla_map(void) { + return DRIVE_TEST_REPO_ROOT "/pufferlib/resources/drive/binaries/carla/opendrive__Town01.bin"; +} + +static inline const char *drive_nuplan_map(void) { + return DRIVE_TEST_REPO_ROOT + "/pufferlib/resources/drive/binaries/nuplan/nuplan__00018a38-0063-54d1-a3c1-1ab931a4a1e5.bin"; +} + +static inline char *drive_test_strdup(const char *src) { + size_t n = strlen(src) + 1; + char *dst = (char *) malloc(n); + memcpy(dst, src, n); + return dst; +} + +static inline Agent drive_test_agent(float x, float y, float heading) { + Agent agent = {0}; + agent.type = VEHICLE; + agent.sim_x = x; + agent.sim_y = y; + agent.sim_z = 0.0f; + agent.sim_heading = heading; + agent.cos_heading = cosf(heading); + agent.sin_heading = sinf(heading); + agent.sim_length = 4.0f; + agent.sim_width = 2.0f; + agent.sim_height = 1.5f; + agent.sim_valid = 1; + agent.current_lane_idx = 1; + agent.previous_lane_idx = 1; + agent.wheelbase = 2.7f; + copy_pose_to_prev(&agent); + update_agent_radius(&agent); + return agent; +} + +static inline Drive drive_test_env_config( + const char *map_file, + int simulation_mode, + int num_agents, + int use_map_cache) { + Drive env = {0}; + env.render_mode = RENDER_WINDOW; + env.action_type = 0; + env.dynamics_model = CLASSIC; + env.reward_goal = 1.0f; + env.reward_collision = 3.0f; + env.reward_offroad = 3.0f; + env.reward_comfort = 0.05f; + env.reward_lane_align = 0.025f; + env.reward_vel_align = 1.0f; + env.reward_lane_center = 0.0038f; + env.reward_center_bias = 0.0f; + env.reward_velocity = 0.0025f; + env.reward_reverse = 0.005f; + env.reward_stop_line = 1.0f; + env.reward_timestep = 0.000025f; + env.reward_overspeed = 0.05f; + env.reward_ade = 0.0f; + env.collision_behavior = 0; + env.offroad_behavior = 0; + env.traffic_light_behavior = 0; + env.use_map_cache = use_map_cache; + env.emit_completed_episodes = 1; + env.goal_radius = 2.0f; + env.goal_speed = 3.0f; + env.min_waypoint_spacing = 20.0f; + env.max_waypoint_spacing = 60.0f; + env.num_target_waypoints = 3; + env.target_type = TARGET_STATIC; + env.goal_on_lane = 1; + env.obs_slots_lane_n = 32; + env.obs_slots_boundary_n = 32; + env.obs_slots_lane_kept = 32; + env.obs_slots_boundary_kept = 32; + env.obs_slots_partners_n = 16; + env.obs_slots_traffic_controls_n = 4; + env.traffic_control_scope = TRAFFIC_CONTROL_SCOPE_TRAFFIC_LIGHTS; + env.dt = 0.1f; + env.spawn_initial_speed = 0.0f; + env.scenario_length = 91; + env.termination_mode = 0; + env.inactive_agent_threshold = 0.4f; + env.map_name = drive_test_strdup(map_file); + env.num_controllable_agents = num_agents; + env.num_max_agents = 64; + env.init_step = 0; + env.timestep = 0; + env.init_mode = INIT_ALL_VALID; + env.control_mode = simulation_mode == SIMULATION_REPLAY ? CONTROL_SDC_ONLY : CONTROL_VEHICLES; + env.sdc_controller = CONTROLLER_POLICY; + env.non_sdc_controller = CONTROLLER_POLICY; + env.non_vehicle_controller = CONTROLLER_REPLAY; + env.simulation_mode = simulation_mode; + env.reward_conditioning = 0; + env.reward_randomization = 0; + env.compute_eval_metrics = 1; + env.eval_mode = 0; + env.obs_norm_goal_offset_m = 100.0f; + env.obs_norm_xy_offset_m = 100.0f; + env.obs_norm_veh_length_m = 15.0f; + env.obs_norm_veh_width_m = 10.0f; + env.obs_norm_road_seg_length_m = 5.0f; + env.obs_norm_road_seg_width_m = 5.0f; + env.obs_range_traffic_control_m = 100.0f; + env.obs_range_partner_m = 100.0f; + env.obs_range_road_front_m = 120.0f; + env.obs_range_road_behind_m = 20.0f; + env.obs_range_road_side_m = 30.0f; + env.partner_blindness_prob = 0.0f; + env.partner_blindness_trigger_prob = 0.1f; + env.phantom_braking_prob = 0.0f; + env.phantom_braking_trigger_prob = 0.0f; + env.phantom_braking_duration = 10; + return env; +} + +static inline Drive drive_test_make_env(const char *map_file, int simulation_mode, int num_agents, int use_map_cache) { + Drive env = drive_test_env_config(map_file, simulation_mode, num_agents, use_map_cache); + allocate(&env); + c_reset(&env); + return env; +} + +static inline void drive_set_neutral_actions(Drive *env) { + if (env->action_type == 0) { + int *actions = (int *) env->actions; + int neutral = env->dynamics_model == JERK ? 7 : 31; + for (int i = 0; i < env->active_agent_count; i++) { + actions[i] = neutral; + } + return; + } + float (*actions)[2] = (float (*)[2]) env->actions; + for (int i = 0; i < env->active_agent_count; i++) { + actions[i][0] = 0.0f; + actions[i][1] = 0.0f; + } +} + +static inline int drive_all_finite(const float *values, int count) { + for (int i = 0; i < count; i++) { + if (!isfinite(values[i])) { + return 0; + } + } + return 1; +} + +static inline int drive_map_cache_live_count(void) { + int live = 0; + for (int i = 0; i < g_map_cache_count; i++) { + if (g_map_cache[i] != NULL) { + live++; + } + } + return live; +} + +static inline void drive_map_cache_clear(void) { + for (int i = 0; i < g_map_cache_count; i++) { + if (g_map_cache[i] != NULL) { + free_shared_map_data(g_map_cache[i]); + } + } + free(g_map_cache); + g_map_cache = NULL; + g_map_cache_count = 0; +} + +#endif diff --git a/tests/drive/include/test.h b/tests/drive/include/test.h new file mode 100644 index 0000000000..11f75d5c75 --- /dev/null +++ b/tests/drive/include/test.h @@ -0,0 +1,67 @@ +#ifndef DRIVE_TEST_H +#define DRIVE_TEST_H + +#include +#include +#include +#include + +#define EXPECT_TRUE(expr) \ + do { \ + if (!(expr)) { \ + printf("FAIL %s:%d: %s\n", __FILE__, __LINE__, #expr); \ + return 1; \ + } \ + } while (0) + +#define EXPECT_FALSE(expr) EXPECT_TRUE(!(expr)) + +#define EXPECT_EQ_INT(actual, expected) \ + do { \ + int actual_value = (actual); \ + int expected_value = (expected); \ + if (actual_value != expected_value) { \ + printf("FAIL %s:%d: %s == %d, expected %d\n", __FILE__, __LINE__, #actual, actual_value, expected_value); \ + return 1; \ + } \ + } while (0) + +#define EXPECT_NEAR(actual, expected, tol) \ + do { \ + float actual_value = (actual); \ + float expected_value = (expected); \ + if (fabsf(actual_value - expected_value) > (tol)) { \ + printf("FAIL %s:%d: %s == %f, expected %f\n", __FILE__, __LINE__, #actual, actual_value, expected_value); \ + return 1; \ + } \ + } while (0) + +#define EXPECT_FINITE(value) \ + do { \ + float finite_value = (value); \ + if (!isfinite(finite_value)) { \ + printf("FAIL %s:%d: %s is not finite: %f\n", __FILE__, __LINE__, #value, finite_value); \ + return 1; \ + } \ + } while (0) + +#define RUN_TEST(test_fn) \ + do { \ + int failed = test_fn(); \ + if (failed) { \ + failures++; \ + } else { \ + printf("PASS %s\n", #test_fn); \ + } \ + } while (0) + +static inline int test_summary(int failures) { + if (failures) { + printf("%d test groups failed\n", failures); + return 1; + } + printf("All test groups passed\n"); + return 0; +} + +#endif diff --git a/tests/drive/test_drive_env_smoke.c b/tests/drive/test_drive_env_smoke.c new file mode 100644 index 0000000000..3477e659f1 --- /dev/null +++ b/tests/drive/test_drive_env_smoke.c @@ -0,0 +1,89 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +static int run_case(const char *name, const char *map_file, int simulation_mode, int num_agents) { + srand(7); + Drive env = drive_test_make_env(map_file, simulation_mode, num_agents, 0); + int obs_size = compute_observation_size(&env); + + EXPECT_TRUE(env.active_agent_count > 0); + EXPECT_TRUE(env.observations != NULL); + EXPECT_TRUE(env.rewards != NULL); + EXPECT_TRUE(drive_all_finite(env.observations, env.active_agent_count * obs_size)); + + int saw_log = 0; + for (int t = 0; t < env.scenario_length + 5; t++) { + drive_set_neutral_actions(&env); + c_step(&env); + EXPECT_TRUE(drive_all_finite(env.observations, env.active_agent_count * obs_size)); + EXPECT_TRUE(drive_all_finite(env.rewards, env.active_agent_count)); + if (env.completed_episodes_count > 0 || env.log.n > 0.0f) { + saw_log = 1; + } + for (int i = 0; i < env.active_agent_count; i++) { + Agent *agent = &env.agents[env.active_agent_indices[i]]; + int terminal_flags = (agent->metrics_array[COLLISION_IDX] > 0.0f) + + (agent->metrics_array[OFFROAD_IDX] > 0.0f) + (agent->metrics_array[RED_LIGHT_IDX] > 0.0f); + EXPECT_TRUE(terminal_flags <= 1); + } + } + + EXPECT_TRUE(saw_log); + printf( + "case %s active=%d log_n=%.0f completed=%d\n", + name, + env.active_agent_count, + env.log.n, + env.completed_episodes_count); + free_allocated(&env); + return 0; +} + +static int test_carla_gigaflow_load_step_log(void) { + return run_case("carla-gigaflow", drive_carla_map(), SIMULATION_GIGAFLOW, 32); +} + +static int test_nuplan_gigaflow_load_step_log(void) { + return run_case("nuplan-gigaflow", drive_nuplan_map(), SIMULATION_GIGAFLOW, 32); +} + +static int test_nuplan_replay_load_step_log(void) { + return run_case("nuplan-replay", drive_nuplan_map(), SIMULATION_REPLAY, 1); +} + +static int test_truncation_and_completed_episode_queue(void) { + srand(11); + Drive env = drive_test_env_config(drive_carla_map(), SIMULATION_GIGAFLOW, 8, 0); + env.scenario_length = 3; + allocate(&env); + c_reset(&env); + + for (int t = 0; t < 3; t++) { + drive_set_neutral_actions(&env); + c_step(&env); + } + + EXPECT_EQ_INT(env.completed_episodes_count, 1); + EXPECT_EQ_INT(env.next_episode_index, 1); + for (int i = 0; i < env.active_agent_count; i++) { + EXPECT_EQ_INT(env.truncations[i], 1); + } + + CompletedEpisodeSummary summary = {0}; + EXPECT_EQ_INT(pop_completed_episode_summary(&env, &summary), 1); + EXPECT_EQ_INT(env.completed_episodes_count, 0); + EXPECT_EQ_INT(summary.episode_index, 0); + EXPECT_NEAR(summary.n, (float) env.active_agent_count, 1e-5f); + + free_allocated(&env); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_carla_gigaflow_load_step_log); + RUN_TEST(test_nuplan_gigaflow_load_step_log); + RUN_TEST(test_nuplan_replay_load_step_log); + RUN_TEST(test_truncation_and_completed_episode_queue); + return test_summary(failures); +} diff --git a/tests/drive/test_drive_geometry.c b/tests/drive/test_drive_geometry.c new file mode 100644 index 0000000000..ff6a526e2e --- /dev/null +++ b/tests/drive/test_drive_geometry.c @@ -0,0 +1,273 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +static int test_heading_normalization(void) { + EXPECT_NEAR(normalize_heading(3.0f * (float) M_PI), (float) M_PI, 1e-5f); + EXPECT_NEAR(normalize_heading(-3.0f * (float) M_PI), -(float) M_PI, 1e-5f); + EXPECT_NEAR(compute_heading_diff(1.5f * (float) M_PI, 0.0f), -0.5f * (float) M_PI, 1e-5f); + EXPECT_NEAR(compute_heading_diff(-1.5f * (float) M_PI, 0.0f), 0.5f * (float) M_PI, 1e-5f); + return 0; +} + +static int test_point_to_segment_distance_cases(void) { + EXPECT_NEAR(compute_point_to_segment_distance(0.0f, 1.0f, -1.0f, 0.0f, 1.0f, 0.0f), 1.0f, 1e-5f); + EXPECT_NEAR(compute_point_to_segment_distance(2.0f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f), 1.0f, 1e-5f); + EXPECT_NEAR(compute_point_to_segment_distance(0.0f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f), 0.0f, 1e-5f); + EXPECT_NEAR(compute_point_to_segment_distance(3.0f, 4.0f, 0.0f, 0.0f, 0.0f, 0.0f), 5.0f, 1e-5f); + return 0; +} + +static int test_obb_collision_cases(void) { + Agent ego = drive_test_agent(0.0f, 0.0f, 0.0f); + Agent other = drive_test_agent(3.9f, 0.0f, 0.0f); + EXPECT_TRUE(check_obb_collision(&ego, &other)); + + other = drive_test_agent(4.1f, 0.0f, 0.0f); + EXPECT_FALSE(check_obb_collision(&ego, &other)); + + other = drive_test_agent(4.0f, 0.0f, 0.0f); + EXPECT_TRUE(check_obb_collision(&ego, &other)); + + other = drive_test_agent(0.0f, 0.0f, 0.0f); + other.sim_z = 2.0f; + EXPECT_FALSE(check_obb_collision(&ego, &other)); + + other = drive_test_agent(1.0f, 0.0f, (float) M_PI / 4.0f); + EXPECT_TRUE(check_obb_collision(&ego, &other)); + + other = drive_test_agent(4.4f, 0.0f, (float) M_PI / 4.0f); + EXPECT_FALSE(check_obb_collision(&ego, &other)); + + return 0; +} + +static int test_obb_collision_dimensions(void) { + Agent ego = drive_test_agent(0.0f, 0.0f, 0.0f); + ego.sim_length = 12.0f; + ego.sim_width = 2.5f; + update_agent_radius(&ego); + + Agent other = drive_test_agent(7.9f, 0.0f, 0.0f); + EXPECT_TRUE(check_obb_collision(&ego, &other)); + other = drive_test_agent(8.1f, 0.0f, 0.0f); + EXPECT_FALSE(check_obb_collision(&ego, &other)); + + ego = drive_test_agent(0.0f, 0.0f, 0.0f); + ego.sim_width = 1.0f; + update_agent_radius(&ego); + other = drive_test_agent(0.0f, 1.4f, 0.0f); + other.sim_width = 1.0f; + update_agent_radius(&other); + EXPECT_FALSE(check_obb_collision(&ego, &other)); + other = drive_test_agent(0.0f, 0.9f, 0.0f); + other.sim_width = 1.0f; + update_agent_radius(&other); + EXPECT_TRUE(check_obb_collision(&ego, &other)); + + return 0; +} + +static int test_moving_obb_collision_cases(void) { + Agent ego = drive_test_agent(8.0f, 0.0f, 0.0f); + Agent other = drive_test_agent(0.0f, 0.0f, 0.0f); + ego.prev_x = -8.0f; + EXPECT_TRUE(check_moving_obb_collision(&ego, &other, 16.0f, 0.0f)); + + ego = drive_test_agent(3.0f, 0.0f, 0.0f); + other = drive_test_agent(-3.0f, 0.0f, 0.0f); + ego.prev_x = -3.0f; + other.prev_x = 3.0f; + EXPECT_TRUE(check_moving_obb_collision(&ego, &other, 6.0f, 6.0f)); + + ego = drive_test_agent(0.0f, 0.0f, 0.0f); + other = drive_test_agent(0.0f, 3.0f, 0.0f); + ego.prev_x = -5.0f; + other.prev_x = -5.0f; + EXPECT_FALSE(check_moving_obb_collision(&ego, &other, 5.0f, 5.0f)); + + return 0; +} + +static int test_moving_obb_collision_dimensions(void) { + Agent ego = drive_test_agent(10.0f, 0.0f, 0.0f); + ego.sim_length = 2.0f; + ego.sim_width = 2.0f; + update_agent_radius(&ego); + ego.prev_x = -10.0f; + Agent other = drive_test_agent(0.0f, 0.0f, 0.0f); + other.sim_length = 1.0f; + other.sim_width = 1.0f; + update_agent_radius(&other); + EXPECT_TRUE(check_moving_obb_collision(&ego, &other, 20.0f, 0.0f)); + + other = drive_test_agent(0.0f, 2.0f, 0.0f); + other.sim_length = 1.0f; + other.sim_width = 1.0f; + update_agent_radius(&other); + EXPECT_FALSE(check_moving_obb_collision(&ego, &other, 20.0f, 0.0f)); + + return 0; +} + +static int test_collision_check_filters(void) { + Drive env = {0}; + Agent agents[5] = {0}; + int active_agent_indices[2] = {0, 1}; + int static_agent_indices[3] = {2, 3, 4}; + + agents[0] = drive_test_agent(0.0f, 0.0f, 0.0f); + agents[1] = drive_test_agent(0.0f, 0.0f, 0.0f); + agents[1].removed = 1; + agents[2] = drive_test_agent(INVALID_POSITION, 0.0f, 0.0f); + agents[3] = drive_test_agent(0.0f, 0.0f, 0.0f); + agents[3].sim_valid = 0; + agents[4] = drive_test_agent(3.9f, 0.0f, 0.0f); + + env.agents = agents; + env.num_agents = 5; + env.active_agent_count = 2; + env.active_agent_indices = active_agent_indices; + env.static_agent_indices = static_agent_indices; + + EXPECT_EQ_INT(collision_check(&env, 0), 4); + return 0; +} + +static int test_segment_aabb_cases(void) { + float through_a[2] = {-3.0f, 0.0f}; + float through_b[2] = {3.0f, 0.0f}; + EXPECT_TRUE(check_segment_intersects_aabb(through_a, through_b, 1.0f, 1.0f)); + + float parallel_a[2] = {-3.0f, 2.0f}; + float parallel_b[2] = {3.0f, 2.0f}; + EXPECT_FALSE(check_segment_intersects_aabb(parallel_a, parallel_b, 1.0f, 1.0f)); + + float inside[2] = {0.5f, 0.5f}; + EXPECT_TRUE(check_segment_intersects_aabb(inside, inside, 1.0f, 1.0f)); + + float outside[2] = {2.0f, 0.0f}; + EXPECT_FALSE(check_segment_intersects_aabb(outside, outside, 1.0f, 1.0f)); + + float tangent_a[2] = {-3.0f, 1.0f}; + float tangent_b[2] = {3.0f, 1.0f}; + EXPECT_TRUE(check_segment_intersects_aabb(tangent_a, tangent_b, 1.0f, 1.0f)); + + return 0; +} + +static int test_swept_line_box_cases(void) { + Agent agent = drive_test_agent(4.0f, 0.0f, 0.0f); + agent.prev_x = -4.0f; + EXPECT_TRUE(check_segment_crosses_moving_box(0.0f, -3.0f, 0.0f, 3.0f, &agent)); + + agent = drive_test_agent(0.0f, 5.0f, 0.0f); + agent.prev_y = -5.0f; + EXPECT_TRUE(check_segment_crosses_moving_box(-10.0f, 0.0f, 10.0f, 0.0f, &agent)); + + agent = drive_test_agent(4.0f, 0.0f, 0.0f); + agent.prev_x = -4.0f; + EXPECT_FALSE(check_segment_crosses_moving_box(-2.0f, 5.0f, 2.0f, 5.0f, &agent)); + + return 0; +} + +static int test_stop_line_red_light_gating(void) { + Drive env = {0}; + TrafficControlElement tc = {0}; + int red_state[1] = {TRAFFIC_CONTROL_STATE_RED}; + int state[1] = {TRAFFIC_CONTROL_STATE_RED}; + int controlled_lane[1] = {1}; + Agent agent = drive_test_agent(1.0f, 0.0f, 0.0f); + + agent.prev_x = -3.0f; + tc.type = TRAFFIC_CONTROL_TYPE_TRAFFIC_LIGHT; + tc.state_size = 1; + tc.states = state; + tc.stop_line[0] = 0.0f; + tc.stop_line[1] = -1.0f; + tc.stop_line[3] = 0.0f; + tc.stop_line[4] = 1.0f; + tc.heading = 0.0f; + tc.num_controlled_lanes = 1; + tc.controlled_lanes = controlled_lane; + env.num_traffic_elements = 1; + env.traffic_elements = &tc; + env.timestep = 0; + + EXPECT_TRUE(check_stop_line_crossing(&env, &agent)); + + state[0] = TRAFFIC_CONTROL_STATE_GREEN; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + state[0] = TRAFFIC_CONTROL_STATE_YELLOW; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + state[0] = TRAFFIC_CONTROL_STATE_OFF; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + state[0] = TRAFFIC_CONTROL_STATE_RED; + + env.timestep = 1; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + env.timestep = 0; + + controlled_lane[0] = 2; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + controlled_lane[0] = 1; + + agent = drive_test_agent(1.0f, 0.0f, (float) M_PI); + agent.prev_x = -3.0f; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + + agent = drive_test_agent(11.0f, 0.0f, 0.0f); + agent.prev_x = -1.0f; + EXPECT_FALSE(check_stop_line_crossing(&env, &agent)); + + agent = drive_test_agent(1.0f, 1.4f, 0.0f); + agent.prev_x = -3.0f; + agent.prev_y = 1.4f; + agent.sim_width = 0.4f; + update_agent_radius(&agent); + tc.states = red_state; + EXPECT_TRUE(check_stop_line_crossing(&env, &agent)); + + return 0; +} + +static int test_stop_line_stationary_on_red(void) { + Drive env = {0}; + TrafficControlElement tc = {0}; + int state[1] = {TRAFFIC_CONTROL_STATE_RED}; + int controlled_lane[1] = {1}; + Agent agent = drive_test_agent(0.0f, 0.0f, 0.0f); + + tc.type = TRAFFIC_CONTROL_TYPE_TRAFFIC_LIGHT; + tc.state_size = 1; + tc.states = state; + tc.stop_line[0] = 0.0f; + tc.stop_line[1] = -1.0f; + tc.stop_line[3] = 0.0f; + tc.stop_line[4] = 1.0f; + tc.heading = 0.0f; + tc.num_controlled_lanes = 1; + tc.controlled_lanes = controlled_lane; + env.num_traffic_elements = 1; + env.traffic_elements = &tc; + env.timestep = 0; + + EXPECT_TRUE(check_stop_line_crossing(&env, &agent)); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_heading_normalization); + RUN_TEST(test_point_to_segment_distance_cases); + RUN_TEST(test_obb_collision_cases); + RUN_TEST(test_obb_collision_dimensions); + RUN_TEST(test_moving_obb_collision_cases); + RUN_TEST(test_moving_obb_collision_dimensions); + RUN_TEST(test_collision_check_filters); + RUN_TEST(test_segment_aabb_cases); + RUN_TEST(test_swept_line_box_cases); + RUN_TEST(test_stop_line_red_light_gating); + RUN_TEST(test_stop_line_stationary_on_red); + return test_summary(failures); +} diff --git a/tests/drive/test_drive_idm.c b/tests/drive/test_drive_idm.c new file mode 100644 index 0000000000..351d625c99 --- /dev/null +++ b/tests/drive/test_drive_idm.c @@ -0,0 +1,208 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +static void make_lane(RoadMapElement *lane, float *x, float *y, float *z, float *headings, float speed_limit) { + lane->type = LANE_SURFACE_STREET; + lane->segment_size = 2; + lane->x = x; + lane->y = y; + lane->z = z; + lane->headings = headings; + lane->speed_limit = speed_limit; +} + +static int test_leader_defaults_and_update(void) { + IDMLeader leader = idm_no_leader(); + EXPECT_EQ_INT(leader.has_leader, 0); + EXPECT_EQ_INT(leader.leader_agent_idx, -1); + EXPECT_TRUE(isinf(leader.gap)); + + idm_update_best_leader(&leader, 3, 0, -1.0f, -5.0f); + EXPECT_EQ_INT(leader.has_leader, 1); + EXPECT_EQ_INT(leader.leader_agent_idx, 3); + EXPECT_NEAR(leader.gap, IDM_MINIMUM_LEAD_DISTANCE, 1e-5f); + EXPECT_NEAR(leader.leader_speed, 0.0f, 1e-5f); + + idm_update_best_leader(&leader, 4, 0, 10.0f, 3.0f); + EXPECT_EQ_INT(leader.leader_agent_idx, 3); + idm_update_best_leader(&leader, 5, 1, 0.05f, 7.0f); + EXPECT_EQ_INT(leader.leader_agent_idx, 5); + EXPECT_EQ_INT(leader.is_traffic_light, 1); + EXPECT_NEAR(leader.gap, IDM_MINIMUM_LEAD_DISTANCE, 1e-5f); + EXPECT_NEAR(leader.leader_speed, 7.0f, 1e-5f); + return 0; +} + +static int test_z_overlap(void) { + Agent a = drive_test_agent(0.0f, 0.0f, 0.0f); + Agent b = drive_test_agent(0.0f, 0.0f, 0.0f); + b.sim_z = 1.4f; + EXPECT_TRUE(idm_check_z_overlap(&a, &b)); + b.sim_z = 2.0f; + EXPECT_FALSE(idm_check_z_overlap(&a, &b)); + return 0; +} + +static int test_desired_speed_fallback_and_lane_limit(void) { + Drive env = {0}; + RoadMapElement lanes[2] = {0}; + float x0[2] = {0.0f, 10.0f}; + float y0[2] = {0.0f, 0.0f}; + float z0[2] = {0.0f, 0.0f}; + float h0[1] = {0.0f}; + float x1[2] = {10.0f, 20.0f}; + float y1[2] = {0.0f, 0.0f}; + float z1[2] = {0.0f, 0.0f}; + float h1[1] = {0.0f}; + int route[1] = {1}; + Agent agent = drive_test_agent(0.0f, 0.0f, 0.0f); + + make_lane(&lanes[0], x0, y0, z0, h0, 0.0f); + make_lane(&lanes[1], x1, y1, z1, h1, 12.0f); + env.road_elements = lanes; + env.num_road_elements = 2; + agent.current_lane_idx = 0; + agent.route = route; + agent.route_length = 1; + agent.current_route_index = 0; + EXPECT_NEAR(idm_desired_speed(&env, &agent), 12.0f, 1e-5f); + + lanes[1].speed_limit = 0.0f; + EXPECT_NEAR(idm_desired_speed(&env, &agent), IDM_DEFAULT_DESIRED_SPEED, 1e-5f); + return 0; +} + +static int test_route_projection_and_advance(void) { + Drive env = {0}; + RoadMapElement lanes[2] = {0}; + float x0[2] = {0.0f, 10.0f}; + float y0[2] = {0.0f, 0.0f}; + float z0[2] = {0.0f, 0.0f}; + float h0[1] = {0.0f}; + float x1[2] = {10.0f, 20.0f}; + float y1[2] = {0.0f, 0.0f}; + float z1[2] = {0.0f, 0.0f}; + float h1[1] = {0.0f}; + int route[2] = {0, 1}; + Agent agent = drive_test_agent(5.0f, 0.0f, 0.0f); + + make_lane(&lanes[0], x0, y0, z0, h0, 10.0f); + make_lane(&lanes[1], x1, y1, z1, h1, 10.0f); + env.road_elements = lanes; + env.num_road_elements = 2; + agent.route = route; + agent.route_length = 2; + agent.current_route_index = 0; + + IDMLaneProjection projection = idm_project_to_route_lanes(&env, &agent); + EXPECT_EQ_INT(projection.valid, 1); + EXPECT_EQ_INT(projection.lane_idx, 0); + EXPECT_EQ_INT(projection.segment_idx, 0); + EXPECT_NEAR(projection.t, 0.5f, 1e-5f); + + EXPECT_EQ_INT(idm_set_projected_agent_pose(&env, &agent, projection, 7.0f), 1); + EXPECT_NEAR(agent.sim_x, 12.0f, 1e-5f); + EXPECT_NEAR(agent.sim_y, 0.0f, 1e-5f); + + agent = drive_test_agent(5.0f, 0.0f, 0.0f); + agent.route = route; + agent.route_length = 2; + agent.current_route_index = 0; + env.agents = &agent; + float old_heading = 0.0f; + EXPECT_EQ_INT(idm_advance_along_route_lanes(&env, 0, 7.0f, &old_heading), 1); + EXPECT_EQ_INT(agent.current_route_index, 1); + EXPECT_NEAR(agent.sim_x, 12.0f, 1e-5f); + return 0; +} + +static int test_red_yellow_traffic_light_obstacle(void) { + Drive env = {0}; + TrafficControlElement tc = {0}; + int state[1] = {TRAFFIC_CONTROL_STATE_RED}; + int controlled[1] = {2}; + Agent sample = drive_test_agent(5.0f, 0.0f, 0.0f); + sample.prev_x = 4.0f; + + tc.type = TRAFFIC_CONTROL_TYPE_TRAFFIC_LIGHT; + tc.state_size = 1; + tc.states = state; + tc.stop_line[0] = 5.0f; + tc.stop_line[1] = -2.0f; + tc.stop_line[3] = 5.0f; + tc.stop_line[4] = 2.0f; + tc.heading = 0.0f; + tc.num_controlled_lanes = 1; + tc.controlled_lanes = controlled; + env.traffic_elements = &tc; + env.num_traffic_elements = 1; + env.timestep = 0; + + EXPECT_TRUE(idm_sample_hits_red_light(&env, &sample, 2)); + state[0] = TRAFFIC_CONTROL_STATE_YELLOW; + EXPECT_TRUE(idm_sample_hits_red_light(&env, &sample, 2)); + state[0] = TRAFFIC_CONTROL_STATE_GREEN; + EXPECT_FALSE(idm_sample_hits_red_light(&env, &sample, 2)); + EXPECT_FALSE(idm_sample_hits_red_light(&env, &sample, 3)); + return 0; +} + +static int test_leader_selection_and_move_idm(void) { + Drive env = {0}; + RoadMapElement lane = {0}; + float x[2] = {0.0f, 50.0f}; + float y[2] = {0.0f, 0.0f}; + float z[2] = {0.0f, 0.0f}; + float h[1] = {0.0f}; + Agent agents[2] = {0}; + int active[1] = {0}; + int statics[1] = {1}; + int ego_route[1] = {0}; + int other_route[1] = {0}; + + make_lane(&lane, x, y, z, h, 10.0f); + agents[0] = drive_test_agent(0.0f, 0.0f, 0.0f); + agents[1] = drive_test_agent(10.0f, 0.0f, 0.0f); + agents[0].route = ego_route; + agents[0].route_length = 1; + agents[0].current_route_index = 0; + agents[0].current_lane_idx = 0; + agents[1].route = other_route; + agents[1].route_length = 1; + agents[1].current_route_index = 0; + agents[1].current_lane_idx = 0; + agents[1].sim_vx = 2.0f; + update_agent_speed(&agents[1]); + + env.road_elements = &lane; + env.num_road_elements = 1; + env.agents = agents; + env.active_agent_indices = active; + env.static_agent_indices = statics; + env.active_agent_count = 1; + env.static_agent_count = 1; + env.num_agents = 2; + env.dt = 0.1f; + + IDMLeader leader = idm_find_leader_by_route_boxes(&env, 0); + EXPECT_EQ_INT(leader.has_leader, 1); + EXPECT_EQ_INT(leader.leader_agent_idx, 1); + EXPECT_FALSE(leader.is_traffic_light); + + float before = agents[0].sim_x; + move_idm(&env, 0); + EXPECT_TRUE(agents[0].sim_x >= before); + EXPECT_FINITE(agents[0].sim_speed); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_leader_defaults_and_update); + RUN_TEST(test_z_overlap); + RUN_TEST(test_desired_speed_fallback_and_lane_limit); + RUN_TEST(test_route_projection_and_advance); + RUN_TEST(test_red_yellow_traffic_light_obstacle); + RUN_TEST(test_leader_selection_and_move_idm); + return test_summary(failures); +} diff --git a/tests/drive/test_drive_map_cache.c b/tests/drive/test_drive_map_cache.c new file mode 100644 index 0000000000..ef99a89b8a --- /dev/null +++ b/tests/drive/test_drive_map_cache.c @@ -0,0 +1,137 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +#include +#include + +static int test_obs_reward_done_parity_cache_on_vs_off(void) { + const int steps = 20; + srand(12345); + Drive off = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 32, 0); + int obs_count = off.active_agent_count * compute_observation_size(&off); + int agent_count = off.active_agent_count; + float *obs_log = (float *) malloc(steps * obs_count * sizeof(float)); + float *rew_log = (float *) malloc(steps * agent_count * sizeof(float)); + unsigned char *term_log = (unsigned char *) malloc(steps * agent_count * sizeof(unsigned char)); + unsigned char *trunc_log = (unsigned char *) malloc(steps * agent_count * sizeof(unsigned char)); + + for (int t = 0; t < steps; t++) { + drive_set_neutral_actions(&off); + c_step(&off); + memcpy(&obs_log[t * obs_count], off.observations, obs_count * sizeof(float)); + memcpy(&rew_log[t * agent_count], off.rewards, agent_count * sizeof(float)); + memcpy(&term_log[t * agent_count], off.terminals, agent_count * sizeof(unsigned char)); + memcpy(&trunc_log[t * agent_count], off.truncations, agent_count * sizeof(unsigned char)); + } + free_allocated(&off); + + srand(12345); + Drive on = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 32, 1); + EXPECT_EQ_INT(on.active_agent_count, agent_count); + EXPECT_EQ_INT(on.active_agent_count * compute_observation_size(&on), obs_count); + for (int t = 0; t < steps; t++) { + drive_set_neutral_actions(&on); + c_step(&on); + EXPECT_EQ_INT(memcmp(&obs_log[t * obs_count], on.observations, obs_count * sizeof(float)), 0); + EXPECT_EQ_INT(memcmp(&rew_log[t * agent_count], on.rewards, agent_count * sizeof(float)), 0); + EXPECT_EQ_INT(memcmp(&term_log[t * agent_count], on.terminals, agent_count * sizeof(unsigned char)), 0); + EXPECT_EQ_INT(memcmp(&trunc_log[t * agent_count], on.truncations, agent_count * sizeof(unsigned char)), 0); + } + free_allocated(&on); + free(obs_log); + free(rew_log); + free(term_log); + free(trunc_log); + drive_map_cache_clear(); + return 0; +} + +static int close_order_case(const int *order) { + drive_map_cache_clear(); + srand(9); + Drive envs[3]; + for (int i = 0; i < 3; i++) { + envs[i] = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 8, 1); + drive_set_neutral_actions(&envs[i]); + c_step(&envs[i]); + } + EXPECT_EQ_INT(g_map_cache_count, 1); + EXPECT_EQ_INT(drive_map_cache_live_count(), 1); + EXPECT_EQ_INT(g_map_cache[0]->ref_count, 3); + for (int i = 0; i < 3; i++) { + free_allocated(&envs[order[i]]); + } + EXPECT_EQ_INT(drive_map_cache_live_count(), 0); + drive_map_cache_clear(); + return 0; +} + +static int test_multi_env_close_orderings_do_not_crash(void) { + const int order_a[3] = {0, 1, 2}; + const int order_b[3] = {2, 1, 0}; + const int order_c[3] = {1, 2, 0}; + const int order_d[3] = {1, 0, 2}; + EXPECT_EQ_INT(close_order_case(order_a), 0); + EXPECT_EQ_INT(close_order_case(order_b), 0); + EXPECT_EQ_INT(close_order_case(order_c), 0); + EXPECT_EQ_INT(close_order_case(order_d), 0); + return 0; +} + +static int test_cache_size_bounded_by_unique_maps(void) { + drive_map_cache_clear(); + for (int cycle = 0; cycle < 3; cycle++) { + Drive env = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 8, 1); + free_allocated(&env); + EXPECT_EQ_INT(g_map_cache_count, 1); + EXPECT_EQ_INT(drive_map_cache_live_count(), 0); + } + drive_map_cache_clear(); + return 0; +} + +static int test_forked_child_can_build_and_free_its_own_entry(void) { + drive_map_cache_clear(); + Drive warm = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 8, 1); + free_allocated(&warm); + int parent_size_before_fork = g_map_cache_count; + int fds[2]; + EXPECT_EQ_INT(pipe(fds), 0); + + pid_t pid = fork(); + if (pid == 0) { + close(fds[0]); + Drive child = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 8, 1); + int live_after_build = drive_map_cache_live_count(); + free_allocated(&child); + int live_after_close = drive_map_cache_live_count(); + int payload[2] = {live_after_build, live_after_close}; + write(fds[1], payload, sizeof(payload)); + close(fds[1]); + _exit(0); + } + + close(fds[1]); + int payload[2] = {-1, -1}; + int status = 0; + read(fds[0], payload, sizeof(payload)); + close(fds[0]); + waitpid(pid, &status, 0); + EXPECT_TRUE(WIFEXITED(status)); + EXPECT_EQ_INT(WEXITSTATUS(status), 0); + EXPECT_EQ_INT(payload[0], 1); + EXPECT_EQ_INT(payload[1], 0); + EXPECT_EQ_INT(g_map_cache_count, parent_size_before_fork); + + drive_map_cache_clear(); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_obs_reward_done_parity_cache_on_vs_off); + RUN_TEST(test_multi_env_close_orderings_do_not_crash); + RUN_TEST(test_cache_size_bounded_by_unique_maps); + RUN_TEST(test_forked_child_can_build_and_free_its_own_entry); + return test_summary(failures); +} diff --git a/tests/drive/test_drive_observations_rewards.c b/tests/drive/test_drive_observations_rewards.c new file mode 100644 index 0000000000..dce02a3762 --- /dev/null +++ b/tests/drive/test_drive_observations_rewards.c @@ -0,0 +1,179 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +static int test_observation_size_formula(void) { + Drive env = {0}; + env.target_type = TARGET_STATIC; + env.num_target_waypoints = 3; + env.reward_conditioning = 0; + env.obs_slots_partners_n = 2; + env.obs_slots_lane_kept = 5; + env.obs_slots_boundary_kept = 7; + env.obs_slots_traffic_controls_n = 4; + int expected = EGO_FEATURES + 3 * STATIC_TARGET_FEATURES + 2 * PARTNER_FEATURES + 12 * ROAD_FEATURES + + 4 * TRAFFIC_CONTROL_FEATURES + OBS_VALID_COUNT_FEATURES; + EXPECT_EQ_INT(compute_observation_size(&env), expected); + + env.target_type = TARGET_DYNAMIC; + env.reward_conditioning = 1; + expected = EGO_FEATURES + NUM_REWARD_COEFS + 3 * DYNAMIC_TARGET_FEATURES + 2 * PARTNER_FEATURES + 12 * ROAD_FEATURES + + 4 * TRAFFIC_CONTROL_FEATURES + OBS_VALID_COUNT_FEATURES; + EXPECT_EQ_INT(compute_observation_size(&env), expected); + return 0; +} + +static int test_observation_zero_fill_and_valid_counts(void) { + srand(3); + Drive env = drive_test_env_config(drive_carla_map(), SIMULATION_GIGAFLOW, 1, 0); + env.obs_slots_partners_n = 4; + env.obs_slots_lane_n = 8; + env.obs_slots_boundary_n = 8; + env.obs_slots_lane_kept = 4; + env.obs_slots_boundary_kept = 4; + env.obs_slots_traffic_controls_n = 2; + allocate(&env); + c_reset(&env); + + EXPECT_EQ_INT(env.active_agent_count, 1); + int obs_size = compute_observation_size(&env); + float *obs = env.observations; + int partner_base = EGO_FEATURES + env.num_target_waypoints * STATIC_TARGET_FEATURES; + int road_base = partner_base + env.obs_slots_partners_n * PARTNER_FEATURES; + int traffic_base = road_base + (env.obs_slots_lane_kept + env.obs_slots_boundary_kept) * ROAD_FEATURES; + int valid_base = obs_size - OBS_VALID_COUNT_FEATURES; + + EXPECT_NEAR(obs[valid_base + 2], 0.0f, 1e-5f); + EXPECT_TRUE(obs[valid_base] >= 0.0f && obs[valid_base] <= (float) env.obs_slots_lane_kept); + EXPECT_TRUE(obs[valid_base + 1] >= 0.0f && obs[valid_base + 1] <= (float) env.obs_slots_boundary_kept); + EXPECT_TRUE(obs[valid_base + 3] >= 0.0f && obs[valid_base + 3] <= (float) env.obs_slots_traffic_controls_n); + + for (int i = 0; i < env.obs_slots_partners_n * PARTNER_FEATURES; i++) { + EXPECT_NEAR(obs[partner_base + i], 0.0f, 1e-6f); + } + for (int i = traffic_base + (int) obs[valid_base + 3] * TRAFFIC_CONTROL_FEATURES; i < valid_base; i++) { + EXPECT_NEAR(obs[i], 0.0f, 1e-6f); + } + + free_allocated(&env); + return 0; +} + +static int test_metric_terminal_flags_are_exclusive(void) { + srand(5); + Drive env = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 2, 0); + int agent_idx = env.active_agent_indices[0]; + Agent *agent = &env.agents[agent_idx]; + agent->sim_x = env.grid_map->top_left_x - 1000.0f; + agent->sim_y = env.grid_map->top_left_y + 1000.0f; + copy_pose_to_prev(agent); + + compute_metrics(&env, agent_idx, 0); + + EXPECT_NEAR(agent->metrics_array[OFFROAD_IDX], 1.0f, 1e-5f); + EXPECT_NEAR(agent->metrics_array[COLLISION_IDX], 0.0f, 1e-5f); + EXPECT_NEAR(agent->metrics_array[RED_LIGHT_IDX], 0.0f, 1e-5f); + + free_allocated(&env); + return 0; +} + +static void init_reward_env(Drive *env, Agent *agent, Log *log, int *active, float *reward) { + memset(env, 0, sizeof(*env)); + memset(agent, 0, sizeof(*agent)); + memset(log, 0, sizeof(*log)); + *agent = drive_test_agent(0.0f, 0.0f, 0.0f); + active[0] = 0; + env->agents = agent; + env->active_agent_indices = active; + env->logs = log; + env->rewards = reward; + env->active_agent_count = 1; + env->dt = 0.1f; + env->reward_goal = 2.0f; + env->simulation_mode = SIMULATION_GIGAFLOW; + env->num_target_waypoints = 3; + env->compute_eval_metrics = 0; + agent->reward_coefs[REWARD_COEF_COLLISION] = 3.0f; + agent->reward_coefs[REWARD_COEF_OFFROAD] = 4.0f; + agent->reward_coefs[REWARD_COEF_STOP_LINE] = 5.0f; + agent->metrics_array[LANE_ANGLE_IDX] = 1.0f; +} + +static int test_reward_terminal_components(void) { + Drive env; + Agent agent; + Log log; + int active[1]; + float reward[1] = {0}; + + init_reward_env(&env, &agent, &log, active, reward); + agent.sim_speed = 10.0f; + agent.metrics_array[COLLISION_IDX] = 1.0f; + compute_rewards(&env, 0); + EXPECT_NEAR(env.rewards[0], -4.0f, 1e-5f); + EXPECT_NEAR(log.reward_collision, -4.0f, 1e-5f); + + init_reward_env(&env, &agent, &log, active, reward); + reward[0] = 0.0f; + agent.metrics_array[OFFROAD_IDX] = 1.0f; + compute_rewards(&env, 0); + EXPECT_NEAR(env.rewards[0], -4.0f, 1e-5f); + EXPECT_NEAR(log.reward_offroad, -4.0f, 1e-5f); + + init_reward_env(&env, &agent, &log, active, reward); + reward[0] = 0.0f; + agent.metrics_array[RED_LIGHT_IDX] = 1.0f; + compute_rewards(&env, 0); + EXPECT_NEAR(env.rewards[0], -5.0f, 1e-5f); + EXPECT_NEAR(log.reward_red_light, -5.0f, 1e-5f); + + init_reward_env(&env, &agent, &log, active, reward); + reward[0] = 0.0f; + agent.metrics_array[REACHED_GOAL_IDX] = 1.0f; + compute_rewards(&env, 0); + EXPECT_NEAR(env.rewards[0], 2.0f, 1e-5f); + EXPECT_NEAR(log.reward_goal, 2.0f, 1e-5f); + return 0; +} + +static int test_classic_and_jerk_action_clipping(void) { + srand(19); + Drive env = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 1, 0); + env.action_type = 1; + env.dynamics_model = CLASSIC; + Agent *agent = &env.agents[env.active_agent_indices[0]]; + agent->sim_speed_signed = 0.0f; + ((float (*)[2]) env.actions)[0][0] = 5.0f; + ((float (*)[2]) env.actions)[0][1] = 5.0f; + move_dynamics(&env, 0, env.active_agent_indices[0]); + EXPECT_TRUE(agent->sim_speed_signed <= MAX_SPEED); + EXPECT_TRUE(agent->steering_angle <= STEERING_LIMIT); + free_allocated(&env); + + srand(23); + env = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 1, 0); + env.action_type = 1; + env.dynamics_model = JERK; + agent = &env.agents[env.active_agent_indices[0]]; + agent->reward_coefs[REWARD_COEF_THROTTLE] = 1.0f; + agent->reward_coefs[REWARD_COEF_STEER] = 1.0f; + agent->reward_coefs[REWARD_COEF_ACC] = 1.0f; + ((float (*)[2]) env.actions)[0][0] = -5.0f; + ((float (*)[2]) env.actions)[0][1] = 5.0f; + move_dynamics(&env, 0, env.active_agent_indices[0]); + EXPECT_TRUE(agent->accel_long >= ACCEL_LONG_LIMIT[0]); + EXPECT_TRUE(agent->accel_lat <= ACCEL_LAT_LIMIT[1]); + EXPECT_TRUE(agent->steering_angle <= 0.55f); + free_allocated(&env); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_observation_size_formula); + RUN_TEST(test_observation_zero_fill_and_valid_counts); + RUN_TEST(test_metric_terminal_flags_are_exclusive); + RUN_TEST(test_reward_terminal_components); + RUN_TEST(test_classic_and_jerk_action_clipping); + return test_summary(failures); +} diff --git a/tests/drive/test_error.c b/tests/drive/test_error.c index 285e7d46b6..94938244ca 100644 --- a/tests/drive/test_error.c +++ b/tests/drive/test_error.c @@ -9,6 +9,8 @@ // Test helper to run functions that call exit() in child processes int test_exit_function(void (*test_func)(void)) { + fflush(stdout); + fflush(stderr); pid_t pid = fork(); if (pid == 0) { diff --git a/tests/drive/test_perf.c b/tests/drive/test_perf.c new file mode 100644 index 0000000000..2bf60a4a9f --- /dev/null +++ b/tests/drive/test_perf.c @@ -0,0 +1,44 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +#include + +static double now_seconds(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (double) ts.tv_sec + (double) ts.tv_nsec / 1e9; +} + +static int test_simulator_raw_perf(void) { + const double timeout = 5.0; + const int baseline_sps = 24690; + const float threshold = 0.8f * (float) baseline_sps; + srand(17); + Drive env = drive_test_make_env(drive_carla_map(), SIMULATION_GIGAFLOW, 32, 0); + EXPECT_EQ_INT(env.active_agent_count, 32); + + int tick = 0; + double start = now_seconds(); + while (now_seconds() - start < timeout) { + int *actions = (int *) env.actions; + for (int i = 0; i < env.active_agent_count; i++) { + actions[i] = rand() % (7 * 9); + } + c_step(&env); + tick++; + } + double elapsed = now_seconds() - start; + float sps = (float) env.active_agent_count * (float) tick / (float) elapsed; + printf("Steps per second (SPS): %.1f\n", sps); + printf("Ticks: %d elapsed: %.3f\n", tick, elapsed); + + EXPECT_TRUE(sps >= threshold); + free_allocated(&env); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_simulator_raw_perf); + return test_summary(failures); +} diff --git a/tests/drive/test_render_smoke.c b/tests/drive/test_render_smoke.c new file mode 100644 index 0000000000..f5993087ea --- /dev/null +++ b/tests/drive/test_render_smoke.c @@ -0,0 +1,25 @@ +#include "include/drive_fixture.h" +#include "include/test.h" + +static int test_render_default_frame(void) { + srand(13); + SetTraceLogLevel(LOG_WARNING); + Drive env = drive_test_env_config(drive_carla_map(), SIMULATION_GIGAFLOW, 8, 0); + env.render_mode = RENDER_WINDOW; + allocate(&env); + c_reset(&env); + + c_render(&env, VIEW_MODE_DEFAULT); + EXPECT_TRUE(env.client != NULL); + + close_client(env.client); + env.client = NULL; + free_allocated(&env); + return 0; +} + +int main(void) { + int failures = 0; + RUN_TEST(test_render_default_frame); + return test_summary(failures); +} diff --git a/tests/smoke_tests/data/drive_rollout_golden.json b/tests/smoke_tests/data/drive_rollout_golden.json index fe8840e49b..0e527c72a1 100644 --- a/tests/smoke_tests/data/drive_rollout_golden.json +++ b/tests/smoke_tests/data/drive_rollout_golden.json @@ -1,28 +1,28 @@ { "env": { - "avg_distance_per_infraction": 13.796685082571846, - "avg_speed_per_agent": 1.3587334156036377, - "collision_rate": 0.022321428571428572, - "comfort_violation_count": 0.734669314963477, - "dnf_rate": 0.5558035714285714, - "episode_length": 44.75, - "episode_return": -2.448647149971553, - "lane_center_rate": 0.7272513828107289, + "avg_distance_per_infraction": 13.044897828783308, + "avg_speed_per_agent": 1.3496673362595695, + "collision_rate": 0.026785714285714284, + "comfort_violation_count": 0.7239242025784084, + "dnf_rate": 0.5535714285714286, + "episode_length": 42.75, + "episode_return": -2.356188999755042, + "lane_center_rate": 0.7289507346493858, "n": 16.0, "num_goals_reached": 0.0, "offroad_rate": 0.3705357142857143, - "red_light_violation_rate": 0.05133928571428571, + "red_light_violation_rate": 0.049107142857142856, "reward_components/ade": 0.0, - "reward_components/collision": -0.037454242685011456, - "reward_components/comfort": -1.6450888173920768, + "reward_components/collision": -0.045535716095141, + "reward_components/comfort": -1.5535709985664912, "reward_components/goal": 0.0, - "reward_components/lane_align": -0.13894759278212274, - "reward_components/lane_center": -0.0024253637535106725, + "reward_components/lane_align": -0.13295228672879084, + "reward_components/lane_center": -0.0023546499698048656, "reward_components/offroad": -0.5558035714285714, "reward_components/overspeed": 0.0, - "reward_components/red_light": -0.05133928571428571, - "reward_components/reverse": -0.017492182685860565, - "reward_components/timestep": -9.692520091318459e-05, + "reward_components/red_light": -0.049107142857142856, + "reward_components/reverse": -0.016772317766611065, + "reward_components/timestep": -9.303011704884869e-05, "reward_components/velocity": 0.0, "score": 0.0, "velocity_progress_sum": 0.0 diff --git a/tests/smoke_tests/test_simulator_perf.py b/tests/smoke_tests/test_simulator_perf.py deleted file mode 100644 index 115510a513..0000000000 --- a/tests/smoke_tests/test_simulator_perf.py +++ /dev/null @@ -1,56 +0,0 @@ -import time -import json -import numpy as np -import warnings -from pathlib import Path -from pufferlib.ocean.drive.drive import Drive - -# CONFIGURATION - -REPO_ROOT = Path(__file__).resolve().parents[2] -MAP_DIR = str(REPO_ROOT / "pufferlib" / "resources" / "drive" / "binaries" / "carla") - - -def test_simulator_raw(): - timeout = 5 # seconds (short for CI) - atn_cache = 16 # batched action cache - num_agents = 32 - - # ---- Run simulation ---- - env = Drive(num_agents=num_agents, num_maps=1, scenario_length=91, map_dir=MAP_DIR) - obs, _ = env.reset() - tick = 0 - - actions = np.stack( - [np.random.randint(0, space.n, (atn_cache, num_agents)) for space in env.single_action_space], axis=-1 - ) - - step_times = [] - start_time = time.time() - - while time.time() - start_time < timeout: - atn = actions[tick % atn_cache] - t0 = time.time() - env.step(atn) - step_times.append(time.time() - t0) - tick += 1 - - env.close() - - total_time = time.time() - start_time - sps = num_agents * tick / total_time - print(f"Steps per second (SPS): {sps:.1f}") - print(f"Step time mean: {np.mean(step_times):.6f}s, std: {np.std(step_times):.6f}s") - - mean = 24690 # hardcoded baseline mean SPS - via multiple tests - threshold = 0.8 * mean - if sps < threshold: - raise RuntimeError( - f"\033[91m❌ Performance regression detected: {sps:.1f} < {threshold:.1f}.\033[0m\n" - f"Expected mean: {mean:.1f}, got {sps:.1f}" - ) - return - - -if __name__ == "__main__": - test_simulator_raw() diff --git a/tests/unit_tests/test_drive_map_types.py b/tests/unit_tests/test_drive_map_types.py deleted file mode 100644 index 5e7e6d719f..0000000000 --- a/tests/unit_tests/test_drive_map_types.py +++ /dev/null @@ -1,98 +0,0 @@ -"""Map-source smoke + sanity tests for Drive. - -For each (map_source, simulation_mode) pair we confirm that: - - Construction, reset, and stepping a full episode complete inside a - watchdog budget (no hangs). - - Observations and rewards stay finite throughout. - - The engine emits an episode log by `scenario_length`. - -Carla bins ship only a road graph (no logged trajectories), so they're -exercised in gigaflow mode only. nuPlan and WOMD bins ship logged -trajectories and run in both gigaflow and replay so we cover the -full ingestion path on each format.""" - -import os -import signal -from contextlib import contextmanager - -import numpy as np -import pytest - -from pufferlib.ocean.drive.drive import Drive - -REPO_ROOT = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -BIN_ROOT = os.path.join(REPO_ROOT, "pufferlib", "resources", "drive", "binaries") - -CASES = [ - pytest.param("carla", "gigaflow", id="carla-gigaflow"), - pytest.param("nuplan", "gigaflow", id="nuplan-gigaflow"), - pytest.param("nuplan", "replay", id="nuplan-replay"), - # TODO: Rebuild obstacles bins with v0.3 123Drive - # pytest.param("obstacles", "gigaflow", id="womd-gigaflow"), - # pytest.param("obstacles", "replay", id="womd-replay"), -] - - -@contextmanager -def _watchdog(seconds, what): - # SIGALRM is POSIX-only — fine for the Linux/macOS CI matrix. - def _handler(signum, frame): - raise TimeoutError(f"{what} hung for >{seconds}s") - - prev = signal.signal(signal.SIGALRM, _handler) - signal.alarm(seconds) - try: - yield - finally: - signal.alarm(0) - signal.signal(signal.SIGALRM, prev) - - -@pytest.mark.parametrize("map_subdir, sim_mode", CASES) -def test_load_step_log(map_subdir, sim_mode): - map_dir = os.path.join(BIN_ROOT, map_subdir) - assert os.path.isdir(map_dir), f"Test fixture missing: {map_dir}" - - # control_sdc_only is the natural pairing for replay (one policy-controlled - # agent + log-replayed others). gigaflow tests the random-spawn path. - if sim_mode == "replay": - mode_kwargs = dict( - num_agents=1, - min_agents_per_env=1, - max_agents_per_env=1, - control_mode="control_sdc_only", - ) - else: - mode_kwargs = dict(num_agents=32) - - scenario_length = 91 - tag = f"{map_subdir}/{sim_mode}" - - with _watchdog(30, f"Drive() init [{tag}]"): - env = Drive( - num_maps=1, - scenario_length=scenario_length, - resample_frequency=0, - report_interval=1, - simulation_mode=sim_mode, - map_dir=map_dir, - **mode_kwargs, - ) - - with _watchdog(30, f"env.reset() [{tag}]"): - obs, _ = env.reset(seed=0) - assert np.isfinite(obs).all(), f"Non-finite obs after reset [{tag}]" - - logs = [] - with _watchdog(60, f"step loop [{tag}]"): - for _ in range(scenario_length + 5): - actions = np.zeros_like(env.actions) - obs, reward, _, _, info = env.step(actions) - assert np.isfinite(obs).all(), f"Non-finite obs during step [{tag}]" - assert np.isfinite(reward).all(), f"Non-finite reward [{tag}]" - if info: - logs.extend(info) - - assert logs, f"No episode log emitted within {scenario_length + 5} steps [{tag}]" - - env.close() diff --git a/tests/unit_tests/test_map_cache.py b/tests/unit_tests/test_map_cache.py deleted file mode 100644 index 1b3c15790e..0000000000 --- a/tests/unit_tests/test_map_cache.py +++ /dev/null @@ -1,186 +0,0 @@ -"""Tests for the per-process map cache (use_map_cache env knob). - -Covers: cache-on vs cache-off observation parity, refcount discipline across -close orderings, slot-reuse keeping cache size bounded, and per-entry owner_pid -correctness in a forked child. -""" - -import os - -import numpy as np -import pytest - -from pufferlib.ocean.drive import binding as drive_binding -from pufferlib.ocean.drive.drive import Drive - -REPO_ROOT = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -CARLA_MAP_DIR = os.path.join(REPO_ROOT, "pufferlib", "resources", "drive", "binaries", "carla") - - -def _make_drive(use_map_cache: int, num_agents: int = 32, num_maps: int = 1, **kwargs): - """Construct a Drive env on the bundled carla map dir. - - num_agents defaults to 32 to satisfy the default min_agents_per_env=32; the - cache lifecycle is the same regardless of agent count. - """ - return Drive( - num_agents=num_agents, - num_maps=num_maps, - use_map_cache=use_map_cache, - map_dir=CARLA_MAP_DIR, - scenario_length=64, - resample_frequency=0, - report_interval=1, - **kwargs, - ) - - -def _step_and_record(env, action_stream): - """Step `env` through `action_stream` and return (obs, rewards, terminals, truncations) per step.""" - obs_log, rew_log, term_log, trunc_log = [], [], [], [] - for action in action_stream: - np.copyto(env.actions, action) - o, r, term, trunc, _ = env.step(env.actions) - obs_log.append(o.copy()) - rew_log.append(r.copy()) - term_log.append(term.copy()) - trunc_log.append(trunc.copy()) - return obs_log, rew_log, term_log, trunc_log - - -def test_obs_reward_done_parity_cache_on_vs_off(): - """Cache-on must produce byte-identical obs/rewards/terminals/truncations to - cache-off given the same map and seed. Any silent geometry mismatch in the - hit path (wrong vision_range, swapped grid_map pointers, stale neighbor - offsets) surfaces here on the first step. Zero actions are sufficient — the - cache affects only which static geometry is read, so every trajectory - exercises it. - """ - seed = 12345 - num_steps = 50 - - env_off = _make_drive(use_map_cache=0) - env_off.reset(seed=seed) - actions = [np.zeros_like(env_off.actions) for _ in range(num_steps)] - off_obs, off_rew, off_term, off_trunc = _step_and_record(env_off, actions) - env_off.close() - - env_on = _make_drive(use_map_cache=1) - env_on.reset(seed=seed) - on_obs, on_rew, on_term, on_trunc = _step_and_record(env_on, actions) - env_on.close() - - for t in range(num_steps): - np.testing.assert_array_equal(off_obs[t], on_obs[t], err_msg=f"observations diverged at step {t}") - np.testing.assert_array_equal(off_rew[t], on_rew[t], err_msg=f"rewards diverged at step {t}") - np.testing.assert_array_equal(off_term[t], on_term[t], err_msg=f"terminals diverged at step {t}") - np.testing.assert_array_equal(off_trunc[t], on_trunc[t], err_msg=f"truncations diverged at step {t}") - - -@pytest.mark.parametrize( - "close_order", - [ - (0, 1, 2), # forward - (2, 1, 0), # reverse - (1, 2, 0), # rotation - (1, 0, 2), # adjacent swap - ], -) -def test_multi_env_close_orderings_do_not_crash(close_order): - """Three Drive instances on the same map share one cached SharedMapData - entry (refcount=3 once all are alive). Closing them in arbitrary order - must succeed: every close decrements the refcount, only the last one frees - the entry. A double-free or use-after-free surfaces as SIGSEGV / abort - on the final close in that ordering.""" - envs = [_make_drive(use_map_cache=1) for _ in range(3)] - for e in envs: - e.reset(seed=0) - # One step exercises the shared grid_map / neighbor_offsets reads. - for e in envs: - e.step(np.zeros_like(e.actions)) - for idx in close_order: - envs[idx].close() - - -def test_forked_child_can_build_and_free_its_own_entry(): - """In a forked child with the parent's cache pre-populated: building an env - with use_map_cache=1 raises the child's live count by 1, and closing it drops - the live count back to 0 (i.e., the child frees its own entry). The parent's - cache size is unchanged by the child.""" - if os.name != "posix": - pytest.skip("fork start method is POSIX-only") - import multiprocessing as mp - - # Pre-populate the parent's cache so the child inherits a non-empty cache. - warm = _make_drive(use_map_cache=1) - warm.reset(seed=0) - warm.close() - parent_size_before_fork = drive_binding.map_cache_size() - - ctx = mp.get_context("fork") - q = ctx.Queue() - - def child_target(out_q): - try: - env = _make_drive(use_map_cache=1) - env.reset(seed=0) - env.step(np.zeros_like(env.actions)) - live_after_build = drive_binding.map_cache_live_count() - env.close() - live_after_close = drive_binding.map_cache_live_count() - out_q.put(("ok", live_after_build, live_after_close)) - except BaseException as e: - out_q.put(("err", repr(e), None)) - - p = ctx.Process(target=child_target, args=(q,)) - p.start() - p.join(timeout=60) - assert p.exitcode == 0, f"Child process exited with {p.exitcode}" - payload = q.get(timeout=5) - assert payload[0] == "ok", f"Child raised: {payload[1]}" - _, live_after_build, live_after_close = payload - assert live_after_build == 1, f"Expected 1 live cache entry after build in child; got {live_after_build}." - assert live_after_close == 0, f"Expected 0 live cache entries after close in child; got {live_after_close}." - - assert drive_binding.map_cache_size() == parent_size_before_fork, ( - f"Parent's cache size changed across fork ({parent_size_before_fork} -> {drive_binding.map_cache_size()})." - ) - - -def test_cache_size_bounded_by_unique_maps(): - """map_cache_insert reuses NULL slots freed by free_shared_map_data, so the - slot count stays bounded across alloc/free/alloc cycles. Without slot reuse, - every realloc appends past the NULL holes and the count drifts upward. - - Doesn't assert exact slot count (other tests in the same process may have - populated the cache already); asserts the invariant that the count never - grows under repeated cycles on the same set of maps. - """ - - # Allocate envs, then drop and re-allocate twice. Each cycle exercises a - # free followed by an insert. The slot count after each cycle must equal - # the count after the first cycle. - def alloc_three_then_close(): - envs = [_make_drive(use_map_cache=1) for _ in range(3)] - for e in envs: - e.reset(seed=0) - for e in envs: - e.close() - - alloc_three_then_close() - size_after_cycle_1 = drive_binding.map_cache_size() - - alloc_three_then_close() - size_after_cycle_2 = drive_binding.map_cache_size() - - alloc_three_then_close() - size_after_cycle_3 = drive_binding.map_cache_size() - - assert size_after_cycle_2 == size_after_cycle_1, ( - f"Cache grew on the second alloc cycle ({size_after_cycle_1} -> {size_after_cycle_2}); " - f"map_cache_insert is appending past NULL holes instead of reusing them." - ) - assert size_after_cycle_3 == size_after_cycle_1, ( - f"Cache grew on the third alloc cycle ({size_after_cycle_1} -> {size_after_cycle_3}); " - f"slot-reuse invariant should keep the count flat across repeated cycles." - )