//
|
// 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"
|
#include "../utils/xconvert.h"
|
|
#define DEBUG(fmt, args...) LOGD("<road_exam through_something> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
static map <int, int> TargetReduceRec;
|
static map <int, int> TargetReduceRec2;
|
|
#define NOT_ENTER 1
|
#define ENTER_Z 2
|
#define REDUCE_SPEED 4
|
#define STOP_CAR 8
|
#define OVER_SPEED 16
|
|
static void SetTargetReduceRec(map<int, int> &table, int key, int status)
|
{
|
auto it = table.find(key);
|
|
if (it != table.end()) {
|
table.erase(it);
|
}
|
table.insert(pair<int, int>(key, status));
|
}
|
|
static int GetTargetReduceRec(map<int, int> &table, int key)
|
{
|
auto it = table.find(key);
|
|
if (it != table.end()) {
|
return it->second;
|
}
|
return NOT_ENTER;
|
}
|
|
static void RemoveTargetReduceRec(map<int, int> &table, int key)
|
{
|
auto it = table.find(key);
|
|
if (it != table.end()) {
|
table.erase(it);
|
}
|
}
|
|
void ResetTarget(road_exam_map &RoadMap)
|
{
|
TargetReduceRec.clear();
|
TargetReduceRec2.clear();
|
}
|
|
double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
vector<double> nearbyTarget; // 学校、公交站、人行道区域的距离
|
|
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
|
return 100000;
|
// 路口
|
for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
|
PointF point;
|
double distance;
|
|
point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1;
|
point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1;
|
|
distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
|
|
int key = roadIndex * 100 + i;
|
int rec = GetTargetReduceRec(TargetReduceRec, key);
|
|
if (distance > 1e-3 && distance < examParam.crossing_break_valid_distance) {
|
if (rec == NOT_ENTER) {
|
SetTargetReduceRec(TargetReduceRec, key, ENTER_Z);
|
}
|
// 记录刹车
|
if (dobreak && !(rec & REDUCE_SPEED)) {
|
DEBUG("检测到路口刹车动作");
|
SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED);
|
}
|
} else if (distance > 1e-3 && distance < examParam.crossing_stop_valid_distance) {
|
// 路口停车观察
|
if (moveDirect == 0 && !(rec & STOP_CAR)) {
|
DEBUG("检测到路口停车动作");
|
SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR);
|
}
|
} else if (distance > examParam.crossing_break_valid_distance + 5 && rec != NOT_ENTER) {
|
RemoveTargetReduceRec(TargetReduceRec, key);
|
}
|
}
|
|
if (RoadMap.calibrate == 1)
|
goto GET_DISTANCE_OF_NEARBY_TARGET;
|
|
// 人行道、公交站、学校
|
for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
|
if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
|
continue;
|
|
if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
|
double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
|
double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
|
|
int key = i;
|
int rec = GetTargetReduceRec(TargetReduceRec2, key);
|
|
if (distance1 < -1e-3 && distance2 > 1e-3) {
|
nearbyTarget.push_back(0);
|
} else if (distance1 > 1e-3 && distance2 > 1e-3) {
|
nearbyTarget.push_back(distance1);
|
}
|
|
if (distance1 > 1e-3 && distance1 < examParam.crossing_break_valid_distance) {
|
if (rec == NOT_ENTER) {
|
SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
|
}
|
// 记录刹车，停车
|
if (dobreak && !(rec & REDUCE_SPEED)) {
|
DEBUG("检测到人行道等刹车动作");
|
SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
|
}
|
} else if (distance1 > examParam.crossing_break_valid_distance + 5 && rec != NOT_ENTER) {
|
RemoveTargetReduceRec(TargetReduceRec2, key);
|
}
|
} else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
|
double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
|
double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
|
|
int key = i;
|
int rec = GetTargetReduceRec(TargetReduceRec2, key);
|
|
if (distance1 < -1e-3 && distance2 > 1e-3) {
|
nearbyTarget.push_back(0);
|
if (rec == NOT_ENTER) {
|
SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
|
}
|
if (ConvertMs2KMh(speed) > examParam.cross_school_max_speed && !(rec & OVER_SPEED)) {
|
SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);
|
|
DEBUG("通过学校区域超速 %f kmh", ConvertMs2KMh(speed));
|
AddExamFault(41101, rtkTime);
|
}
|
} else if (distance1 < -1e-3 && distance2 < -1e-3) {
|
if (rec != NOT_ENTER) {
|
RemoveTargetReduceRec(TargetReduceRec2, key);
|
}
|
} else {
|
nearbyTarget.push_back(distance1);
|
if (rec != NOT_ENTER) {
|
RemoveTargetReduceRec(TargetReduceRec2, key);
|
}
|
}
|
}
|
}
|
|
GET_DISTANCE_OF_NEARBY_TARGET:
|
if (nearbyTarget.size() > 0) {
|
sort(nearbyTarget.begin(), nearbyTarget.end());
|
return nearbyTarget[0];
|
}
|
return 100000;
|
}
|
|
void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
|
{
|
RECHECK:
|
for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) {
|
int road = it->first / 100;
|
int x = it->first % 100;
|
|
if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
|
DEBUG("越过路口 road %d 路口 %d", road, x);
|
|
CrossRoadCallback(RoadMap, road, x, ExamSchemeCrossing(RoadMap, road, x), car);
|
|
if (RoadMap.calibrate == 0) {
|
if (!(it->second & REDUCE_SPEED)) {
|
// 不按规定减速，不合格
|
DEBUG("不按规定减速");
|
|
if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
|
AddExamFault(40701, rtkTime);
|
} else if (ExamSchemeCrossing(RoadMap, road, x) ==
|
ROAD_ACTIVE_TURN_LEFT) {
|
AddExamFault(40801, rtkTime);
|
} else if (ExamSchemeCrossing(RoadMap, road, x) ==
|
ROAD_ACTIVE_TURN_RIGHT) {
|
AddExamFault(40901, rtkTime);
|
}
|
}
|
if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
|
// 不停车瞭望，不合格
|
DEBUG("不停车瞭望");
|
if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
|
AddExamFault(40701, rtkTime);
|
} else if (ExamSchemeCrossing(RoadMap, road, x) ==
|
ROAD_ACTIVE_TURN_LEFT) {
|
AddExamFault(40801, rtkTime);
|
} else if (ExamSchemeCrossing(RoadMap, road, x) ==
|
ROAD_ACTIVE_TURN_RIGHT) {
|
AddExamFault(40901, rtkTime);
|
}
|
}
|
}
|
RemoveTargetReduceRec(TargetReduceRec, it->first);
|
goto RECHECK;
|
}
|
}
|
|
if (RoadMap.calibrate == 1)
|
return;
|
|
RECHECK2:
|
for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
|
int x = it->first;
|
Line line;
|
int roadIndex = 0;
|
|
if (RoadMap.specialAreas[x].type == SCHOOL_AREA) { // 离开学校区域不按跨线判定
|
continue;
|
}
|
|
for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
|
if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
|
break;
|
}
|
|
PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
|
RoadMap.specialAreas[x].area[0]);
|
MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);
|
|
if (CrashTheLine(line, car, CarModelList)) {
|
if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
|
!(it->second & REDUCE_SPEED)) {
|
DEBUG("人行道 不按规定减速");
|
AddExamFault(41001, rtkTime);
|
}
|
if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
|
!(it->second & REDUCE_SPEED)) {
|
DEBUG("公交站 不按规定减速");
|
AddExamFault(41201, rtkTime);
|
}
|
RemoveTargetReduceRec(TargetReduceRec2, it->first);
|
goto RECHECK2;
|
}
|
}
|
}
|
