// // Created by YY on 2019/10/31. // #include #include #include #include "stop_and_start.h" #include "../driver_test.h" #include "../jni_log.h" #include "../common/apptimer.h" using namespace std; enum { STOP_CAR, START_CAR }; const double STOP_DISTANCE_THRESHOLD_RED = 0.5; const double EDGE_DISTANCE_THRESHOLD_RED = 0.5; const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; const int CAR_START_TIMEOUT = 30; const double EPSILON = 1e-3; static bool SASTesting = false; static double slideDistance; static bool startCarTimeout; static int currTarget; static PointF stopPoint; static int startCarConfirm; // 起步时,持续前进一小段才算 static void StartCarTimeout(union sigval sig); static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car); static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car); static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car); static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car); void StartSAS(void) { SASTesting = true; slideDistance = 0.0; startCarTimeout = false; currTarget = STOP_CAR; } void StopSAS(void) { SASTesting = false; AppTimer_delete(StartCarTimeout); } int TestSAS(vector&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status) { int status = 0; if (!SASTesting) return -2; if (currTarget >= STOP_CAR) { if (CrashRedLine(map, car)) { // 车轮压线 err.push_back(13); status = -1; } } if (currTarget == STOP_CAR) { if (run_status == 0) { double dis1 = DistanceOfHead2Stopline(map, car); double dis2 = DistanceOfTire2Edge(map, car); if (dis1 > STOP_DISTANCE_THRESHOLD_RED) { // 距离停止线前后超出50厘米 err.push_back(12); status = -1; } else if (fabs(dis1) > EPSILON) { // 前保险没有位于停止带内,但没有超出50厘米 err.push_back(17); } if (dis2 > EDGE_DISTANCE_THRESHOLD_RED) { // 距离边线超出50厘米 err.push_back(14); status = -1; } else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) { // 距离边线超出30厘米 err.push_back(18); } // 检查是否拉住手刹 AppTimer_delete(StartCarTimeout); AppTimer_add(StartCarTimeout, D_SEC(CAR_START_TIMEOUT)); slideDistance = 0.0; stopPoint = car->points[0]; startCarConfirm = 0; currTarget = START_CAR; } else if (run_status > 0) { if (ExitTestArea(map, car)) { // 车辆直接驶离测试区,直接淘汰 err.push_back(12); status = -1; } } } else if (currTarget == START_CAR) { if (startCarTimeout) { startCarTimeout = false; //起步时间超过30秒 err.push_back(15); status = -1; } if (run_status > 0) { startCarConfirm++; if (startCarConfirm == 2) { AppTimer_delete(StartCarTimeout); // 起步完成 } if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) { // 后滑超过10厘米,但没超过30厘米 err.push_back(20); } } else if (run_status < 0) { // 后滑了 slideDistance = DistanceOf(stopPoint, car->points[0]); if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED) { // 后滑超过30厘米 err.push_back(16); status = -1; } } if (ExitTestArea(map, car)) { // 测试完成了 status = 1; } } if (status != 0) { StopSAS(); } return status; } static void StartCarTimeout(union sigval sig) { AppTimer_delete(StartCarTimeout); startCarTimeout = true; } // 车轮是否压边线 static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car) { bool ret = false; Line red_line; const int red_lines[][2] = {{0, 8}}; Line frontAxle, rearAxle; MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]); MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]); for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]); if (IntersectionOf(red_line, frontAxle) == GM_Intersection || IntersectionOf(red_line, rearAxle) == GM_Intersection) { ret = true; break; } } return ret; } static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car) { double dis = 0.0; int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->points[0]); int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->points[0]); if (rel1 == 1) { Line line1; MakeLine(&line1, &map->point[4], &map->point[3]); dis = DistanceOf(car->points[0], line1); } else if (rel2 == -1) { Line line2; MakeLine(&line2, &map->point[5], &map->point[6]); dis = DistanceOf(car->points[0], line2); } return dis; } static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car) { Line edge; MakeLine(&edge, &map->point[0], &map->point[8]); double l1 = DistanceOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], edge); double l2 = DistanceOf(car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], edge); return (l1+l2)/2.0; } // 整个车辆都要驶离该测试区域 static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car) { for (int i = 0; i < car->point_num; ++i) { if (IntersectionOfLine(map->point[8], map->point[7], car->points[i]) != -1) return false; } return true; }