//
|
// Created by YY on 2020/4/3.
|
//
|
|
#include "area_exam.h"
|
#include "../test_common/car_sensor.h"
|
#include "../driver_test.h"
|
#include "../jni_log.h"
|
#include "park_bottom.h"
|
#include "stop_and_start.h"
|
#include "park_edge.h"
|
#include "driving_curve.h"
|
#include "turn_a90.h"
|
#include "../utils/xconvert.h"
|
#include "../common/apptimer.h"
|
|
#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
|
|
static int CurrExamStatus = EXAM_AREA_NONE; // 1 测试完成 0 测试中 -1 测试错误退出
|
static int CurrExamMapIndex = -1;
|
static int CurrEnterMapIndex = -1;
|
|
static void DetectEnterOrExitMap(const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
|
static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
|
static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList);
|
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
|
static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime);
|
|
void TerminateAreaExam(void)
|
{
|
CurrExamMapIndex = -1;
|
}
|
|
void InitAreaExam(void)
|
{
|
CurrExamMapIndex = -1;
|
}
|
|
void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
|
{
|
DetectEnterOrExitMap(car, CarModelList, AreaMapList);
|
|
ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
|
}
|
|
void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set)
|
{
|
double ld = 100, rd = 100;
|
|
array.clear();
|
|
for (auto line = line_set.begin(); line != line_set.end(); line++) {
|
double ld_t = 100, rd_t = 100;
|
double d;
|
if (VerticalPointOnLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line)) {
|
d = DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line);
|
if (d < ld_t) {
|
ld_t = d;
|
}
|
}
|
if (VerticalPointOnLine(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line)) {
|
d = DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line);
|
if (d < ld_t) {
|
ld_t = d;
|
}
|
}
|
|
if (VerticalPointOnLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line)) {
|
d = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line);
|
if (d < rd_t) {
|
rd_t = d;
|
}
|
}
|
if (VerticalPointOnLine(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line)) {
|
d = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line);
|
if (d < rd_t) {
|
rd_t = d;
|
}
|
}
|
|
if (isEqual2(ld_t, rd_t)) {
|
if (!isEqual(ld_t, 100)) {
|
if (rd_t < rd)
|
rd = rd_t;
|
if (ld_t < ld)
|
ld = ld_t;
|
}
|
} else if (ld_t > rd_t) {
|
if (rd_t < rd)
|
rd = rd_t;
|
} else {
|
if (ld_t < ld)
|
ld = ld_t;
|
}
|
}
|
|
array.push_back(ld);
|
array.push_back(rd);
|
}
|
|
static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
|
{
|
if (CurrExamMapIndex < 0) {
|
if (CurrEnterMapIndex < 0) {
|
CurrEnterMapIndex = EnterMap(car, CarModelList, mapList);
|
if (CurrEnterMapIndex >= 0) {
|
DEBUG("进入某个子项目 idx = %d", CurrEnterMapIndex);
|
CurrExamMapIndex = CurrEnterMapIndex;
|
CurrExamStatus = EXAM_AREA_START;
|
}
|
} else {
|
if (ExitMap(car, CurrEnterMapIndex, mapList)) {
|
CurrEnterMapIndex = -1;
|
}
|
}
|
}
|
}
|
|
static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime)
|
{
|
if (index >= 0) {
|
if (CurrExamStatus == EXAM_AREA_START) {
|
DEBUG("CurrExamMapIndex %d mtype %d", AreaMapList[index].id, AreaMapList[index].type);
|
|
switch (AreaMapList[index].type) {
|
case MAP_TYPE_PARK_BUTTOM:
|
DEBUG("进入倒车入库场地 %d", AreaMapList[index].id);
|
|
StartParkBottom(AreaMapList[index].id, move, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_STOP_START:
|
DEBUG("进入上坡起步场地 %d", AreaMapList[index].id);
|
|
StartSAS(AreaMapList[index].id, move, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_PART_EDGE:
|
DEBUG("进入侧方位停车场地 %d", AreaMapList[index].id);
|
|
StartParkEdge(AreaMapList[index].id, move, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_CURVE:
|
DEBUG("进入曲线行驶场地 %d", AreaMapList[index].id);
|
|
StartDrivingCurve(AreaMapList[index].id, move, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_TURN_90:
|
DEBUG("进入直角转弯场地 %d", AreaMapList[index].id);
|
|
StartTurnA90(AreaMapList[index].id, move, azimuth, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
default:break;
|
}
|
} else if (CurrExamStatus == EXAM_AREA_RUN) {
|
int testing = 0;
|
switch (AreaMapList[index].type) {
|
case MAP_TYPE_PARK_BUTTOM:
|
testing = TestParkBottom(&AreaMapList[index].map,
|
car, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_STOP_START:
|
testing = TestSAS(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_PART_EDGE:
|
testing = TestParkEdge(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_CURVE:
|
testing = TestDrivingCurve(&AreaMapList[index].map, &AreaMapList[index].map2, car, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_TURN_90:
|
testing = TestTurnA90(&AreaMapList[index].map, car, NULL, azimuth, speed, move, rtkTime);
|
break;
|
default:
|
break;
|
}
|
|
if (testing > 0) {
|
CurrExamStatus = EXAM_AREA_RUN;
|
} else {
|
CurrExamStatus = EXAM_AREA_END;
|
}
|
}
|
if (CurrExamStatus != EXAM_AREA_RUN) {
|
// 某项结束
|
CurrExamStatus = EXAM_AREA_NONE;
|
CurrExamMapIndex = -1;
|
}
|
}
|
}
|
|
static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
|
{
|
for (int i = 0; i < mapList.size() && car != NULL; ++i) {
|
// 车前轮或后轮轨迹越过触发线
|
if (mapList[i].type == MAP_TYPE_STOP_START) {
|
// 构造虚拟的左上角点
|
double x9, y9, xo, yo;
|
|
xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
|
yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
|
|
x9 = 2*xo - mapList[i].map.point[8].X;
|
y9 = 2*yo - mapList[i].map.point[8].Y;
|
|
Line triggerLine;
|
|
triggerLine.X1 = mapList[i].map.point[0].X;
|
triggerLine.Y1 = mapList[i].map.point[0].Y;
|
triggerLine.X2 = x9;
|
triggerLine.Y2 = y9;
|
|
if (CrashTriggerLine(triggerLine, car, CarModelList))
|
return i;
|
}
|
if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
|
// 车头顶点在场地内
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
|
Line enterLine1, enterLine2;
|
|
MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
|
MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
|
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
|
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
|
return i;
|
}
|
}
|
if (mapList[i].type == MAP_TYPE_PART_EDGE) {
|
// 车头顶点在场地内
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
|
Line enterLine;
|
|
MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
|
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
|
return i;
|
}
|
}
|
if (mapList[i].type == MAP_TYPE_TURN_90) {
|
// 车前轮或后轮轨迹越过触发线
|
Line triggerLine;
|
|
MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
|
|
if (CrashTriggerLine(triggerLine, car, CarModelList))
|
return i;
|
}
|
if (mapList[i].type == MAP_TYPE_CURVE) {
|
Line triggerLine;
|
|
MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
|
if (CrashTriggerLine(triggerLine, car, CarModelList))
|
return i;
|
}
|
}
|
return -1;
|
}
|
|
static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList)
|
{
|
bool ret = false;
|
if (index < 0 || index >= mapList.size()) return true;
|
|
if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
|
mapList[index].type == MAP_TYPE_PART_EDGE ||
|
mapList[index].type == MAP_TYPE_TURN_90) {
|
// 全车都需不在地图中
|
Polygon carBody;
|
|
carBody.num = car->bodyNum;
|
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
|
for (int i = 0; i < carBody.num; ++i) {
|
carBody.point[i] = car->carXY[car->body[i]];
|
}
|
|
if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
}
|
if (mapList[index].type == MAP_TYPE_STOP_START) {
|
// 构造虚拟的左上角点
|
double x9, y9, xo, yo;
|
|
bool enter = false;
|
|
xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
|
yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;
|
|
x9 = 2*xo - mapList[index].map.point[8].X;
|
y9 = 2*yo - mapList[index].map.point[8].Y;
|
|
Polygon map;
|
|
map.num = 4;
|
map.point = (PointF *) malloc(map.num * sizeof(PointF));
|
|
map.point[0] = mapList[index].map.point[0];
|
map.point[1] = mapList[index].map.point[8];
|
map.point[2] = mapList[index].map.point[7];
|
map.point[3].X = x9;
|
map.point[3].Y = y9;
|
|
// 全车都需不在地图中
|
Polygon carBody;
|
|
carBody.num = car->bodyNum;
|
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
|
for (int i = 0; i < carBody.num; ++i) {
|
carBody.point[i] = car->carXY[car->body[i]];
|
}
|
|
if (IntersectionOf(&carBody, &map) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
free(map.point);
|
}
|
if (mapList[index].type == MAP_TYPE_CURVE) {
|
ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
|
}
|
|
return ret;
|
}
|
|
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
bool trigger = false;
|
|
if (CarModelList.size() < 5)
|
return trigger;
|
|
Polygon trace, trace2;
|
int pn = 0;
|
|
trace2.num = trace.num = 5;
|
trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
|
trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
|
|
list<car_model *>::iterator iter = CarModelList.begin();
|
|
car_model *c1 = *iter;
|
|
trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
|
trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
|
|
++iter;
|
|
while (iter != CarModelList.end() && pn < trace.num) {
|
car_model *c2 = *iter;
|
|
uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
|
|
if (tdiff >= D_SEC(1)) {
|
trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
|
trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
|
c1 = c2;
|
}
|
++iter;
|
}
|
|
PointF p1, p2;
|
|
p1.X = triggerLine.X1;
|
p1.Y = triggerLine.Y1;
|
p2.X = triggerLine.X2;
|
p2.Y = triggerLine.Y2;
|
|
int pp = 0;
|
for (int p = 1; p < pn; ++p) {
|
Line trace_line, trace2_line;
|
|
MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
|
MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
|
|
if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
|
IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
|
DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
|
// 碰到触发线
|
DEBUG("碰撞触发线 引发地图");
|
trigger = true;
|
goto SEARCH_TRIGGER_LINE_END;
|
}
|
}
|
|
SEARCH_TRIGGER_LINE_END:
|
free(trace.point);
|
|
return trigger;
|
}
|
