//
|
// 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 "../teach/train.h"
|
#include "../teach/teach.h"
|
#include <vector>
|
#include <cstdlib>
|
#include <tuple>
|
|
#define DEBUG(fmt, args...) LOGD("<park_bottom> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
typedef enum {
|
NONE_CTRL_LINE,
|
LEFT_CTRL_LINE,
|
RIGHT_CTRL_LINE
|
} ctrl_line_t;
|
|
static bool occurCrashRedLine;
|
static int parkCount;
|
static int reverseCnt = 0;
|
static bool stopFlag;
|
static ctrl_line_t prevCrossedCtrlLine;
|
|
static bool checkCtrlLine, checkParkspace;
|
static int handleGearOb, handleMoveOb;
|
|
static bool BodyCollidingLine(prime_t &prime);
|
static ctrl_line_t CrossCtrlLine(prime_t &prime);
|
static bool CheckParkspace(prime_t &prime);
|
static void MoveOb(move_status_t curr, move_status_t prev, double distance);
|
static void GearOb(int gear, int prevGear, double move_distance);
|
|
enum {
|
PREPARE_REVERSE,
|
REVERSE_LEFT,
|
REVERSE_RIGHT,
|
OBVERSE_LEFT,
|
OBVERSE_RIGHT
|
};
|
|
/*
|
1. 进入 --- 首次倒库前
|
2. 倒库 ---- 停车到位前（右倒库）
|
3. 左出库
|
4. 倒库 ---- 停车到位前（左倒库）
|
5. 右出库(离开场地)
|
*/
|
|
void StartParkBottom(prime_t &prime)
|
{
|
DEBUG("进入倒库场地");
|
stopFlag = false;
|
parkCount = 0;
|
reverseCnt = 0;
|
checkCtrlLine = false;
|
checkParkspace = false;
|
occurCrashRedLine = false;
|
prime.examing_area.stage = PREPARE_REVERSE;
|
testAll();
|
LoadStageTips(prime);
|
prevCrossedCtrlLine = NONE_CTRL_LINE;
|
handleGearOb = RegisterShiftObserver(GearOb);
|
handleMoveOb = RegisterCarMoveObserver(MoveOb);
|
PlayTTS("您已进入倒车入库区域", NULL);
|
}
|
|
static void ParkTimeout(apptimer_var_t var)
|
{
|
DEBUG("项目超时");
|
AddExamFault(20105);
|
}
|
|
static void MoveOb(move_status_t curr, move_status_t prev, double distance)
|
{
|
// 车辆由运动到停止，记录一下，当换挡发生时，避免原地来回换挡造成误判
|
if (curr == STOP && distance > 0.5) {
|
stopFlag = true;
|
}
|
}
|
|
static void GearOb(int gear, int prevGear, double move_distance)
|
{
|
DEBUG("GearOb %d %f", gear, move_distance);
|
|
if (gear == GEAR_R && stopFlag) { // 移入倒挡
|
stopFlag = false;
|
reverseCnt++;
|
if (reverseCnt == 1) {
|
AppTimer_delete(ParkTimeout);
|
AppTimer_add(ParkTimeout, GetPrime().examParam.park_bottom_limit_time);
|
}
|
checkCtrlLine = true;
|
} else if (prevGear == GEAR_R && stopFlag) { // 从倒挡移出
|
stopFlag = false;
|
checkParkspace = true;
|
}
|
}
|
|
void StopParkBottom(prime_t &prime)
|
{
|
if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
|
return;
|
|
DEBUG("离开倒库场地");
|
|
if (parkCount < 2) {
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
AddExamFault(10103);
|
}
|
AppTimer_delete(ParkTimeout);
|
prime.examing_area.type = MAP_TYPE_NONE;
|
UnregisterShiftObserver(handleGearOb);
|
UnregisterCarMoveObserver(handleMoveOb);
|
}
|
|
void TestParkBottom(prime_t &prime)
|
{
|
if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
|
return;
|
|
train(prime);
|
|
if (checkCtrlLine) {
|
ctrl_line_t crossCtrlLine = CrossCtrlLine(prime);
|
|
if (crossCtrlLine == NONE_CTRL_LINE) {
|
// 倒车前，前轮未驶过控制线
|
DEBUG("倒车前，前轮未驶过控制线");
|
AddExamFault(20104);
|
} else if (crossCtrlLine == prevCrossedCtrlLine) {
|
// 重复跨越同一控制线，不按规定线路，顺序形式，不合格
|
AddExamFault(20101);
|
DEBUG("不按规定线路，顺序形式， 同 %d 侧", prevCrossedCtrlLine);
|
} else {
|
prevCrossedCtrlLine = crossCtrlLine;
|
DEBUG("开始 %d 侧 倒库", prevCrossedCtrlLine);
|
}
|
|
if (DeltaYaw(YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
|
std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4]), prime.pModeling[prime.curr_modeling_index].yaw) < 90) {
|
DEBUG("右侧倒库");
|
prime.examing_area.stage = REVERSE_RIGHT;
|
} else {
|
DEBUG("左侧倒库");
|
prime.examing_area.stage = REVERSE_LEFT;
|
}
|
LoadStageTips(prime);
|
checkCtrlLine = false;
|
}
|
|
if (checkParkspace) {
|
if (CheckParkspace(prime)) {
|
DEBUG("倒库成功");
|
} else {
|
DEBUG("倒库不入");
|
AddExamFault(20103);
|
}
|
parkCount++;
|
if (prime.examing_area.stage == REVERSE_RIGHT) {
|
prime.examing_area.stage = OBVERSE_LEFT; // 从左侧倒库
|
} else {
|
prime.examing_area.stage = OBVERSE_RIGHT; // 离开场地
|
}
|
LoadStageTips(prime);
|
checkParkspace = false;
|
}
|
|
if (BodyCollidingLine(prime)) {
|
if (!occurCrashRedLine) {
|
occurCrashRedLine = true;
|
// 车身出线，不合格
|
AddExamFault(10116);
|
DEBUG("车身出线");
|
}
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
teach(prime);
|
}
|
|
// 车身或车轮皆不得碰触实线
|
static bool BodyCollidingLine(prime_t &prime)
|
{
|
MakePolygon car_body(prime.pModel->body.size());
|
|
for (int i = 0; i < prime.pModel->body.size(); ++i) {
|
car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
|
}
|
|
Line frontAxle, rearAxle;
|
|
MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
|
MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
|
|
const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
|
|
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
|
Line red_line;
|
MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
|
if (IntersectionOf(red_line, frontAxle) == GM_Intersection
|
|| IntersectionOf(red_line, rearAxle) == GM_Intersection
|
|| IntersectionOf(red_line, car_body.GetPolygon()) != GM_None) {
|
return true;
|
}
|
}
|
return false;
|
}
|
|
// 检测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, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0]);
|
MAKE_LINE(right_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[6], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[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) == REL_POS_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) == REL_POS_RIGHT) {
|
return RIGHT_CTRL_LINE;
|
}
|
|
return NONE_CTRL_LINE;
|
}
|
|
static bool CheckParkspace(prime_t &prime) {
|
MakePolygon car_body(prime.pModel->body.size());
|
|
for (int i = 0; i < prime.pModel->body.size(); ++i) {
|
car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
|
}
|
|
MakePolygon park_area({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[8],
|
std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
|
std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4],
|
std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[9]});
|
|
return (bool) (IntersectionOf(car_body.GetPolygon(), park_area.GetPolygon()) == GM_Containment);
|
}
|
