//
|
// Created by YY on 2020/3/20.
|
//
|
|
#include <map>
|
#include <algorithm>
|
|
#include "through_something.h"
|
#include "../driver_test.h"
|
#include "../test_common/Geometry.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "../test_common/car_sensor.h"
|
#include "road_exam.h"
|
|
#define DEBUG(fmt, args...) LOGD("<through_something> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
static const double LASTEST_BREAK_POINT = 30.0;
|
static const double NEXT_ROAD_TIP = 100.0; // 到达路口前提示下一个怎么走
|
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
|
static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh
|
|
static int breakActive;
|
static int stopActive;
|
static bool crashRedLine;
|
|
static map<int, bool> breakRecord; // 记录车辆驶过各个停止线前刹车动作
|
|
void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
|
{
|
if (index == -1)
|
return;
|
DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
|
if (!RoadMapList[index].tts.empty()) {
|
DEBUG("播放TTS %s", RoadMapList[index].tts.c_str());
|
PlayTTS(RoadMapList[index].tts.c_str());
|
} else {
|
DEBUG("没有TTS");
|
}
|
breakActive = 0;
|
stopActive = 0;
|
crashRedLine = false;
|
}
|
|
void InitThroughSomething(road_exam_map &RoadMap)
|
{
|
breakRecord.clear();
|
// 为每一个刹车检测点
|
for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
|
if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
|
breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false));
|
}
|
}
|
for (int i = 0; i < RoadMap.roads.size(); ++i) {
|
breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false));
|
}
|
}
|
|
int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
|
LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
PointF p1, p2;
|
|
double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
|
RoadMapList[index].stopLine);
|
p1.X = RoadMapList[index].stopLine.X1;
|
p1.Y = RoadMapList[index].stopLine.Y1;
|
p2.X = RoadMapList[index].stopLine.X2;
|
p2.Y = RoadMapList[index].stopLine.Y2;
|
|
if (IntersectionOfLine(p1, p2, car->carXY[car->axial[AXIAL_FRONT]]) == -1) {
|
distance2StopLine = -distance2StopLine;
|
}
|
|
// 距离停止线30米内是否有刹车动作
|
if (breakActive == 0) {
|
if (distance2StopLine > 0 && distance2StopLine <= LASTEST_BREAK_POINT) {
|
if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
|
breakActive = 1;
|
}
|
}
|
|
if (distance2StopLine < 0 && breakActive != 1){
|
breakActive = -1;
|
// 不按规定减速，不合格
|
DEBUG("不踩下刹车，不合格");
|
switch (RoadMapList[index].type) {
|
case THROUGH_INTERSECTION_MAP:
|
AddExamFault(41, rtkTime);
|
break;
|
case TURN_LEFT_MAP:
|
AddExamFault(43, rtkTime);
|
break;
|
case TURN_RIGHT_MAP:
|
AddExamFault(46, rtkTime);
|
break;
|
case THROUGH_ZEBRA_CROSSING_MAP:
|
AddExamFault(48, rtkTime);
|
break;
|
case THROUGH_SCHOOL_MAP:
|
AddExamFault(49, rtkTime);
|
break;
|
case THROUGH_BUS_STATION_MAP:
|
AddExamFault(50, rtkTime);
|
break;
|
default:
|
break;
|
}
|
}
|
}
|
|
// 有停止标记的，是否停车（停止线5米内有停车动作）
|
if (RoadMapList[index].flagStop != 0 && stopActive == 0) {
|
if (distance2StopLine < 0.0) {
|
// 不停车瞭望，不合格
|
DEBUG("不停车瞭望");
|
switch (RoadMapList[index].type) {
|
case THROUGH_INTERSECTION_MAP:
|
AddExamFault(42, rtkTime);
|
break;
|
case TURN_LEFT_MAP:
|
AddExamFault(44, rtkTime);
|
break;
|
case TURN_RIGHT_MAP:
|
AddExamFault(47, rtkTime);
|
break;
|
default:
|
break;
|
}
|
stopActive = -1;
|
} else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
|
if (moveDirect == 0) {
|
stopActive = 1;
|
}
|
}
|
}
|
|
if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) {
|
if (!crashRedLine) {
|
DEBUG("不按考试员指令行驶");
|
crashRedLine = true;
|
// 不按考试员指令行驶
|
AddExamFault(3, rtkTime);
|
}
|
} else {
|
crashRedLine = false;
|
}
|
|
if (ExitSonArea(index, RoadMapList, car)) {
|
DEBUG("离开通过something区域");
|
return -1;
|
}
|
|
return index;
|
}
|
|
|
|
void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
int BreakDone = ReadCarStatus(BREAK);
|
|
// 计算车前进轨迹延长线
|
double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
|
PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw);
|
PointF extPoint2 = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
|
Line extLine, extLine2;
|
MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
|
MakeLine(&extLine2, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint2);
|
|
// 路口刹车点
|
for (int i = 0; i < map.roads.size(); ++i) {
|
// 车头和路口距离不足30米
|
if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection &&
|
IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) {
|
DEBUG("进入减速区");
|
if (BreakDone == BREAK_ACTIVE) {
|
auto itx = breakRecord.find(map.roads[i].id);
|
if (itx != breakRecord.end()) {
|
itx->second = true;
|
}
|
}
|
}
|
// 跨线后，检查刹车动作
|
if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
|
auto itx = breakRecord.find(map.roads[i].id);
|
if (itx != breakRecord.end()) {
|
if (itx->second == false) {
|
// 不按规定减速，不合格
|
DEBUG("不按规定减速");
|
|
}
|
itx->second = false;
|
}
|
}
|
}
|
// 人行道、公交站刹车点；学校限速区
|
for (int i = 0; i < map.specialAreas.size(); i++) {
|
if (map.specialAreas[i].type == GRID_AREA)
|
continue;
|
|
if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
|
// 计算点到左侧路边线的垂点
|
int road = 0;
|
for (road = 0; road < map.roads.size(); ++road) {
|
if (map.roads[road].id == map.specialAreas[i].road)
|
break;
|
}
|
|
PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
|
DEBUG("计算垂点1 (%f, %f)", vPoint.X, vPoint.Y);
|
|
map.specialAreas[i].leftPoints.push_back(vPoint);
|
|
vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
|
DEBUG("计算垂点2 (%f, %f)", vPoint.X, vPoint.Y);
|
map.specialAreas[i].leftPoints.push_back(vPoint);
|
}
|
|
if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
|
Line startLine;
|
|
MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);
|
|
// 车头和斑马线距离不足30米
|
if (IntersectionOf(extLine, startLine) == GM_Intersection &&
|
IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
|
DEBUG("进入减速区");
|
if (BreakDone == BREAK_ACTIVE) {
|
auto itx = breakRecord.find(map.specialAreas[i].id);
|
if (itx != breakRecord.end()) {
|
itx->second = true;
|
}
|
}
|
}
|
|
// 跨线后，检查刹车动作
|
if (CrashTheLine(startLine, car, CarModelList)) {
|
auto itx = breakRecord.find(map.specialAreas[i].id);
|
if (itx != breakRecord.end()) {
|
if (itx->second == false) {
|
// 不按规定减速，不合格
|
DEBUG("不按规定减速");
|
}
|
itx->second = false;
|
}
|
}
|
} else if (map.specialAreas[i].type == SCHOOL_AREA) {
|
|
}
|
}
|
}
|
