//
|
// Created by YY on 2019/10/23.
|
//
|
|
#include "park_edge.h"
|
#include "../Geometry.h"
|
#include "../driver_test.h"
|
#include "../common/apptimer.h"
|
#include "../native-lib.h"
|
|
#include <vector>
|
#include <cstdlib>
|
|
using namespace std;
|
|
enum {
|
ARRIVED_START,
|
PARK_CAR,
|
START_CAR
|
};
|
|
const int PARK_TIMEOUT = 90;
|
const uint32_t STOP_CAR_TIME = D_SEC(2);
|
|
static int prev_run_status;
|
static int runStatusBeforeStop;
|
static uint32_t stopTimepoint = 0;
|
static bool parkTimeout;
|
static bool occurCrashRedLine1, occurCrashRedLine2;
|
static bool PETesting = false;
|
static int currTarget;
|
static bool carStopEvent; // 中途停车标记
|
static bool carParkSuccess; // 是否停在库位
|
static int leaveParkCnt; // 车辆离开库位刚发生的时，检查是否开启转向灯
|
|
static void ParkEdgeTimeout(union sigval sig);
|
static bool CrashRedLine1(const Polygon *map, const car_model_cache_t *car);
|
static bool CrashRedLine2(const Polygon *map, const car_model_cache_t *car);
|
static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
|
static bool ExitParkArea(const Polygon *map, const car_model_cache_t *car);
|
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
|
|
bool EnterParkEdgeArea(const Polygon *car, const Polygon *tire, const Polygon *map)
|
{
|
if (IntersectionOf(tire->point[0], map) == GM_Containment &&
|
IntersectionOf(tire->point[1], map) == GM_Containment &&
|
IntersectionOf(tire->point[4], map) == GM_Containment &&
|
IntersectionOf(tire->point[5], map) == GM_Containment) {
|
return true;
|
}
|
return false;
|
}
|
|
void StartParkEdge(void)
|
{
|
prev_run_status = 0;
|
parkTimeout = false;
|
occurCrashRedLine1 = occurCrashRedLine2 = false; // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
|
PETesting = true;
|
currTarget = ARRIVED_START;
|
}
|
|
void StopParkEdge(void)
|
{
|
AppTimer_delete(ParkEdgeTimeout);
|
PETesting = false;
|
}
|
|
int TestParkEdge(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
|
{
|
int status = 0;
|
|
if (!PETesting)
|
return 0;
|
|
if (currTarget >= PARK_CAR) {
|
if (CrashRedLine1(map, car)) {
|
if (!occurCrashRedLine1) {
|
occurCrashRedLine1 = true;
|
// 车轮压边线，每次扣10分
|
err.push_back(23);
|
}
|
} else {
|
occurCrashRedLine1 = false;
|
}
|
|
if (CrashRedLine2(map, car)) {
|
if (!occurCrashRedLine2) {
|
occurCrashRedLine2 = true;
|
// 车身压库位线，每次扣10分
|
err.push_back(24);
|
}
|
} else {
|
occurCrashRedLine2 = false;
|
}
|
|
if (parkTimeout) {
|
// 超时90秒，不合格
|
err.push_back(22);
|
status = -1;
|
}
|
|
if (prev_run_status != run_status) {
|
if (run_status == 0) {
|
// 车停了
|
runStatusBeforeStop = prev_run_status;
|
stopTimepoint = AppTimer_GetTickCount();
|
} else {
|
// 车动了，且和停车前的运行状态一致
|
if (runStatusBeforeStop == run_status && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
|
// 中途停车，扣5分
|
err.push_back(26);
|
}
|
}
|
}
|
}
|
|
if (currTarget == ARRIVED_START) {
|
if (ExitTestArea(map, car)) {
|
// 直接驶离测试区域
|
status = -2;
|
} else
|
if (run_status < 0) {
|
AppTimer_add(ParkEdgeTimeout, D_SEC(PARK_TIMEOUT));
|
currTarget = PARK_CAR;
|
TextOsd(0, "开始倒库");
|
}
|
} else if (currTarget == PARK_CAR) {
|
if (run_status < 0) {
|
carStopEvent = false;
|
} else if (run_status == 0) {
|
// 立即检查是否停车到位，也许是中途停车，先不管，待发生前进事件后，再断定是否停车到位
|
if (!carStopEvent) {
|
carStopEvent = true;
|
carParkSuccess = EnterParking(map, car);
|
}
|
} else {
|
if (carStopEvent) {
|
if (!carParkSuccess) {
|
// 不合格：车身出线
|
err.push_back(21);
|
status = -1;
|
}
|
}
|
carStopEvent = false;
|
leaveParkCnt = 0;
|
currTarget = START_CAR;
|
TextOsd(0, "开始出库");
|
}
|
} else if (currTarget == START_CAR) {
|
if (run_status > 0) {
|
leaveParkCnt++;
|
|
if (leaveParkCnt == 1) {
|
// 未开启转向灯，扣10分
|
err.push_back(25);
|
}
|
|
if (ExitParkArea(map, car)) {
|
// 项目完成
|
status = 1;
|
TextOsd(0, "项目完成");
|
}
|
}
|
}
|
|
if (status != 0) {
|
StopParkEdge();
|
}
|
|
prev_run_status = run_status;
|
return status;
|
}
|
|
static void ParkEdgeTimeout(union sigval sig) {
|
AppTimer_delete(ParkEdgeTimeout);
|
parkTimeout = true;
|
}
|
|
// 车轮是否压道路边线
|
static bool CrashRedLine1(const Polygon *map, const car_model_cache_t *car)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{0, 7}, {1, 2}, {5, 6}};
|
|
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 bool CrashRedLine2(const Polygon *map, const car_model_cache_t *car)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{2, 3}, {3, 4}, {4, 5}};
|
|
Polygon car_body;
|
|
car_body.num = car->desc->body_num;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->points[car->desc->body[i]];
|
}
|
|
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, &car_body) != GM_None) {
|
ret = true;
|
break;
|
}
|
}
|
|
free(car_body.point);
|
return ret;
|
}
|
|
static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
|
bool succ = false;
|
|
Polygon parking;
|
Polygon car_body;
|
|
car_body.num = car->desc->body_num;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->points[car->desc->body[i]];
|
}
|
|
MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
|
|
if (IntersectionOf(&car_body, &parking) == GM_Containment) {
|
succ = true;
|
}
|
|
CleanPolygon(&parking);
|
free(car_body.point);
|
|
return succ;
|
}
|
|
// 整个车辆都要驶过前库位线
|
static bool ExitParkArea(const Polygon *map, const car_model_cache_t *car)
|
{
|
for (int i = 0; i < car->point_num; ++i) {
|
if (IntersectionOfLine(map->point[4], map->point[5], car->points[i]) != -1)
|
return false;
|
}
|
return true;
|
}
|
|
// 整个车辆都要驶过最前端控制线
|
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[6], map->point[7], car->points[i]) != -1)
|
return false;
|
}
|
return true;
|
}
|
