//
|
// Created by YY on 2019/11/4.
|
//
|
|
#include "driving_curve.h"
|
#include "../jni_log.h"
|
#include "../driver_test.h"
|
#include "../common/apptimer.h"
|
#include "../utils/xconvert.h"
|
#include "../master/comm_if.h"
|
#include "../native-lib.h"
|
#include "area_exam.h"
|
|
#include <vector>
|
#include <cstdlib>
|
|
using namespace std;
|
|
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
|
|
static bool testing = false;
|
static int mapIndex = 0;
|
static uint32_t stopTimepoint = 0;
|
|
static bool reportStopCarTimeout;
|
static int prevMoveDirect;
|
static bool crashRedLine;
|
static struct scan_window_t {
|
int leftStart;
|
int leftEnd;
|
int rightStart;
|
int rightEnd;
|
} scanWindow;
|
|
static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
|
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
|
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);
|
|
void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
DEBUG("进入曲线行驶场地");
|
|
testing = true;
|
mapIndex = index;
|
|
prevMoveDirect = moveDirect;
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
}
|
reportStopCarTimeout = false;
|
crashRedLine = false;
|
|
scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
|
|
MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
|
}
|
|
// 曲线场地关键点
|
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
|
int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
// 判断是否在范围内，如果4个车轮都不在曲线内，视为车辆离开场地
|
vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
|
car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
|
car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
|
car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};
|
|
int collide = 0;
|
|
for (auto px: pxs) {
|
if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
|
// 位于前半部,实际场地左右边线起点、终点和圆心基本不会共线，可能差异近一米，所以要分开计算
|
// 判断的点超过左右起点，即在线的左侧
|
if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
|
if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
|
collide = 1;
|
continue;
|
}
|
}
|
if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
|
if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
|
collide = 2;
|
continue;
|
}
|
}
|
} else {
|
// 判断的点不超过左右终点，即在线的左侧
|
if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
|
if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
|
collide = 2;
|
continue;
|
}
|
}
|
if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
|
if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
|
collide = 1;
|
continue;
|
}
|
}
|
}
|
}
|
|
}
|
|
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
Polygon tireRect;
|
int who = 0;
|
vector<double> dtox;
|
vector<Line> line_set;
|
int s;
|
|
MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
|
car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
|
car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
|
car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]});
|
|
// 更新车头扫描线
|
if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
|
DEBUG("离开场地");
|
testing = false;
|
goto TEST_END;
|
}
|
|
// 更新车尾扫描线
|
UpdateEndLine(false, &scanWindow, map, map2, &tireRect);
|
|
// 计算边距
|
s = scanWindow.leftStart;
|
for (int e = scanWindow.leftStart - 1; e >= scanWindow.leftEnd; --e) {
|
Line redLine;
|
MakeLine(&redLine, &map->point[s], &map->point[e]);
|
line_set.push_back(redLine);
|
s = e;
|
}
|
|
s = scanWindow.rightStart;
|
for (int e = scanWindow.rightStart - 1; e >= scanWindow.rightEnd; --e) {
|
Line redLine;
|
MakeLine(&redLine, &map2->point[s], &map2->point[e]);
|
line_set.push_back(redLine);
|
s = e;
|
}
|
|
DistanceOfTire2X(dtox, car, line_set);
|
|
MA_SendDistance(dtox[0], dtox[1]);
|
|
DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
|
|
if (CrashRedLine(map, map2, car, &scanWindow, who)) {
|
if (!crashRedLine) {
|
crashRedLine = true;
|
// 车轮压边线，不合格
|
AddExamFault(20601, rtkTime);
|
DEBUG("车轮压边线");
|
if (who == 1) {
|
PlayTTS("压左曲线", NULL);
|
} else if (who == 2) {
|
PlayTTS("压右曲线", NULL);
|
}
|
}
|
} else {
|
crashRedLine = false;
|
}
|
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
reportStopCarTimeout = false;
|
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
} else {
|
|
}
|
prevMoveDirect = moveDirect;
|
} else if (moveDirect == 0) {
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.curve_pause_criteria) && !reportStopCarTimeout) {
|
// 停车超2秒，不合格
|
AddExamFault(20602, rtkTime);
|
DEBUG("中途停车");
|
reportStopCarTimeout = true;
|
}
|
}
|
|
TEST_END:
|
CleanPolygon(&tireRect);
|
|
if (!testing) {
|
MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
|
}
|
|
return testing ? 1 : 0;
|
}
|
|
static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
|
{
|
Line start;
|
|
bool update = true;
|
int direct = 0;
|
int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;
|
|
while (update) {
|
update = false;
|
MakeLine(&start, &map->point[scanWindow.leftStart], &map2->point[scanWindow.rightStart]);
|
|
if (IntersectionOf(start, tireRect) == GM_None) {
|
if (direct != 1) {
|
direct = -1;
|
// 入场方向扫描
|
tempLeft = scanWindow.leftStart;
|
tempRight = scanWindow.rightStart;
|
|
if (scanWindow.leftStart > 0) {
|
update = true;
|
scanWindow.leftStart--;
|
}
|
if (scanWindow.rightStart > 0) {
|
update = true;
|
scanWindow.rightStart--;
|
}
|
|
if (scanWindow.leftStart <= scanWindow.leftEnd && scanWindow.rightStart <= scanWindow.rightEnd) {
|
DEBUG("车辆丢失，重新搜索 %d %d", scanWindow.leftStart, scanWindow.rightStart);
|
|
// 车辆丢失，重新搜索
|
update = false;
|
scanWindow.leftEnd = scanWindow.rightEnd = 0;
|
|
if (UpdateEndLine(true, &scanWindow, map, map2, tireRect)) {
|
DEBUG("匹配成功 %d %d", scanWindow.leftStart, scanWindow.leftEnd);
|
direct = 0;
|
update = true;
|
} else {
|
DEBUG("匹配失败");
|
return false;
|
}
|
}
|
}
|
} else {
|
if (direct != -1) {
|
// 出场方向扫描
|
direct = 1;
|
if (scanWindow.leftStart < map->num - 1) {
|
update = true;
|
scanWindow.leftStart++;
|
}
|
if (scanWindow.rightStart < map2->num - 1) {
|
update = true;
|
scanWindow.rightStart++;
|
}
|
} else {
|
scanWindow.leftStart = tempLeft;
|
scanWindow.rightStart = tempRight;
|
}
|
}
|
}
|
|
return true;
|
}
|
|
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
|
{
|
bool update = true;
|
bool crash = false;
|
|
int direct = 0;
|
int tempLeft = zone->leftEnd;
|
int tempRight = zone->rightEnd;
|
|
Line end;
|
|
while (update) {
|
update = false;
|
MakeLine(&end, &map->point[zone->leftEnd], &map2->point[zone->rightEnd]);
|
|
if (IntersectionOf(end, tireRect) == GM_None) {
|
if (direct != -1) {
|
// 出场方向扫描
|
direct = 1;
|
tempLeft = zone->leftEnd;
|
tempRight = zone->rightEnd;
|
|
if (zone->leftEnd < map->num - 1) {
|
update = true;
|
zone->leftEnd++;
|
}
|
if (zone->rightEnd < map2->num - 1) {
|
update = true;
|
zone->rightEnd++;
|
}
|
}
|
} else {
|
if (!crash) {
|
crash = true;
|
if (mode) {
|
zone->leftStart = zone->leftEnd;
|
zone->rightStart = zone->rightEnd;
|
|
|
DEBUG("第一次接触 %d %d %d", zone->leftStart, zone->leftEnd, tempLeft);
|
}
|
}
|
|
if (direct != 1) {
|
// 入场方向扫描
|
direct = -1;
|
if (zone->leftEnd > 0) {
|
update = true;
|
zone->leftEnd--;
|
}
|
if (zone->rightEnd > 0) {
|
update = true;
|
zone->rightEnd--;
|
}
|
} else {
|
zone->leftEnd = tempLeft;
|
zone->rightEnd = tempRight;
|
}
|
}
|
}
|
|
return crash;
|
}
|
|
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
|
{
|
// 全车都需不在地图中
|
bool ret = false;
|
|
Polygon carBody;
|
Polygon bigMap;
|
|
bigMap.num = map->num + map2->num;
|
bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));
|
|
int i = 0;
|
for (; i < map->num; ++i) {
|
bigMap.point[i] = map->point[i];
|
}
|
for (int j = map2->num; j > 0; --j) {
|
bigMap.point[i++] = map2->point[j-1];
|
}
|
|
carBody.num = car->bodyNum;
|
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
|
|
for (int i = 0; i < carBody.num; ++i) {
|
carBody.point[i] = car->carXY[car->body[i]];
|
}
|
|
if (IntersectionOf(&carBody, &bigMap) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
free(bigMap.point);
|
|
return ret;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
|
{
|
bool ret = false;
|
|
Line frontTireAxial, rearTireAxial;
|
Line redLine;
|
|
MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
int s = zone->leftStart;
|
for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
|
MakeLine(&redLine, &map->point[s], &map->point[e]);
|
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
|
who = 1;
|
return true;
|
}
|
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
|
who = 1;
|
return true;
|
}
|
s = e;
|
}
|
|
s = zone->rightStart;
|
for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
|
MakeLine(&redLine, &map2->point[s], &map2->point[e]);
|
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
|
who = 2;
|
return true;
|
}
|
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
|
who = 2;
|
return true;
|
}
|
s = e;
|
}
|
|
return ret;
|
}
|
