//
|
// Created by YY on 2019/10/23.
|
//
|
|
#include "park_bottom.h"
|
#include "../common/apptimer.h"
|
#include "../Geometry.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "../driver_test.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
|
};
|
|
const int PARK_TIMEOUT = 210;
|
|
static bool PBTesting = false;
|
static bool trigLeaveTestAreaDetect = false;
|
static bool leaveTestArea = false;
|
static bool stopCar2S = false;
|
|
static int currTarget;
|
static bool leftTireCrossLeftLine, leftTireCrossRightLine, rightTireCrossLeftLine, rightTireCrossRightLine;
|
static char first_ctrl_line_id;
|
static bool carStopEvent; // 中途停车标记
|
static bool carParkSuccess; // 是否停在库位
|
static bool parkTimeout;
|
|
static void StopCarTimeout(union sigval sig);
|
static void LeaveTestAreaLongtime(union sigval sig);
|
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car);
|
static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
|
static void ParkBottomTimeout(union sigval sig);
|
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
|
|
void StartParkBottom(void)
|
{
|
stopCar2S = false;
|
trigLeaveTestAreaDetect = false;
|
leaveTestArea = false;
|
PBTesting = true;
|
parkTimeout = false;
|
first_ctrl_line_id = 0;
|
currTarget = FIRST_TOUCH_CTRL_LINE;
|
leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
|
|
TextOsd(0, "ParkBottom");
|
}
|
|
void StopParkBottom(void)
|
{
|
PBTesting = false;
|
AppTimer_delete(StopCarTimeout);
|
AppTimer_delete(ParkBottomTimeout);
|
AppTimer_delete(LeaveTestAreaLongtime);
|
currTarget = NONE;
|
|
TextOsd(0, "ParkBottom End");
|
}
|
|
int TestParkBottom(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
|
{
|
int status = 0;
|
|
if (!PBTesting)
|
return -2;
|
|
DEBUG("TestParkBottom speed %f dir %d", speed, run_status);
|
|
|
if (currTarget > FIRST_TOUCH_CTRL_LINE) {
|
// 是否超时
|
if (parkTimeout) {
|
// 不合格：动作超时
|
err.push_back(10);
|
status = -1;
|
}
|
// 是否压线
|
if (CrashRedLine(map, car)) {
|
// 不合格：车身出线
|
err.push_back(7);
|
status = -1;
|
}
|
|
if (trigLeaveTestAreaDetect) {
|
if (IntersectionOf(car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], map) == GM_Containment &&
|
IntersectionOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], map) == GM_Containment) {
|
trigLeaveTestAreaDetect = false;
|
AppTimer_delete(LeaveTestAreaLongtime);
|
}
|
}
|
}
|
|
if (currTarget == FIRST_TOUCH_CTRL_LINE ||
|
currTarget == SECOND_TOUCH_CTRL_LINE ||
|
currTarget == THIRD_TOUCH_CTRL_LINE) {
|
if (run_status > 0) {
|
if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
|
// 扣5分：中途停车超过2秒
|
err.push_back(11);
|
}
|
|
if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
|
(leftTireCrossRightLine && rightTireCrossRightLine))) {
|
DEBUG("CrossCtrlLine");
|
CrossCtrlLine(map, car);
|
} else if (currTarget == FIRST_TOUCH_CTRL_LINE || currTarget == SECOND_TOUCH_CTRL_LINE) {
|
// 跨过控制线后，车辆持续向前行驶，处理这些乱搞情况
|
// 整个车都离开测试区后，如果持续15秒，还没回到测试区，就忽略该测试或者淘汰
|
if (leaveTestArea) {
|
if (currTarget == FIRST_TOUCH_CTRL_LINE) {
|
status = -2;
|
TextOsd(0, "来道场玩的");
|
DEBUG("来道场玩的");
|
} else {
|
// 不合格：未按规定线路行驶（直接跑出测试区了）
|
err.push_back(6);
|
status = -1;
|
TextOsd(0, "直接跑出测试区了");
|
DEBUG("直接跑出测试区了");
|
}
|
} else if (!trigLeaveTestAreaDetect) {
|
Polygon car_body;
|
|
car_body.num = car->desc->body_num;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->points[car->desc->body[i]];
|
}
|
|
if (IntersectionOf(map, &car_body) == GM_None) {
|
trigLeaveTestAreaDetect = true;
|
AppTimer_delete(LeaveTestAreaLongtime);
|
AppTimer_add(LeaveTestAreaLongtime, D_SEC(15));
|
DEBUG("开始离场计时");
|
}
|
|
free(car_body.point);
|
}
|
}
|
|
if (currTarget == THIRD_TOUCH_CTRL_LINE) {
|
char the_ctrl_line_crossed = 0;
|
|
if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
|
the_ctrl_line_crossed = 'L';
|
} else if (leftTireCrossRightLine && rightTireCrossRightLine) {
|
the_ctrl_line_crossed = 'R';
|
}
|
|
if (the_ctrl_line_crossed != 0 && the_ctrl_line_crossed == first_ctrl_line_id) {
|
// 项目完成
|
status = 1;
|
} else if (the_ctrl_line_crossed != 0) {
|
// 不合格：未按规定线路行驶（未回到起始点）
|
err.push_back(6);
|
status = -1;
|
}
|
}
|
if (carStopEvent)
|
AppTimer_delete(StopCarTimeout);
|
carStopEvent = false;
|
stopCar2S = false;
|
} else if (run_status < 0) {
|
// 左右倒库大纲并未要求谁先完成，故以先越过的控制线为准，下次得越过另外一条
|
char the_ctrl_line_crossed = 0;
|
|
if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
|
the_ctrl_line_crossed = 'L';
|
} else if (leftTireCrossRightLine && rightTireCrossRightLine) {
|
the_ctrl_line_crossed = 'R';
|
}
|
|
if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
|
// 不合格：未按规定线路行驶（试图做2次同方向的倒库）
|
err.push_back(6);
|
status = -1;
|
} else if (the_ctrl_line_crossed > 0 && first_ctrl_line_id == 0) {
|
first_ctrl_line_id = the_ctrl_line_crossed;
|
// 项目正式开始，210秒内完成
|
AppTimer_delete(ParkBottomTimeout);
|
AppTimer_add(ParkBottomTimeout, D_SEC(PARK_TIMEOUT));
|
currTarget = FIRST_PARK;
|
carParkSuccess = false;
|
parkTimeout = false;
|
leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
|
|
TextOsd(0, "第一次倒库");
|
} else if (the_ctrl_line_crossed > 0) {
|
currTarget = SECOND_PARK;
|
carParkSuccess = false;
|
leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
|
TextOsd(0, "第二次倒库");
|
} else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
|
// 不合格：倒车前，2前轮没驶过控制线
|
err.push_back(9);
|
status = -1;
|
}
|
if (carStopEvent)
|
AppTimer_delete(StopCarTimeout);
|
carStopEvent = false;
|
stopCar2S = false;
|
} else {
|
if (!carStopEvent) {
|
AppTimer_delete(StopCarTimeout);
|
AppTimer_add(StopCarTimeout, D_SEC(2));
|
}
|
carStopEvent = true;
|
}
|
} else if (currTarget == FIRST_PARK || currTarget == SECOND_PARK) {
|
if (run_status < 0) {
|
if (stopCar2S) {
|
// 扣5分：中途停车
|
err.push_back(11);
|
}
|
|
carStopEvent = false;
|
stopCar2S = false;
|
} else if (run_status == 0) {
|
// 立即检查是否停车到位，也许是中途停车，先不管，待发生前进事件后，再断定是否停车到位
|
if (!carStopEvent) {
|
carStopEvent = true;
|
carParkSuccess = EnterParking(map, car);
|
AppTimer_delete(StopCarTimeout);
|
AppTimer_add(StopCarTimeout, D_SEC(2));
|
}
|
} else {
|
if (carStopEvent) {
|
if (!carParkSuccess) {
|
// 不合格：倒库不入
|
err.push_back(8);
|
status = -1;
|
} else if (currTarget == FIRST_PARK) {
|
currTarget = SECOND_TOUCH_CTRL_LINE;
|
TextOsd(0, "过另一根控制线");
|
} else {
|
currTarget = THIRD_TOUCH_CTRL_LINE;
|
TextOsd(0, "再过第一根控制线");
|
}
|
}
|
carStopEvent = false;
|
stopCar2S = false;
|
}
|
}
|
|
if (status != 0) {
|
StopParkBottom();
|
}
|
|
return status;
|
}
|
|
static void StopCarTimeout(union sigval sig) {
|
AppTimer_delete(StopCarTimeout);
|
|
stopCar2S = true;
|
}
|
|
static void LeaveTestAreaLongtime(union sigval sig) {
|
AppTimer_delete(LeaveTestAreaLongtime);
|
leaveTestArea = true;
|
trigLeaveTestAreaDetect = false;
|
}
|
|
static void ParkBottomTimeout(union sigval sig) {
|
AppTimer_delete(ParkBottomTimeout);
|
parkTimeout = true;
|
}
|
|
// 检测2前轮是否正向越过左右控制线
|
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car)
|
{
|
Line leftCtrlLine, rightCtrlLine;
|
Line track1;
|
PointF p1, p2;
|
|
car_model_cache_t *prev_car = GetCarModelCache(1);
|
if (prev_car == NULL)
|
return;
|
|
MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]);
|
MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]);
|
|
// 左前轮，取轮宽的中点
|
p1.X = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
|
p1.Y = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
|
|
p2.X = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
|
p2.Y = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
|
|
MakeLine(&track1, &p1, &p2);
|
|
if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
|
IntersectionOf(p1, map) == GM_None) {
|
leftTireCrossLeftLine = true;
|
}
|
|
if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
|
IntersectionOf(p1, map) == GM_None) {
|
leftTireCrossRightLine = true;
|
}
|
|
// 右前轮
|
p1.X = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
|
p1.Y = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
|
|
p2.X = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
|
p2.Y = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
|
|
MakeLine(&track1, &p1, &p2);
|
|
if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
|
IntersectionOf(p1, map) == GM_None) {
|
rightTireCrossLeftLine = true;
|
}
|
if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
|
IntersectionOf(p1, map) == GM_None) {
|
rightTireCrossRightLine = true;
|
}
|
}
|
|
static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
|
bool succ = false;
|
|
Polygon parking;
|
Polygon car_body;
|
|
car_body.num = car->desc->body_num;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->points[car->desc->body[i]];
|
}
|
|
MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
|
|
if (IntersectionOf(&car_body, &parking) == GM_Containment) {
|
succ = true;
|
}
|
|
CleanPolygon(&parking);
|
free(car_body.point);
|
|
return succ;
|
}
|
|
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
|
|
Polygon car_body;
|
|
car_body.num = car->desc->body_num;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->points[car->desc->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;
|
}
|
