//
|
// Created by YY on 2019/10/23.
|
//
|
//
|
// 0 ____________________________________7
|
//
|
//
|
// 1 _____________2 5_________________6
|
// 8| |9
|
// | |
|
// | |
|
// | |
|
// 3--------4
|
//
|
|
#include "park_bottom.h"
|
#include "../common/apptimer.h"
|
#include "../test_common/Geometry.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "../driver_test.h"
|
#include "../utils/xconvert.h"
|
#include "../master/comm_if.h"
|
#include "area_exam.h"
|
#include "../test_common/car_sensor.h"
|
#include "../test_common/odo_graph.h"
|
#include <vector>
|
#include <cstdlib>
|
|
#define DEBUG(fmt, args...) LOGD("<park_bottom> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
enum {
|
NONE,
|
FIRST_TOUCH_CTRL_LINE,
|
FIRST_PARK,
|
SECOND_TOUCH_CTRL_LINE,
|
SECOND_PARK,
|
THIRD_TOUCH_CTRL_LINE
|
};
|
|
enum {
|
TESTING,
|
TEST_FAIL, // 因触发某些规则，在车身未完全立场情况下，提前终止部分测试
|
TEST_FINISH
|
};
|
|
typedef enum {
|
NONE_CTRL_LINE,
|
LEFT_CTRL_LINE,
|
RIGHT_CTRL_LINE
|
} ctrl_line_t;
|
|
static bool reverseCar = false;
|
|
const uint32_t CHECK_PARK_DELAY = 400;
|
|
static uint32_t stopTimepoint;
|
static int prevMoveDirect;
|
static bool occurCrashRedLine;
|
|
static uint32_t firstReverseTimepoint;
|
|
static int parkCount;
|
static char carray[3];
|
static int darray[3];
|
static int parkStatus[3];
|
|
static int gearAtStop;
|
static int currGear;
|
static ctrl_line_t prevCrossedCtrlLine;
|
static double odo;
|
|
static ctrl_line_t CrossCtrlLine(prime_t &prime);
|
static bool EnterParking(prime_t &prime);
|
static bool CrashRedLine(prime_t &prime);
|
|
|
void StartParkBottom(prime_t &prime)
|
{
|
DEBUG("StartParkBottom");
|
|
reverseCar = false;
|
memset(carray, 0, sizeof(carray));
|
memset(darray, 0, sizeof(darray));
|
memset(parkStatus, 0, sizeof(parkStatus));
|
prevMoveDirect = prime.pMotion->move;
|
|
firstReverseTimepoint = 0;
|
|
parkCount = 0;
|
|
occurCrashRedLine = false;
|
|
currGear = ReadCarStatus(GEAR);
|
prevCrossedCtrlLine = NONE_CTRL_LINE;
|
stopTimepoint = 0;
|
|
odo = ReadOdo();
|
|
PlayTTS("您已进入倒车入库区域", NULL);
|
}
|
|
void TestParkBottom(prime_t &prime)
|
{
|
ctrl_line_t crossCtrlLine = NONE_CTRL_LINE;
|
uint32_t tp = AppTimer_GetTickCount();
|
|
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 (gear_change) {
|
if (currGear == GEAR_R) {
|
// 挂倒挡,检测是否过控制线
|
DEBUG("开始挂倒挡");
|
if (!reverseCar) {
|
DEBUG("开始首轮入库");
|
reverseCar = true;
|
MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
|
firstReverseTimepoint = tp; // 开始210秒计时
|
}
|
crossCtrlLine = CrossCtrlLine(prime);
|
|
if (parkCount >= 2) {
|
DEBUG("开始次轮入库");
|
parkCount = 0;
|
MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
|
firstReverseTimepoint = tp; // 开始210秒计时
|
}
|
|
if (crossCtrlLine == NONE_CTRL_LINE) {
|
// 倒车前，前轮未驶过控制线
|
AddExamFault(20104);
|
DEBUG("倒车前，前轮未驶过控制线");
|
} else if (crossCtrlLine == prevCrossedCtrlLine) {
|
// 重复跨越同一控制线，不按规定线路，顺序形式，不合格
|
AddExamFault(20101);
|
DEBUG("不按规定线路，顺序形式， 同 %c 侧", prevCrossedCtrlLine);
|
} else {
|
prevCrossedCtrlLine = crossCtrlLine;
|
DEBUG("开始 %c 侧 倒库", prevCrossedCtrlLine);
|
}
|
} else {
|
// 从倒挡移出，检测是否入库
|
DEBUG("从倒挡移出");
|
|
parkCount++;
|
|
DEBUG("库位检查次数 = %d", parkCount);
|
|
if (EnterParking(prime)) {
|
DEBUG("倒库成功");
|
} else {
|
AddExamFault(20103);
|
DEBUG("倒库不入");
|
}
|
}
|
}
|
|
if (ExitParkArea(prime)) {
|
// 离开场地
|
DEBUG("离开场地");
|
if (parkCount < 2) {
|
AddExamFault(10103);
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
}
|
prime.curr_exam_map.type = 0;
|
goto TEST_END;
|
}
|
|
if (BodyCollidingLine(prime) >= 0) {
|
if (!occurCrashRedLine) {
|
occurCrashRedLine = true;
|
// 车身出线，不合格
|
AddExamFault(10116);
|
DEBUG("车身出线");
|
}
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
if (prime.pMotion->move != prevMoveDirect) {
|
if (prime.pMotion->move == STOP) {
|
stopTimepoint = tp;
|
gearAtStop = (currGear == GEAR_R ? 1 : 0);
|
DEBUG("停车了");
|
DEBUG("停车时挡位 = %d", gearAtStop);
|
} else if (prevMoveDirect == STOP && stopTimepoint > 0) {
|
DEBUG("继续行驶");
|
|
DEBUG("停车时间 %ld", tp - stopTimepoint);
|
DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);
|
|
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
|
&& gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
|
// 停车超2秒，每次扣5分
|
AddExamFault(20106);
|
DEBUG("中途停车");
|
}
|
}
|
|
prevMoveDirect = prime.pMotion->move;
|
}
|
TEST_END:
|
DEBUG("倒库结束");
|
MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0);
|
}
|
|
static int BodyCollidingLine(prime_t &prime)
|
{
|
vector<Line> lines;
|
|
Polygon car_body;
|
|
car_body.num = prime.pModel->body.size();
|
car_body.point = new PointF[car_body.num];
|
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
|
}
|
|
Line line;
|
MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
|
lines.push_back(line);
|
MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]);
|
lines.push_back(line);
|
MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]);
|
lines.push_back(line);
|
MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]);
|
lines.push_back(line);
|
MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]);
|
lines.push_back(line);
|
MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]);
|
lines.push_back(line);
|
|
int idx = 0;
|
|
for (auto line: lines) {
|
if (IntersectionOf(line, &car_body) != GM_None) {
|
break;
|
}
|
idx++;
|
}
|
delete []car_body.point;
|
return idx < lines.size()? idx : -1;
|
}
|
|
// 检测2前轮是否正向越过左右控制线
|
static ctrl_line_t CrossCtrlLine(prime_t &prime)
|
{
|
// 过右控制线
|
Line left_trace, right_trace;
|
Line left_ctrl, right_ctrl;
|
|
MAKE_LINE(left_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
|
prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]]);
|
MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
|
prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
|
|
MAKE_LINE(left_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]);
|
MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
|
|
|
if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
|
IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
|
IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == RELATION_LEFT) {
|
return LEFT_CTRL_LINE;
|
}
|
|
if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
|
IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
|
IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == RELATION_RIGHT) {
|
return RIGHT_CTRL_LINE;
|
}
|
|
return NONE_CTRL_LINE;
|
}
|
|
// 需要库入口线宽计算在内
|
static bool EnterParking(prime_t &prime) {
|
bool succ = false;
|
|
Polygon park_area;
|
Polygon car_body;
|
|
car_body.num = prime.pModel->body.size();
|
car_body.point = new PointF[car_body.num];
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
|
}
|
|
PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
|
YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]));
|
|
PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
|
prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
|
YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]));
|
|
MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9});
|
|
if (IntersectionOf(&car_body, &park_area) == GM_Containment) {
|
succ = true;
|
}
|
|
CleanPolygon(&park_area);
|
free(car_body.point);
|
|
DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");
|
|
return succ;
|
}
|
|
// 4个车轮和车头点不在场地中
|
bool ExitParkArea(prime_t &prime)
|
{
|
Polygon polygon;
|
|
polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size();
|
polygon.point = new PointF[polygon.num];
|
for (int i = 0; i < polygon.num; ++i) {
|
polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i];
|
}
|
|
int num = 0;
|
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
|
delete []polygon.point;
|
|
return num == 5? true : false;
|
}
|
