//
|
// Created by YY on 2019/10/31.
|
//
|
|
#include <cstdlib>
|
#include <vector>
|
#include <cmath>
|
|
#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<int>&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;
|
}
|
