//
|
// Created by YY on 2019/10/23.
|
//
|
//
|
// 4 ___________________________ 3
|
// | |
|
// | |
|
// | |
|
// ________________| |___________1
|
// 6 5 2
|
//
|
//
|
// 7______________________________________________________0
|
//
|
|
#include "park_edge.h"
|
#include "../jni_log.h"
|
#include "../test_common/Geometry.h"
|
#include "../driver_test.h"
|
#include "../common/apptimer.h"
|
#include "../native-lib.h"
|
#include "../utils/xconvert.h"
|
#include "../test_common/car_sensor.h"
|
#include "../master/comm_if.h"
|
#include "area_exam.h"
|
#include "../test_common/odo_graph.h"
|
|
#include <vector>
|
#include <cstdlib>
|
|
#define DEBUG(fmt, args...) LOGD("<park_edge> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
enum {
|
TESTING,
|
TEST_FAIL, // 因触发某些规则，在车身未完全立场情况下，提前终止部分测试
|
TEST_FINISH
|
};
|
|
const uint32_t CHECK_PARK_DELAY = 400;
|
|
static bool reportExamTimeout;
|
static bool reportParkFail;
|
|
static uint32_t stopTimepoint = 0;
|
static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3;
|
static int prevMoveStatus, storeMoveStatusBeforeStop;
|
static int parkStatus;
|
static int gearAtStop;
|
static bool occurMoveBack, checkPark, parkSuccess, checkLight;
|
static uint32_t moveBackTimePoint;
|
static int testStatus;
|
static int exitAreaCfm;
|
static int currGear;
|
static double odo;
|
|
static bool CrashRedLine1(const Polygon *map, const car_model_t *car);
|
static bool CrashRedLine2(const Polygon *map, const car_model_t *car);
|
static bool CrashRedLine3(const Polygon *map, const car_model_t *car);
|
static bool EnterParking(const Polygon *map, const car_model_t *car);
|
static bool ExitParkArea(const Polygon *map, const car_model_t *car);
|
static bool ExitParkArea2(const Polygon *map, const car_model_t *car);
|
|
void StartParkEdge(prime_t &prime)
|
{
|
DEBUG("进入侧方停车场地");
|
|
testStatus = TESTING;
|
|
occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
|
reportExamTimeout = false;
|
reportParkFail = false;
|
prevMoveStatus = prime.pMotion->move;
|
parkSuccess = false;
|
parkStatus = 0;
|
occurMoveBack = false;
|
checkPark = false;
|
checkLight = false;
|
gearAtStop = -1;
|
stopTimepoint = 0;
|
odo = ReadOdo();
|
currGear = ReadCarStatus(GEAR);
|
exitAreaCfm = 0;
|
|
PlayTTS("您已进入侧方停车区域", NULL);
|
// 仅当发生倒车，才意味着项目开始
|
/*if (moveStatus == -1) {
|
occurMoveBack = true;
|
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
}*/
|
}
|
|
int TestParkEdge(prime_t &prime)
|
{
|
vector<double> dtox;
|
vector<Line> line_set;
|
Line distance_line;
|
|
bool gear_change = false;
|
int gear = ReadCarStatus(GEAR);
|
|
if (gear == GEAR_R) {
|
if (currGear != GEAR_R) {
|
gear_change = true;
|
currGear = GEAR_R;
|
}
|
} else if (currGear == GEAR_R) {
|
gear_change = true;
|
currGear = gear;
|
}
|
|
if (gear_change) {
|
// 检查上一次挡位的行驶距离，过小就放弃，避开学员原地挂挡重试
|
double run_distance = ReadOdo() - odo;
|
|
DEBUG("2次挡位运行距离 %f", run_distance);
|
|
if (run_distance < 1) {
|
gear_change = false;
|
DEBUG("2次挡位运行距离过小，忽略");
|
}
|
odo = ReadOdo();
|
}
|
|
// 首次挂倒挡， 才意味着项目开始
|
if (testStatus == TESTING && gear_change) {
|
if (currGear == GEAR_R) {
|
if (!occurMoveBack) {
|
DEBUG("首次侧方停车");
|
} else {
|
DEBUG("再次侧方停车");
|
}
|
|
{
|
checkPark = false;
|
occurMoveBack = true;
|
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
|
rtkTime->mss * 10); // 开始计时
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
|
}
|
} else {
|
if (occurMoveBack && !checkPark) {
|
// 检查车身入库情况
|
DEBUG("检查车身入库情况");
|
checkPark = true;
|
checkLight = false;
|
if (EnterParking(map, car)) {
|
parkStatus = 1;
|
parkSuccess = true;
|
} else {
|
// 停止后，车身出线，不合格
|
AddExamFault(20401, rtkTime);
|
parkSuccess = false;
|
DEBUG("移库不入");
|
}
|
}
|
}
|
}
|
|
if (CrashRedLine1(map, car)) {
|
if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
|
// 车轮压边线，每次扣10分
|
AddExamFault(20403, rtkTime);
|
DEBUG("车轮压边线");
|
occurCrashRedLine1 = true;
|
}
|
} else {
|
occurCrashRedLine1 = false;
|
}
|
|
if (CrashRedLine2(map, car)) {
|
if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
|
// 车身压库位线，每次扣10分
|
AddExamFault(20404, rtkTime);
|
DEBUG("车身压库位线");
|
occurCrashRedLine2 = true;
|
}
|
} else {
|
occurCrashRedLine2 = false;
|
}
|
|
if (CrashRedLine3(map, car)) {
|
if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) {
|
// 车身压库位线，每次扣10分
|
AddExamFault(20407, rtkTime);
|
DEBUG("车身压库位线");
|
occurCrashRedLine3 = true;
|
}
|
} else {
|
occurCrashRedLine3 = false;
|
}
|
|
if (ExitParkArea2(map, car)) {
|
/*if (!parkSuccess && occurMoveBack && !reportParkFail) {
|
// 直接驶离测试区，认为移库不入
|
AddExamFault(10103, rtkTime);
|
reportParkFail = true;
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
}*/
|
if (occurMoveBack && !checkPark) {
|
// 倒车直接驶离测试区
|
AddExamFault(10103, rtkTime);
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
}
|
|
testStatus = TEST_FINISH;
|
goto TEST_END;
|
}
|
|
// 距离检测
|
MakeLine(&distance_line, &map->point[0], &map->point[7]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[1], &map->point[2]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[2], &map->point[3]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[3], &map->point[4]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[4], &map->point[5]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[5], &map->point[6]);
|
line_set.push_back(distance_line);
|
DistanceOfTire2X(dtox, car, line_set);
|
MA_SendDistance(dtox[0], dtox[1]);
|
|
if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
|
// 入库后一直前进，车头移出驶离线
|
if (++exitAreaCfm >= 4) { // 避免信号漂移造成的误判
|
AddExamFault(10103, rtkTime);
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
testStatus = TEST_FAIL;
|
}
|
} else {
|
exitAreaCfm = 0;
|
}
|
|
if (occurMoveBack) {
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
if (!reportExamTimeout && tp - moveBackTimePoint >= examParam.park_edge_limit_time) {
|
// 超时90秒，不合格
|
AddExamFault(20402, rtkTime);
|
reportExamTimeout = true;
|
DEBUG("移库90秒超时");
|
}
|
}
|
|
if (moveStatus != prevMoveStatus) {
|
if (moveStatus == 0) {
|
parkStatus = 0;
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
storeMoveStatusBeforeStop = prevMoveStatus;
|
gearAtStop = (currGear == GEAR_R ? 1 : 0);
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
DEBUG("停车时挡位 = %d", gearAtStop);
|
} else if (prevMoveStatus == 0 && stopTimepoint > 0) {
|
DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
DEBUG("停车时间 %ld", tp - stopTimepoint);
|
DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);
|
|
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
|
&& occurMoveBack
|
&& gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
|
// 停车超2秒，每次扣5分
|
AddExamFault(20406, rtkTime);
|
DEBUG("停车超时");
|
}
|
|
if (moveStatus == 1 && checkPark && !checkLight) {
|
// 在这里检查转向灯状态
|
checkLight = true;
|
if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
|
// 不开转向灯，扣10分
|
AddExamFault(20405, rtkTime);
|
DEBUG("未开启转向灯");
|
}
|
}
|
|
/*if (moveStatus == storeMoveStatusBeforeStop) {
|
// 同方向再启动，继续判断是否停车超时
|
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) {
|
// 停车超2秒，每次扣5分
|
AddExamFault(20406, rtkTime);
|
DEBUG("停车超时");
|
}
|
} else {
|
// 倒车切换到前进
|
if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) {
|
if (parkStatus == 0) {
|
if (EnterParking(map, car)) {
|
parkStatus = 1;
|
parkSuccess = true;
|
}
|
}
|
|
if (!parkSuccess && !reportParkFail) {
|
// 停止后，车身出线，不合格
|
AddExamFault(20401, rtkTime);
|
reportParkFail = true;
|
DEBUG("移库不入");
|
}
|
|
// 在这里检查转向灯状态
|
if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
|
// 不开转向灯，扣10分
|
AddExamFault(20405, rtkTime);
|
DEBUG("未开启转向灯");
|
}
|
}
|
}*/
|
|
/* if (moveStatus == -1 && !occurMoveBack) {
|
DEBUG("开始倒车");
|
occurMoveBack = true;
|
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
|
}*/
|
}
|
|
prevMoveStatus = moveStatus;
|
} /*else if (moveStatus == 0 && parkStatus == 0) {
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
|
if (EnterParking(map, car)) {
|
parkStatus = 1;
|
parkSuccess = true;
|
} else {
|
parkStatus = -1;
|
}
|
}
|
}*/
|
|
TEST_END:
|
if (testStatus == TEST_FINISH) {
|
DEBUG("侧方停车结束");
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
|
return 0;
|
}
|
return 1;
|
}
|
|
int EnterParkEdge(prime_t &prime) {
|
if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
|
return -1;
|
}
|
|
PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
|
PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
|
PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
|
PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
|
|
Line left_trace, right_trace;
|
|
MAKE_LINE(left_trace, lp1, lp2);
|
MAKE_LINE(right_trace, rp1, rp2);
|
|
// 不同地区左右倒库次序可能不同，所以两个方向都可以进入
|
for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
|
Line left_entrance_line;
|
|
MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
|
|
if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection
|
&& IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) {
|
return i;
|
}
|
}
|
return -1;
|
}
|
|
// 车轮是否压道路边线
|
static bool CrashRedLine1(const Polygon *map, const car_model_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->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_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_t *car)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}};
|
|
Polygon car_body;
|
|
car_body.num = car->bodyNum;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->carXY[car->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 CrashRedLine3(const Polygon *map, const car_model_t *car) {
|
bool ret = false;
|
|
if (!occurMoveBack) {
|
// 倒车前，车身不得压库位虚线
|
Polygon car_body;
|
Line red_line;
|
|
car_body.num = car->bodyNum;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->carXY[car->body[i]];
|
}
|
|
MakeLine(&red_line, &map->point[2], &map->point[5]);
|
if (IntersectionOf(red_line, &car_body) != GM_None) {
|
ret = true;
|
}
|
|
free(car_body.point);
|
}
|
|
return ret;
|
}
|
