//
|
// Created by YY on 2019/10/23.
|
//
|
//
|
// 4 ___________________________ 3
|
// | |
|
// | |
|
// | |
|
// ________________|9 8|___________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>
|
#include <tuple>
|
|
#define DEBUG(fmt, args...) LOGD("<park_edge> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
static bool overpass_parkingspace;
|
static bool check_parking_space;
|
|
const uint32_t CHECK_PARK_DELAY = 400;
|
|
static bool occurCrashRedLine;
|
static bool firstMoveBack, checkLight;
|
|
static void ParkTimeout(apptimer_var_t val);
|
static bool CrashRedLine(prime_t &prime);
|
static bool CheckParkspace(prime_t &prime);
|
static bool OverpassParkspace(prime_t &prime);
|
static void MotionChange(move_status_t mv);
|
|
enum {
|
PE_PREPARE_PARK,
|
PE_PARKING,
|
PE_EXIT
|
};
|
|
void StartParkEdge(prime_t &prime)
|
{
|
DEBUG("进入侧方停车场地");
|
|
occurCrashRedLine = false; // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
|
firstMoveBack = false;
|
checkLight = false;
|
check_parking_space = false;
|
prime.examing_area.stage = PE_PREPARE_PARK;
|
PlayTTS("您已进入侧方停车区域", NULL);
|
}
|
|
void StopParkEdge(prime_t &prime)
|
{
|
if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
|
return;
|
DEBUG("离开侧方停车场地");
|
|
if (!check_parking_space) {
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
AddExamFault(10103);
|
}
|
prime.examing_area.type = MAP_TYPE_NONE;
|
|
AppTimer_delete(ParkTimeout);
|
}
|
|
static void ParkTimeout(apptimer_var_t val)
|
{
|
// 超时90秒，不合格
|
DEBUG("移库90秒超时");
|
AddExamFault(20402);
|
}
|
|
void TestParkEdge(prime_t &prime)
|
{
|
if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
|
return;
|
// 检测后轮是否驶过库位前段
|
if (!overpass_parkingspace) {
|
overpass_parkingspace = OverpassParkspace(prime);
|
}
|
|
if (prime.pMotion->move == BACKWARD && prime.sensor.gear == GEAR_R) {
|
if (!firstMoveBack) {
|
// 开始挂倒挡倒车
|
firstMoveBack = true;
|
if (!overpass_parkingspace) {
|
DEBUG("后轮没有开过库位线就开始倒车");
|
AddExamFault(10103);
|
}
|
prime.examing_area.stage = PE_PARKING;
|
AppTimer_delete(ParkTimeout);
|
AppTimer_add(ParkTimeout, prime.examParam.park_edge_limit_time);
|
}
|
}
|
|
//从倒车状态，看是否从倒挡退出(如果只是看停车状态，无法和中途停车有效区分)
|
if (firstMoveBack && prime.sensor.gear != GEAR_R) {
|
if (!check_parking_space) {
|
check_parking_space = true;
|
if (!CheckParkspace(prime)) {
|
// 停止后，车身出线，不合格
|
DEBUG("车身出线");
|
AddExamFault(20401);
|
}
|
prime.examing_area.stage = PE_EXIT;
|
}
|
}
|
|
// 检测左转向灯
|
if (check_parking_space && prime.pMotion->move == FORWARD && !checkLight) {
|
checkLight = true;
|
if (prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT) {
|
// 不开转向灯，扣10分
|
DEBUG("未开启转向灯");
|
AddExamFault(20405);
|
}
|
}
|
|
if (CrashRedLine(prime)) {
|
if (!occurCrashRedLine) {
|
// 车身压库位线，每次扣10分
|
DEBUG("车身压库位线");
|
AddExamFault(20404);
|
occurCrashRedLine = true;
|
}
|
} else {
|
occurCrashRedLine = false;
|
}
|
}
|
|
// 车轮或车身是否压道路边线
|
static bool CrashRedLine(prime_t &prime)
|
{
|
const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
|
|
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]]);
|
|
MakePolygon body(prime.pModel->body.size());
|
for (int i = 0; i < prime.pModel->body.size(); ++i) {
|
body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
|
}
|
|
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_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_EDGE>(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, body.GetPolygon()) != GM_None) {
|
return true;
|
}
|
}
|
|
return false;
|
}
|
|
static bool CheckParkspace(prime_t &prime) {
|
DEBUG("检查停车到位...");
|
|
MakePolygon area({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[8],
|
std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
|
std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
|
std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[9]});
|
MakePolygon car_body(prime.pModel->body.size());
|
for (int i = 0; i < prime.pModel->body.size(); ++i) {
|
car_body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
|
}
|
|
return (IntersectionOf(car_body.GetPolygon(), area.GetPolygon()) == GM_Containment)? true : false;
|
}
|
|
static bool OverpassParkspace(prime_t &prime)
|
{
|
Line parkspace_top;
|
|
MAKE_LINE(parkspace_top, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5]);
|
|
if (IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT
|
&& IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT) {
|
return true;
|
}
|
return false;
|
}
|
