//
|
// Created by YY on 2020/3/17.
|
//
|
|
#include "road_exam.h"
|
#include "../driver_test.h"
|
#include "../utils/xconvert.h"
|
#include "../common/apptimer.h"
|
#include "../jni_log.h"
|
#include "../defs.h"
|
#include "../test_common/car_sensor.h"
|
#include "../native-lib.h"
|
#include "through_something.h"
|
#include "../master/comm_if.h"
|
#include "drive_straight.h"
|
#include "stop_car.h"
|
#include "operate_gear.h"
|
#include "../test_common/odo_graph.h"
|
#include "car_start.h"
|
#include "overtake.h"
|
#include "change_lane.h"
|
#include "prepare.h"
|
|
#include <cmath>
|
#include <vector>
|
#include <list>
|
#include <map>
|
#include <string>
|
#include <cstdlib>
|
#include <algorithm>
|
|
#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
|
enum {
|
CROSSING_NOT_HINT,
|
CROSSING_HAS_HINT,
|
};
|
|
typedef struct {
|
int road;
|
int sep;
|
int no;
|
int total;
|
int guide;
|
} lane_t;
|
|
typedef struct {
|
int edgeIndex;
|
int pointIndex;
|
PointF point;
|
} projection_t;
|
|
typedef struct {
|
int gain;
|
struct RtkTime time;
|
} change_lane_t;
|
|
typedef struct {
|
int road_index;
|
int stop_line_index;
|
int sep;
|
double distance;
|
} road_end_point_t;
|
|
static const int INVALID_ROAD = -1;
|
|
static const int CROSSING_TURN_THRESHOLD = 35;
|
static const int TURN_THRESHOLD = 3;
|
|
static bool occurOverSpeed;
|
static bool occurSecondBreak;
|
|
|
static int prevMoveDirect;
|
|
static bool StopCarOnRedArea;
|
static PointF stopPoint;
|
static bool prevGearError = false;
|
static int GearNSlideStatus = 0;
|
|
static bool slideLongDistance;
|
static bool slideNormalDistance;
|
static bool occurSlide;
|
|
static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;
|
static struct RtkTime gearChangeTimePoint;
|
static struct RtkTime gearErrorTimePoint;
|
static struct RtkTime gearNSlideTimePoint;
|
|
static int gearChange;
|
|
static car_sensor_value_t turnSignalStatus;
|
|
static int gearErrorTime;
|
|
static int currExamMapIndex;
|
|
static lane_t Lane;
|
static change_lane_t ChangeLane;
|
|
static int CrashLineType;
|
|
static map<int, car_sensor_value_t> CarSensorValue;
|
static map<int, int> CrossingHint;
|
static map<int, bool> ErrorLaneReport;
|
|
typedef struct {
|
int road_id;
|
double distance;
|
} trigger_detect_t;
|
static map<int, trigger_detect_t> TriggerDetect; // 距离本路段各触发点的距离
|
|
#define ROAD_EXAM_READY_NEXT 0
|
#define ROAD_EXAM_FREE_RUN 1
|
#define ROAD_EXAM_ITEM_CAR_START 2
|
#define ROAD_EXAM_ITEM_STRAIGHT 3
|
#define ROAD_EXAM_ITEM_OP_GEAR 4
|
#define ROAD_EXAM_ITEM_CHANGE_LANE 5
|
#define ROAD_EXAM_ITEM_OVER_TAKE 6
|
#define ROAD_EXAM_ITEM_CAR_STOP 7
|
|
#define ROAD_EXAM_ITEM_NOT_EXEC 0
|
#define ROAD_EXAM_ITEM_EXECING 1
|
#define ROAD_EXAM_ITEM_EXECED 2
|
|
|
typedef struct {
|
int name;
|
int status;
|
} RoadExamItem_t;
|
static map<int, int> RoadExamItem;
|
static int RoadExamStatus;
|
static bool win;
|
|
static struct RtkTime beginTurnTime, prevDetectTurnTime;
|
static int startTurnYaw, prevYaw;
|
static int turnCnt, turnTimeCnt;
|
static int prevTurnWise;
|
|
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
|
static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
|
static int isTurn(int currYaw, int prevYaw, int thres);
|
static void ResetTurnDetect(const car_model *car);
|
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
|
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);
|
|
static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
|
|
static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
|
|
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
|
|
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);
|
|
void InitRoadExam(road_exam_map &RoadMap)
|
{
|
DEBUG("Start road_exam");
|
|
occurOverSpeed = false;
|
|
occurSecondBreak = false;
|
|
prevMoveDirect = 0;
|
|
StopCarOnRedArea = false;
|
occurSlide = false;
|
slideLongDistance = false;
|
slideNormalDistance = false;
|
prevGearError = false;
|
gearErrorTime = 0;
|
GearNSlideStatus = 0;
|
|
currExamMapIndex = -1;
|
|
Lane.road = Lane.sep = Lane.no = -1;
|
Lane.guide = 0;
|
|
|
CrashLineType = -1;
|
turnCnt = -1;
|
|
ResetOdo();
|
ResetTarget(RoadMap);
|
// 初始化考项
|
RoadExamItem.clear();
|
RoadExamItem[ROAD_EXAM_ITEM_CAR_START] = ROAD_EXAM_ITEM_NOT_EXEC;
|
RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] = ROAD_EXAM_ITEM_NOT_EXEC;
|
RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] = ROAD_EXAM_ITEM_NOT_EXEC;
|
RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] = ROAD_EXAM_ITEM_NOT_EXEC;
|
RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] = ROAD_EXAM_ITEM_NOT_EXEC;
|
RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
|
|
RoadExamStatus = ROAD_EXAM_READY_NEXT;
|
|
TriggerDetect.clear();
|
|
win = false;
|
}
|
|
void TerminateRoadExam(void)
|
{
|
TerminateDummyLightExam();
|
TerminatePrepare();
|
}
|
|
/*********************************************************************
|
* 计算某点到道路边线的投影点
|
* @param edge
|
* @param road
|
* @param point
|
* @return
|
*/
|
PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
|
{
|
PointF p1, p2;
|
PointF proj;
|
|
vector<PointF> projArray;
|
|
Line line;
|
|
// 计算垂点
|
for (int i = 0; i < edge.size(); ++i) {
|
p1 = edge[i].points[0];
|
for (int j = 1; j < edge[i].points.size(); ++j) {
|
p2 = edge[i].points[j];
|
MakeLine(&line, &p1, &p2);
|
|
PointF vp;
|
if (VerticalPointOnLine(point, line, vp)) {
|
projArray.push_back(vp);
|
}
|
|
p1 = p2;
|
}
|
}
|
|
if (projArray.size() == 0) {
|
// 没有任何垂点，找到最近的点
|
proj = edge[0].points[0];
|
for (int i = 0; i < edge.size(); ++i) {
|
for (int j = 1; j < edge[i].points.size(); ++j) {
|
if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, proj)) {
|
proj = edge[i].points[j];
|
}
|
}
|
}
|
} else {
|
// 最近的垂点
|
proj = projArray[0];
|
for (int i = 1; i < projArray.size(); ++i) {
|
if (DistanceOf(point, projArray[i]) < DistanceOf(point, proj)) {
|
proj = projArray[i];
|
}
|
}
|
}
|
|
return proj;
|
}
|
|
static projection_t CalcProjectionWithRoadEdgeEx(vector<edge_t> &edge, PointF point)
|
{
|
PointF p1, p2;
|
Line line;
|
projection_t theProj;
|
|
vector<projection_t> projSet;
|
|
// 计算垂点
|
for (int i = 0; i < edge.size(); ++i) {
|
p1 = edge[i].points[0];
|
for (int j = 1; j < edge[i].points.size(); ++j) {
|
p2 = edge[i].points[j];
|
MakeLine(&line, &p1, &p2);
|
|
PointF vp;
|
if (VerticalPointOnLine(point, line, vp)) {
|
projection_t proj;
|
proj.edgeIndex = i;
|
proj.pointIndex = j - 1;
|
proj.point = vp;
|
projSet.push_back(proj);
|
}
|
|
p1 = p2;
|
}
|
}
|
|
if (projSet.size() == 0) {
|
// 没有任何垂点，找到最近的点
|
theProj.edgeIndex = 0;
|
theProj.pointIndex = 0;
|
theProj.point = edge[0].points[0];
|
|
for (int i = 0; i < edge.size(); ++i) {
|
for (int j = 1; j < edge[i].points.size(); ++j) {
|
if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, theProj.point)) {
|
theProj.edgeIndex = i;
|
theProj.pointIndex = j - 1;
|
theProj.point = edge[i].points[j];
|
}
|
}
|
}
|
} else {
|
// 最近的垂点
|
theProj = projSet[0];
|
for (int i = 1; i < projSet.size(); ++i) {
|
if (DistanceOf(point, projSet[i].point) < DistanceOf(point, theProj.point)) {
|
theProj = projSet[i];
|
}
|
}
|
}
|
|
return theProj;
|
}
|
|
double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
|
{
|
double distance = 0;
|
|
projection_t projection, refProjection;
|
|
projection = CalcProjectionWithRoadEdgeEx(edge, point);
|
refProjection = CalcProjectionWithRoadEdgeEx(edge, refPoint);
|
|
if (projection.edgeIndex != refProjection.edgeIndex || projection.pointIndex != refProjection.pointIndex) {
|
bool ahead = false;
|
if (projection.edgeIndex != refProjection.edgeIndex) {
|
if (projection.edgeIndex > refProjection.edgeIndex) {
|
projection_t temp;
|
temp = projection;
|
projection = refProjection;
|
refProjection = temp;
|
ahead = true;
|
}
|
int a = projection.edgeIndex;
|
int b = projection.pointIndex + 1;
|
|
if (b < edge[a].points.size()) {
|
distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
|
}
|
|
for (; a < refProjection.edgeIndex; ++a) {
|
for (; b < edge[a].points.size() - 1; ++b) {
|
distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
|
}
|
b = 0;
|
}
|
|
for (int i = 0; i < refProjection.pointIndex; i++) {
|
distance += DistanceOf(edge[a].points[i], edge[a].points[i+1]);
|
}
|
|
distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
|
} else {
|
if (projection.pointIndex > refProjection.pointIndex) {
|
projection_t temp;
|
temp = projection;
|
projection = refProjection;
|
refProjection = temp;
|
ahead = true;
|
}
|
int a = projection.edgeIndex;
|
int b = projection.pointIndex + 1;
|
|
if (b < edge[a].points.size()) {
|
distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
|
}
|
|
for (; b < refProjection.pointIndex; b++) {
|
distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
|
}
|
|
distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
|
}
|
if (ahead) {
|
distance *= -1.0;
|
}
|
} else {
|
distance = DistanceOf(projection.point, refProjection.point);
|
|
if (DistanceOf(projection.point, edge[projection.edgeIndex].points[projection.pointIndex]) >
|
DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex])) {
|
distance *= -1.0;
|
}
|
}
|
|
return distance;
|
}
|
|
/***************************************************************
|
* 车辆所在道路，根据车辆的中轴前点
|
* @param currRoadIndex, 优先检测当前道路
|
* @param RoadMap
|
* @param car
|
* @return
|
*/
|
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car)
|
{
|
int n = 0;
|
int index = currRoadIndex;
|
|
if (index < 0) {
|
index = 0;
|
}
|
|
while (n < RoadMap.roads.size()) {
|
bool changeSegment = false;
|
vector<PointF> roadOutLine;
|
Polygon area;
|
|
for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) {
|
for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) {
|
if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
|
continue;
|
}
|
changeSegment = false;
|
roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]);
|
}
|
changeSegment = true;
|
}
|
|
for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) {
|
for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) {
|
if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
|
continue;
|
}
|
changeSegment = false;
|
roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]);
|
}
|
changeSegment = true;
|
}
|
|
area.num = roadOutLine.size();
|
area.point = (PointF *) malloc(area.num * sizeof(PointF));
|
|
for (int i = 0; i < area.num; ++i) {
|
area.point[i] = roadOutLine[i];
|
}
|
|
if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
|
free(area.point);
|
break;
|
}
|
|
free(area.point);
|
|
index = (index + 1) % RoadMap.roads.size();
|
n++;
|
}
|
|
if (n >= RoadMap.roads.size()) {
|
index = -1;
|
}
|
|
return index;
|
}
|
|
/************************************************
|
* 车轮压实线，前后轴交叉，前后轮迹交叉
|
* @param currCrashLineType
|
* @param RoadMap
|
* @param car
|
* @param CarModelList
|
* @return
|
*/
|
static int CrashRedLine(int currCrashLineType, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
Line frontTireAxle, rearTireAxle;
|
Line frontLeftTireTrack, frontRightTireTrack;
|
Line rearLeftTireTrack, rearRightTireTrack;
|
bool track = false;
|
int lineType = -1;
|
PointF p11, p12, p21, p22, p31, p32, p41, p42;
|
|
MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
if (CarModelList.size() >= 2) {
|
list<car_model *>::iterator iter = CarModelList.begin();
|
p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
|
p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
|
p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
|
p41 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];
|
|
struct RtkTime time1 = ((car_model *)(*iter))->tm;
|
|
++iter;
|
|
p12 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
|
p22 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
|
p32 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
|
p42 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];
|
|
struct RtkTime time2 = ((car_model *)(*iter))->tm;
|
|
if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) {
|
MakeLine(&frontLeftTireTrack, &p11, &p12);
|
MakeLine(&frontRightTireTrack, &p21, &p22);
|
MakeLine(&rearLeftTireTrack, &p31, &p32);
|
MakeLine(&rearRightTireTrack, &p41, &p42);
|
track = true;
|
}
|
}
|
|
PointF p1, p2;
|
Line redLine;
|
|
// 左右路边实线
|
for (int n = 0; n < RoadMap.roads.size(); ++n) {
|
for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) {
|
if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) {
|
p1 = RoadMap.roads[n].leftEdge[m].points[0];
|
for (int l = 1; l < RoadMap.roads[n].leftEdge[m].points.size(); ++l) {
|
p2 = RoadMap.roads[n].leftEdge[m].points[l];
|
|
MakeLine(&redLine, &p1, &p2);
|
|
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
|
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
|
lineType = LINE_SOLID;
|
goto CRASH_LINE_CONFIRM;
|
}
|
|
if (track &&
|
(IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
|
lineType = LINE_SOLID;
|
goto CRASH_LINE_CONFIRM;
|
}
|
p1 = p2;
|
}
|
}
|
}
|
|
for (int m = 0; m < RoadMap.roads[n].rightEdge.size(); ++m) {
|
if (RoadMap.roads[n].rightEdge[m].character == LINE_SOLID && RoadMap.roads[n].rightEdge[m].points.size() >= 2) {
|
p1 = RoadMap.roads[n].rightEdge[m].points[0];
|
for (int l = 1; l < RoadMap.roads[n].rightEdge[m].points.size(); ++l) {
|
p2 = RoadMap.roads[n].rightEdge[m].points[l];
|
|
MakeLine(&redLine, &p1, &p2);
|
|
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
|
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
|
lineType = LINE_SOLID;
|
goto CRASH_LINE_CONFIRM;
|
}
|
|
if (track &&
|
(IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
|
lineType = LINE_SOLID;
|
goto CRASH_LINE_CONFIRM;
|
}
|
p1 = p2;
|
}
|
}
|
}
|
|
// 分道实线
|
// DEBUG("%d 分道段 %d", n, RoadMap.roads[n].separate.size());
|
|
for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
|
// 一组车道
|
// DEBUG("%d %d 车道数 %d", n, m, RoadMap.roads[n].separate[m].lines.size());
|
|
for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
|
// 多根分道线
|
|
// DEBUG("%d %d %d 线段树 %d", n, m, l, RoadMap.roads[n].separate[m].lines[l].size());
|
for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
|
// 一根分道线中线型相同的
|
int character = RoadMap.roads[n].separate[m].lines[l][a].character;
|
|
if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2)
|
continue;
|
|
p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
|
|
for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
|
p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
|
MakeLine(&redLine, &p1, &p2);
|
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
|
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
|
lineType = character;
|
goto CRASH_LINE_CONFIRM;
|
}
|
if (track &&
|
(IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
|
lineType = character;
|
goto CRASH_LINE_CONFIRM;
|
}
|
p1 = p2;
|
}
|
|
}
|
}
|
}
|
}
|
|
for (int n = 0; n < RoadMap.forbidLines.size(); ++n) {
|
// 地图围栏
|
if (RoadMap.forbidLines[n].type == LINE_BOUNDARY && track && RoadMap.forbidLines[n].points.size() >= 2) {
|
p1 = RoadMap.forbidLines[n].points[0];
|
for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
|
p2 = RoadMap.forbidLines[n].points[m];
|
MakeLine(&redLine, &p1, &p2);
|
if (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection) {
|
lineType = LINE_BOUNDARY;
|
goto CRASH_LINE_CONFIRM;
|
}
|
p1 = p2;
|
}
|
}
|
// 其它红线
|
if (RoadMap.forbidLines[n].type == LINE_SOLID && RoadMap.forbidLines[n].points.size() >= 2) {
|
p1 = RoadMap.forbidLines[n].points[0];
|
for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
|
p2 = RoadMap.forbidLines[n].points[m];
|
MakeLine(&redLine, &p1, &p2);
|
|
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
|
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
|
lineType = LINE_SOLID;
|
goto CRASH_LINE_CONFIRM;
|
}
|
|
if (track &&
|
(IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
|
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
|
lineType = LINE_SOLID;
|
goto CRASH_LINE_CONFIRM;
|
}
|
p1 = p2;
|
}
|
}
|
}
|
|
CRASH_LINE_CONFIRM:
|
if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
|
if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
|
// 非法跨线
|
lineType += 100;
|
}
|
} else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) {
|
if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) {
|
// 非法跨线
|
lineType += 100;
|
}
|
}
|
return lineType;
|
}
|
|
static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
|
{
|
for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) {
|
double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge);
|
double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge);
|
if (d1 < -1e-3 && d2 > 1e-3 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
|
return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo];
|
}
|
}
|
return 0;
|
}
|
|
/********************************************
|
* 计算某点在哪个车道
|
* @param point
|
* @param RoadMap
|
* @param roadIndex
|
* @return
|
*/
|
static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex)
|
{
|
Line leftProjLine, rightProjLine;
|
Line sep;
|
PointF p0, p1, p2;
|
|
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
|
return false;
|
|
p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
|
p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
|
|
MakeLine(&leftProjLine, &point, &p1);
|
MakeLine(&rightProjLine, &point, &p2);
|
|
for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
|
struct car_lane_pos {
|
int lane_no;
|
char pos; // 点在车道的左右位置
|
|
bool operator == (const int &x) {
|
return (this->lane_no == x);
|
}
|
};
|
|
vector<struct car_lane_pos> CLP;
|
bool oneMoreIntersection = false;
|
|
for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) { // 每一根线分组
|
bool intersection = false;
|
for (int k = 0; !intersection && k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) { // 同一线型的线分组
|
|
p1 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0];
|
for (int m = 1; m < RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size(); ++m) {
|
p2 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[m];
|
MakeLine(&sep, &p1, &p2);
|
|
if (IntersectionOf(sep, leftProjLine) == GM_Intersection) {
|
intersection = true;
|
struct car_lane_pos temp;
|
|
temp.lane_no = j;
|
temp.pos = 'R';
|
CLP.push_back(temp);
|
break;
|
}
|
if (IntersectionOf(sep, rightProjLine) == GM_Intersection) {
|
intersection = true;
|
struct car_lane_pos temp;
|
|
temp.lane_no = j;
|
temp.pos = 'L';
|
CLP.push_back(temp);
|
break;
|
}
|
p1 = p2;
|
}
|
}
|
if (!intersection) {
|
struct car_lane_pos temp;
|
temp.lane_no = j;
|
temp.pos = 0;
|
CLP.push_back(temp);
|
} else {
|
oneMoreIntersection = true;
|
}
|
}
|
|
if (oneMoreIntersection) {
|
// 考虑到采集偏差可能每一根线无法对齐，只要其中一根相交，其它不相交的也参与
|
for (vector<struct car_lane_pos>::iterator it = CLP.begin(); it != CLP.end(); ++it) {
|
if ((*it).pos == 0) {
|
int m = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no].size();
|
int n = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points.size();
|
|
if (DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], point) >
|
DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1], point)) {
|
// 车辆接近车道尾部
|
MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-2], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1]);
|
} else {
|
// 车辆接近车道头部
|
MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[1]);
|
}
|
|
struct car_lane_pos temp;
|
|
temp.lane_no = (*it).lane_no;
|
|
if (IntersectionOfLine(point, sep) == -1) {
|
temp.pos = 'R';
|
} else {
|
temp.pos = 'L';
|
}
|
(*it) = temp;
|
}
|
}
|
|
lane.road = roadIndex;
|
lane.sep = i;
|
lane.no = CLP.size();
|
lane.total = CLP.size() + 1;
|
|
for (int x = 0; x < CLP.size(); ++x) {
|
if (CLP[x].pos == 'L') {
|
lane.no = x;
|
break;
|
}
|
}
|
lane.guide = GetGuideDirect(RoadMap, p0, lane.road, lane.sep, lane.no);
|
return true;
|
}
|
}
|
|
lane.road = roadIndex;
|
lane.sep = 0;
|
lane.no = 0;
|
lane.total = 0;
|
lane.guide = 0;
|
|
return false;
|
}
|
|
static void WriteCarSensorValue(int name, int value, const struct RtkTime *rtkTime)
|
{
|
car_sensor_value_t nv;
|
|
nv.name = name;
|
nv.value = value;
|
nv.time = *rtkTime;
|
|
auto it = CarSensorValue.find(name);
|
|
if (it != CarSensorValue.end()) {
|
if (it->second.value != value || name == ENGINE_RPM) {
|
CarSensorValue[name] = nv;
|
}
|
} else {
|
CarSensorValue.insert(pair<int, car_sensor_value_t>(name, nv));
|
}
|
}
|
|
car_sensor_value_t ReadCarSensorValue(int name)
|
{
|
car_sensor_value_t nv = {.name = -1};
|
|
auto it = CarSensorValue.find(name);
|
|
if (it != CarSensorValue.end()) {
|
nv = it->second;
|
}
|
|
return nv;
|
}
|
|
static void UpdateCarSensor(const struct RtkTime *rtkTime)
|
{
|
WriteCarSensorValue(TURN_SIGNAL_LAMP, ReadCarStatus(TURN_SIGNAL_LAMP), rtkTime);
|
WriteCarSensorValue(HAND_BREAK, ReadCarStatus(HAND_BREAK), rtkTime);
|
WriteCarSensorValue(DOOR, ReadCarStatus(DOOR), rtkTime);
|
WriteCarSensorValue(ENGINE_RPM, ReadCarStatus(ENGINE_RPM), rtkTime);
|
WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
|
WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
|
WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
|
}
|
|
static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
static int checkCrashGreenTimeout = 0;
|
static bool crashDottedLine = false;
|
int newLineType = CrashRedLine(CrashLineType, roadIndex, RoadMap, car, CarModelList);
|
|
if (newLineType >= 100) {
|
newLineType -= 100;
|
DEBUG("非法跨越分道线");
|
AddExamFault(30119, rtkTime);
|
}
|
if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) {
|
// 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
|
DEBUG("撞道路边缘线");
|
AddExamFault(30119, rtkTime);
|
}
|
if (newLineType == LINE_BOUNDARY && CrashLineType != LINE_BOUNDARY) {
|
// 车辆越界，逃跑了，不合格
|
DEBUG("车辆越界");
|
AddExamFault(30103, rtkTime);
|
}
|
|
if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
|
if (!crashDottedLine) {
|
checkCrashGreenTimeout = 0;
|
// 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
|
crashGreenStartTime = *rtkTime;
|
turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
|
DEBUG("开始压虚线 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
|
}
|
crashDottedLine = true;
|
} else {
|
crashDottedLine = false;
|
}
|
|
if (crashDottedLine) {
|
// 压虚线
|
if (moveDirect != 0) {
|
if (checkCrashGreenTimeout == 0) {
|
checkCrashGreenTimeout = 1;
|
crashGreenRunTime = *rtkTime; // 运动中压虚线的开始时间点
|
} else if (checkCrashGreenTimeout == 1) {
|
if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= examParam.crash_dotted_line_cumulative_time) {
|
DEBUG("长时间压虚线");
|
checkCrashGreenTimeout = 2;
|
// 长时间骑轧车道分界线行驶，不合格
|
AddExamFault(30120, rtkTime);
|
}
|
}
|
} else {
|
// 停车就不计时了
|
checkCrashGreenTimeout = 0;
|
}
|
} else {
|
// 不再压虚线
|
checkCrashGreenTimeout = 0;
|
}
|
|
CrashLineType = newLineType;
|
}
|
|
int DetectLane(road_exam_map &RoadMap, const car_model *car, int roadIndex, const struct RtkTime *rtkTime)
|
{
|
lane_t newLane;
|
int gain = 0;
|
|
if (CrashLineType == LINE_DOTTED || CrashLineType == LINE_HALF_SOLID_LEFT || CrashLineType == LINE_HALF_SOLID_RIGHT) {
|
// 排除跨线中
|
return 0;
|
}
|
if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) {
|
if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
|
gain = newLane.no - Lane.no;
|
} else {
|
DEBUG("车道分段切换 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no);
|
ChangeLane.gain = 0;
|
}
|
|
// 检查转向灯
|
if (gain != 0) {
|
DEBUG("变道 new lane %d, gain %d", newLane.no, gain);
|
if (gain < 0) {
|
if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(30121, rtkTime);
|
} else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
|
examParam.turn_signal_min_advance) {
|
DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
|
crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
|
crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
|
crashGreenStartTime.mss * 10,
|
turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
|
turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
|
|
// 不足3秒，不合格
|
AddExamFault(30122, rtkTime);
|
}
|
} else {
|
if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(30121, rtkTime);
|
} else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
|
examParam.turn_signal_min_advance) {
|
DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
|
crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
|
crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
|
crashGreenStartTime.mss * 10,
|
turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
|
turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
|
// 不足3秒，不合格
|
AddExamFault(30122, rtkTime);
|
}
|
}
|
|
if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < examParam.continuous_change_lane_min_time) {
|
DEBUG("连续变道");
|
AddExamFault(30125, rtkTime);
|
}
|
ChangeLane.gain = gain;
|
ChangeLane.time = *rtkTime;
|
}
|
|
Lane = newLane;
|
}
|
return gain;
|
}
|
|
static void ChangeCrossingStatus(int roadIndex, int index, int status)
|
{
|
int key = roadIndex * 100 + index;
|
|
auto it = CrossingHint.find(key);
|
|
if (it != CrossingHint.end()) {
|
CrossingHint.erase(it);
|
}
|
CrossingHint.insert(pair<int, int>(key, status));
|
}
|
|
static int GetCrossingStatus(int roadIndex, int index)
|
{
|
int key = roadIndex * 100 + index;
|
|
auto it = CrossingHint.find(key);
|
|
if (it != CrossingHint.end()) {
|
return it->second;
|
}
|
return CROSSING_NOT_HINT;
|
}
|
|
/*****************************************************
|
* 以正常通过路口或自由驾驶离开路段后
|
* @param roadIndex
|
*/
|
static void ResetCrossingStatus(road_exam_map &RoadMap, int roadIndex)
|
{
|
while (true) {
|
bool w = false;
|
for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
|
if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
|
ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
|
CrossingChange(RoadMap.roads[roadIndex].id, it->first % 100, 0);
|
w = true;
|
break;
|
}
|
}
|
if (!w)
|
break;
|
}
|
}
|
|
static void SetErrorLaneRpt(int roadIndex, int index, bool status)
|
{
|
int key = roadIndex * 100 + index;
|
|
auto it = ErrorLaneReport.find(key);
|
|
if (it != ErrorLaneReport.end()) {
|
ErrorLaneReport.erase(it);
|
}
|
ErrorLaneReport.insert(pair<int, bool>(key, status));
|
}
|
|
static bool GetErrorLaneRpt(int roadIndex, int index)
|
{
|
int key = roadIndex * 100 + index;
|
|
auto it = ErrorLaneReport.find(key);
|
|
if (it != ErrorLaneReport.end()) {
|
return it->second;
|
}
|
return false;
|
}
|
|
static void ResetErrorLaneRpt(int roadIndex)
|
{
|
while (true) {
|
bool w = false;
|
for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
|
if (it->first / 100 == roadIndex && it->second == true) {
|
SetErrorLaneRpt(it->first / 100, it->first % 100, false);
|
w = true;
|
break;
|
}
|
}
|
if (!w)
|
break;
|
}
|
}
|
|
/*********************************************************
|
* 计算当前所在分道线的剩余长度
|
* @param RoadMap
|
* @param lane
|
* @param car
|
* @return
|
*/
|
static double SeparateLength(road_exam_map &RoadMap, lane_t lane, const car_model *car)
|
{
|
double distance = 0;
|
|
if (lane.road < 0 || lane.total == 0 || lane.sep < 0)
|
return distance;
|
|
int n = RoadMap.roads[lane.road].separate[lane.sep].lines[0].size();
|
int m = RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points.size();
|
|
return CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]],
|
RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points[m-1],
|
RoadMap.roads[lane.road].rightEdge);
|
}
|
|
/***********************************************************
|
* 前向最近的路口停止线或导向线起点，一定距离内做出路口提示，或项目自动触发
|
* @param roadIndex
|
* @param RoadMap
|
* @param car
|
*/
|
static road_end_point_t NearbyRoadEndPoint(int roadIndex, road_exam_map &RoadMap, const car_model *car)
|
{
|
road_end_point_t rec;
|
|
rec.road_index = roadIndex;
|
rec.stop_line_index = 0;
|
rec.distance = 0;
|
|
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
|
return rec;
|
|
// 最近的路口距离
|
int stop_line_index;
|
PointF nearbyStopPoint;
|
|
NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
|
nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
|
nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;
|
|
double dis1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], nearbyStopPoint, RoadMap.roads[roadIndex].rightEdge);
|
|
struct ep_ {
|
int sep;
|
double distance;
|
} ep;
|
|
vector<struct ep_> dset;
|
|
ep.sep = -1;
|
ep.distance = dis1;
|
dset.push_back(ep);
|
|
// 最近的分道线距离，且不超过最近路口
|
for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
|
for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {
|
for (int k = 0; k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {
|
if (RoadMap.roads[roadIndex].separate[i].lines[j][k].character != LINE_DOTTED &&
|
RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size() > 0) {
|
double dis2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0], RoadMap.roads[roadIndex].rightEdge);
|
|
if (dis2 < -1e-3) {
|
continue;
|
}else if (dis2 < dis1) {
|
// 找出最短的点
|
ep.sep = i;
|
ep.distance = dis2;
|
dset.push_back(ep);
|
continue;
|
}
|
goto FIND_END_POINT;
|
}
|
}
|
}
|
}
|
|
FIND_END_POINT:
|
sort(dset.begin(), dset.end(), [=](struct ep_ x, struct ep_ y)->bool {return x.distance < y.distance;});
|
|
rec.stop_line_index = stop_line_index;
|
rec.distance = dset[0].distance;
|
rec.sep = dset[0].sep;
|
|
return rec;
|
}
|
|
int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
|
{
|
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
|
return -1;
|
}
|
for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
|
if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
|
RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
|
return RoadMap.examScheme[0].crossingActive[i].active;
|
}
|
}
|
return -1;
|
}
|
|
static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
|
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
|
return;
|
|
if (distance > 5 && distance < 70) {
|
// 提示路口怎么走
|
if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
|
// 项目标定模式下，这里提示标定本路口动作
|
if (RoadMap.calibrate) {
|
CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
|
} else {
|
int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);
|
|
switch (act) {
|
case ROAD_ACTIVE_FORWARD:
|
DEBUG("路口提示 直行");
|
PlayTTS(examParam.crossing_go_straight_tts, NULL);
|
break;
|
case ROAD_ACTIVE_TURN_LEFT:
|
DEBUG("路口提示 左转");
|
PlayTTS(examParam.crossing_turn_left_tts, NULL);
|
break;
|
case ROAD_ACTIVE_TURN_RIGHT:
|
DEBUG("路口提示 右转");
|
PlayTTS(examParam.crossing_turn_right_tts, NULL);
|
break;
|
case ROAD_ACTIVE_TURN_BACKWARD:
|
DEBUG("路口提示 掉头");
|
PlayTTS(examParam.crossing_turn_back_tts, NULL);
|
break;
|
default:
|
DEBUG("路口提示 未配置");
|
PlayTTS(examParam.crossing_turn_unknown_tts, NULL);
|
break;
|
}
|
}
|
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
|
}
|
} else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
|
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);
|
|
if (RoadMap.calibrate) {
|
CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
|
}
|
}
|
}
|
|
static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
|
{
|
int guide = 0, stopLine = 0;
|
double distance;
|
|
struct nearby_crossing {
|
int stopLine;
|
int guide;
|
double distance;
|
};
|
|
vector<struct nearby_crossing> set;
|
|
for (int i = 0; i < road.stopLine.size(); ++i) {
|
PointF point;
|
|
point.X = road.stopLine[i].line.X1;
|
point.Y = road.stopLine[i].line.Y1;
|
|
distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point,
|
road.rightEdge);
|
|
if (distance > 1e-3) {
|
struct nearby_crossing temp;
|
|
temp.stopLine = i;
|
temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
|
temp.distance = distance;
|
|
set.push_back(temp);
|
}
|
}
|
|
if (set.size() > 0) {
|
distance = set[0].distance;
|
guide = set[0].guide;
|
|
for (int i = 1; i < set.size(); ++i) {
|
if (distance > set[i].distance) {
|
distance = set[i].distance;
|
guide = set[i].guide;
|
stopLine = set[i].stopLine;
|
}
|
}
|
}
|
|
stopLineIndex = stopLine;
|
return guide;
|
}
|
|
static void RingBreak(void)
|
{
|
static int oldValue = BREAK_INACTIVE;
|
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
|
|
if (brk.name == BREAK) {
|
if (brk.value == BREAK_ACTIVE && oldValue != BREAK_ACTIVE) {
|
PlayRing();
|
}
|
oldValue = brk.value;
|
}
|
}
|
|
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0;
|
int forward = 0; // 车辆相对当前道路的
|
UpdateCarSensor(rtkTime);
|
|
UpdataOdo(speed, moveDirect, rtkTime);
|
|
if (!win) {
|
win = true;
|
turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
|
}
|
|
if (RoadMap.calibrate == 0) {
|
// 刹车提示音
|
RingBreak();
|
|
// 超速检测
|
if (ConvertMs2KMh(speed) > examParam.road_max_speed) {
|
if (!occurOverSpeed) {
|
occurOverSpeed = true;
|
// 超速，不合格
|
DEBUG("超速 %f", ConvertMs2KMh(speed));
|
AddExamFault(30118, rtkTime);
|
}
|
} else if (ConvertMs2KMh(speed) < examParam.road_max_speed - 5) {
|
occurOverSpeed = false;
|
}
|
|
// 副刹车检测
|
if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
|
// 副刹车踩下，不合格
|
if (!occurSecondBreak) {
|
DEBUG("副刹车动作了");
|
occurSecondBreak = true;
|
AddExamFault(30132, rtkTime);
|
}
|
} else {
|
occurSecondBreak = false;
|
}
|
|
// 两次换挡不进
|
static int prevGear = -1;
|
|
int currGear = ReadCarStatus(GEAR);
|
if (currGear != GEAR_N && prevGear == GEAR_N) {
|
// 一次换挡
|
if (gearChange == currGear && TimeGetDiff(rtkTime, &gearChangeTimePoint) < D_SEC(5)) {
|
AddExamFault(30113, rtkTime);
|
}
|
|
gearChange = currGear;
|
gearChangeTimePoint = *rtkTime;
|
}
|
prevGear = currGear;
|
|
// 挡位匹配检测
|
bool currGearError = false;
|
bool currGearNSlide = false;
|
|
switch (ReadCarStatus(GEAR)) {
|
case GEAR_N:
|
if (moveDirect != 0) {
|
// 空档滑行
|
currGearNSlide = true;
|
}
|
break;
|
case GEAR_1:
|
if (ConvertMs2KMh(speed) < examParam.gear_speed_table[0][0] ||
|
ConvertMs2KMh(speed) > examParam.gear_speed_table[0][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_2:
|
if (ConvertMs2KMh(speed) < examParam.gear_speed_table[1][0] ||
|
ConvertMs2KMh(speed) > examParam.gear_speed_table[1][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_3:
|
if (ConvertMs2KMh(speed) < examParam.gear_speed_table[2][0] ||
|
ConvertMs2KMh(speed) > examParam.gear_speed_table[2][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_4:
|
if (ConvertMs2KMh(speed) < examParam.gear_speed_table[3][0] ||
|
ConvertMs2KMh(speed) > examParam.gear_speed_table[3][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_5:
|
if (ConvertMs2KMh(speed) < examParam.gear_speed_table[4][0] ||
|
ConvertMs2KMh(speed) > examParam.gear_speed_table[4][1]) {
|
currGearError = true;
|
}
|
break;
|
default:
|
break;
|
}
|
// 空档滑行超时
|
if (currGearNSlide) {
|
if (GearNSlideStatus == 0) {
|
DEBUG("检测到空挡滑行");
|
GearNSlideStatus = 1;
|
gearNSlideTimePoint = *rtkTime;
|
}
|
if (GearNSlideStatus == 1 &&
|
TimeGetDiff(rtkTime, &gearNSlideTimePoint) > examParam.gear_n_allow_time) {
|
// 空档滑行超5秒，不合格
|
DEBUG("挡位滑行，超过5秒");
|
AddExamFault(30114, rtkTime);
|
GearNSlideStatus = 2;
|
}
|
} else if (GearNSlideStatus != 0) {
|
GearNSlideStatus = 0;
|
DEBUG("空挡滑行结束");
|
}
|
|
// 挡位不匹配超时
|
if (currGearError && prevGearError) {
|
DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
|
ReadCarStatus(GEAR), ConvertMs2KMh(speed));
|
gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
|
}
|
if (gearErrorTime > examParam.gear_speed_error_cumulative_time) {
|
// 累计15秒，挡位-车速不匹配，不合格
|
DEBUG("挡位错误超过15秒");
|
AddExamFault(30112, rtkTime);
|
gearErrorTime = 0;
|
}
|
|
prevGearError = currGearError;
|
if (prevGearError) {
|
gearErrorTimePoint = *rtkTime;
|
}
|
|
// 起步后滑
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = *rtkTime;
|
StopCarOnRedArea = false;
|
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD,
|
rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
|
if (slideNormalDistance) {
|
// 后滑，扣10分
|
AddExamFault(30202, rtkTime);
|
DEBUG("后滑超过10厘米, 但不超过30厘米");
|
}
|
|
slideNormalDistance = false;
|
slideLongDistance = false;
|
occurSlide = false;
|
} else if (moveDirect == -1 && prevMoveDirect == 0) {
|
DEBUG("开始后滑");
|
stopPoint = car->basePoint;
|
occurSlide = true;
|
}
|
prevMoveDirect = moveDirect;
|
} else if (moveDirect == 0) {
|
// 持续停车
|
if (TimeGetDiff(rtkTime, &stopTimepoint) >= CorrectPauseCriteria(examParam.road_pause_criteria) && !StopCarOnRedArea &&
|
StopOnRedArea(RoadMap, car)) {
|
// 停车超2秒，停在红区，不合格
|
AddExamFault(30128, rtkTime);
|
DEBUG("禁停区停车");
|
StopCarOnRedArea = true;
|
}
|
} else if (moveDirect == -1) {
|
// 持续后滑
|
if (occurSlide) {
|
double slideDistance = DistanceOf(stopPoint, car->basePoint);
|
|
if (slideDistance > examParam.road_slide_yellow_distance) {
|
slideNormalDistance = true;
|
}
|
|
if (slideDistance > examParam.road_slide_red_distance && !slideLongDistance) {
|
// 后滑超过30厘米, 不合格
|
AddExamFault(30107, rtkTime);
|
DEBUG("后滑超过30厘米");
|
slideLongDistance = true;
|
slideNormalDistance = false;
|
occurSlide = false;
|
}
|
}
|
} else {
|
// 前进
|
}
|
}
|
|
// 检测离开此路段，全车需不在范围内
|
int oldid = currExamMapIndex;
|
currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
|
if (oldid != currExamMapIndex) {
|
DEBUG("道路ID切换 %d ======> %d", oldid, currExamMapIndex);
|
|
if (oldid >= 0) {
|
ResetCrossingStatus(RoadMap, oldid);
|
ResetErrorLaneRpt(oldid);
|
}
|
|
if (RoadMap.calibrate) {
|
if (currExamMapIndex < 0 && oldid >= 0) {
|
RoadChange(RoadMap.roads[oldid].id, 0);
|
} else if (currExamMapIndex >= 0) {
|
RoadChange(RoadMap.roads[currExamMapIndex].id, 1);
|
}
|
}
|
Lane.guide = 0;
|
}
|
|
if (currExamMapIndex >= 0) {
|
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
|
// 检测通过路口、人行道等区域时，释放刹车或减速
|
TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
|
// 检查
|
projection_t projection;
|
|
uint32_t tm1 = AppTimer_GetTickCount();
|
|
projection = CalcProjectionWithRoadEdgeEx(RoadMap.roads[currExamMapIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
|
|
if (projection.edgeIndex < RoadMap.roads[currExamMapIndex].rightEdge.size()) {
|
if (projection.pointIndex < RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points.size() - 1) {
|
double roadYaw = YawOf(RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex + 1],
|
RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex]);
|
|
int deltaAng;
|
|
if (ABS((int)roadYaw - (int)car->yaw) > 180) {
|
deltaAng = 360 - ABS((int)roadYaw - (int)car->yaw);
|
} else {
|
deltaAng = ABS((int)roadYaw - (int)car->yaw);
|
}
|
|
if (deltaAng < 90) {
|
forward = 1;
|
} else {
|
forward = -1;
|
}
|
// DEBUG("路段方向 %f 车辆方向 %f 差值 %d 耗时 %ld", roadYaw, car->yaw, deltaAng, AppTimer_GetTickCount() - tm1);
|
}
|
}
|
}
|
|
ExitTarget(RoadMap, car, CarModelList, rtkTime);
|
|
if (RoadMap.calibrate == 0) {
|
oldid = CrashLineType;
|
// 检测压线状态
|
DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
|
|
if (oldid != CrashLineType) {
|
DEBUG("压线类型切换 %d", CrashLineType);
|
}
|
|
oldid = Lane.guide;
|
DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
|
|
// DEBUG("Lane信息 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
|
if (Lane.guide > 0 && currExamMapIndex >= 0) {
|
int stop_line_index;
|
int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
|
RoadMap.roads[currExamMapIndex], car);
|
|
if (act != 0 && !(act & Lane.guide)) {
|
if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
|
DEBUG("不按规定车道标向行驶 %d: %d 期望 = %d guide = %d", currExamMapIndex,
|
stop_line_index, act, Lane.guide);
|
AddExamFault(30117, rtkTime);
|
SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
|
}
|
}
|
}
|
|
if (Lane.guide != oldid) {
|
DEBUG("导向类型切换 %d", Lane.guide);
|
}
|
}
|
|
if (currExamMapIndex >= 0 && Lane.guide == 0) {
|
BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
|
|
road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
|
// 提示路口怎么走
|
if (forward == 1) {
|
HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);
|
}
|
|
double freeSepDis = SeparateLength(RoadMap, Lane, car);
|
|
// 剩余距离就是导向线起点的距离
|
if (Lane.sep >= 0 && Lane.sep == ep.sep) {
|
freeSepDis = ep.distance;
|
}
|
|
RoadCrossingFree = freeSepDis;
|
|
// DEBUG("车速 %f, 累计行驶距离 %f, 直道剩余距离 %f, 车道剩余距离 %f 特殊区域距离 %f", ConvertMs2KMh(speed), ReadOdo(), BigStraightRoadFree, RoadCrossingFree, TargetFree);
|
}
|
|
// 额外的转向检测
|
|
if (RoadMap.calibrate == 0) {
|
DetectTurn(car, moveDirect, rtkTime);
|
|
ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
|
BigStraightRoadFree,
|
TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
|
ItemExam2(RoadMap, currExamMapIndex, car, CarModelList, forward, moveDirect);
|
}
|
}
|
|
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
|
{
|
static double freeRunDistance;
|
static double totalRunDistance;
|
static double freeRunExceptDistance = 50.0;
|
|
if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
|
if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
|
CarStartInit();
|
totalRunDistance = ReadOdo();
|
RoadExamStatus = ROAD_EXAM_ITEM_CAR_START;
|
} else {
|
bool not_complete = false;
|
|
if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
|
not_complete = true;
|
// if (straight > 170 && road_end > 170) {
|
// StartDriveStraightExam();
|
// RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
|
// return;
|
// }
|
}
|
if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
|
not_complete = true;
|
// if (road_end > 170) {
|
// StartOperateGearExam();
|
// RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
|
// return;
|
// }
|
}
|
if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
|
not_complete = true;
|
// if (road_end > 150 && Lane.total > 1) {
|
// StartChaneLaneExam(Lane.no);
|
// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
|
// return;
|
// }
|
}
|
if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
|
not_complete = true;
|
// if (road_end > 200 && Lane.total > 1) {
|
// if (Lane.no == 0) {
|
// // 已在最左车道，先变更车道？
|
// StartChaneLaneExam(Lane.no);
|
// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
|
// } else {
|
// StartOvertakeExam(Lane.no);
|
// RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
|
// }
|
// return;
|
// }
|
}
|
|
if (!not_complete) {
|
if (road_end > 200 && ReadOdo() > examParam.road_total_distance) {
|
RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
|
StartStopCarExam();
|
return;
|
}
|
}
|
}
|
} else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
|
if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
|
DEBUG("寻找下一个子项目");
|
RoadExamStatus = ROAD_EXAM_READY_NEXT;
|
}
|
} else {
|
bool testing = false;
|
|
switch (RoadExamStatus) {
|
case ROAD_EXAM_ITEM_CAR_START:
|
testing = TestCarStart(car, speed, moveDirect, rtkTime);
|
break;
|
case ROAD_EXAM_ITEM_OP_GEAR:
|
testing = TestOperateGear(rtkTime);
|
break;
|
case ROAD_EXAM_ITEM_CHANGE_LANE:
|
testing = TestChangeLane(Lane.no, rtkTime);
|
break;
|
case ROAD_EXAM_ITEM_OVER_TAKE:
|
testing = TestOvertake(Lane.no, rtkTime);
|
break;
|
case ROAD_EXAM_ITEM_STRAIGHT:
|
testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime);
|
break;
|
case ROAD_EXAM_ITEM_CAR_STOP:
|
testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
|
break;
|
default:break;
|
}
|
|
if (!testing) {
|
DEBUG("当前子项结束");
|
RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
|
if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
|
freeRunExceptDistance = 60.0;
|
} else if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_STOP) {
|
// 考试结束
|
MA_SendExamStatus(0, 0);
|
PlayTTS("考试结束", NULL);
|
} else {
|
freeRunExceptDistance = 2;//250.0;
|
}
|
|
RoadExamStatus = ROAD_EXAM_FREE_RUN;
|
freeRunDistance = ReadOdo();
|
}
|
}
|
}
|
|
static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
|
{
|
int item = 0;
|
|
if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
|
item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect);
|
}
|
|
if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
|
StartDriveStraightExam();
|
RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
|
} else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
|
StartOperateGearExam();
|
RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
|
} else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 1) {
|
StartChaneLaneExam(Lane.no);
|
RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
|
} else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 2) {
|
StartOvertakeExam(Lane.no);
|
RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
|
} else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 5) {
|
if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_EXECED &&
|
RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_EXECED &&
|
RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_EXECED &&
|
RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_EXECED) {
|
StartStopCarExam();
|
RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
|
}
|
}
|
}
|
|
/*******************************************************************
|
* 车辆越过停止线的方式通过路口
|
* @param RoadMap
|
* @param road
|
* @param stop_line
|
* @param active
|
* @param car
|
*/
|
void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
|
{
|
if (RoadMap.calibrate) {
|
CrossingChange(RoadMap.roads[road].id, stop_line, 0);
|
}
|
SetErrorLaneRpt(road, stop_line, false);
|
if (active != ROAD_ACTIVE_FORWARD) {
|
ResetTurnDetect(car);
|
}
|
}
|
|
static int isTurn(int currYaw, int prevYaw, int thres)
|
{
|
// DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
|
int deltaAng = 0;
|
|
if (ABS(currYaw - prevYaw) > 180) {
|
deltaAng = 360 - ABS(currYaw-prevYaw);
|
} else {
|
deltaAng = ABS(currYaw - prevYaw);
|
}
|
|
|
// DEBUG("角度差值 %d", deltaAng);
|
|
if (deltaAng >= thres) {
|
if((( currYaw + 360 - prevYaw) % 360) < 180) {
|
// DEBUG("右转");
|
return deltaAng;
|
} else {
|
// DEBUG("左转");
|
return 0 - deltaAng;
|
}
|
}
|
|
return 0;
|
}
|
|
static void ResetTurnDetect(const car_model *car)
|
{
|
turnCnt = 0;
|
turnTimeCnt = 0;
|
prevDetectTurnTime = car->tm;
|
prevYaw = car->yaw;
|
}
|
|
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
int angle;
|
|
if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
|
return;
|
}
|
|
angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);
|
|
prevYaw = (int) car->yaw;
|
prevDetectTurnTime = *rtkTime;
|
|
if (turnCnt == 0) {
|
if (angle != 0 && moveDirect != 0) {
|
DEBUG("有转向迹象");
|
turnCnt++;
|
beginTurnTime = *rtkTime;
|
startTurnYaw = (int) car->yaw;
|
|
turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
|
}
|
} else if (turnCnt == 1) {
|
if (angle != 0 && moveDirect != 0) {
|
if (angle * prevTurnWise > 0) {
|
DEBUG("确认转向");
|
// 同向转动
|
turnCnt++;
|
turnTimeCnt = 0;
|
} else {
|
beginTurnTime = *rtkTime;
|
startTurnYaw = (int) car->yaw;
|
|
turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
|
}
|
} else {
|
turnCnt = 0;
|
}
|
} else if (turnCnt >= 2) {
|
if (moveDirect == 0) {
|
// 暂停
|
} else {
|
turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
|
int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
|
DEBUG("转动角度 %d", wise);
|
if (ABS(wise) >= CROSSING_TURN_THRESHOLD) {
|
// 确认转弯行为，检测开始刚转弯时转向灯情况
|
turnCnt = -1;
|
|
if (wise < 0) {
|
if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(30121, rtkTime);
|
} else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
|
examParam.turn_signal_min_advance) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
AddExamFault(30122, rtkTime);
|
}
|
} else {
|
if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(30121, rtkTime);
|
} else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
|
examParam.turn_signal_min_advance) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
AddExamFault(30122, rtkTime);
|
}
|
}
|
} else if (turnTimeCnt > D_SEC(10)) {
|
// 取消转向检测
|
DEBUG("取消转向检测1");
|
turnCnt = -1;
|
}
|
}
|
}
|
|
if (turnCnt < 2 && moveDirect != 0) {
|
turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
|
if (turnTimeCnt > D_SEC(15)) {
|
// 取消转向检测
|
DEBUG("取消转向检测2");
|
turnCnt = -1;
|
}
|
}
|
|
prevTurnWise = angle;
|
}
|
|
/**********************************************************
|
* 按整个车身是否覆盖计算
|
* @param RoadMap
|
* @param car
|
* @return
|
*/
|
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car)
|
{
|
for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
|
Polygon redArea;
|
redArea.num = 0;
|
|
if (RoadMap.specialAreas[i].type == GRID_AREA && RoadMap.specialAreas[i].area.size() == 4) {
|
redArea.num = 4;
|
redArea.point = (PointF *)malloc(4 * sizeof(PointF));
|
|
redArea.point[0] = RoadMap.specialAreas[i].area[0];
|
redArea.point[1] = RoadMap.specialAreas[i].area[1];
|
redArea.point[2] = RoadMap.specialAreas[i].area[2];
|
redArea.point[3] = RoadMap.specialAreas[i].area[3];
|
} else if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING) {
|
int j = 0;
|
for (; j < RoadMap.roads.size(); ++j) {
|
if (RoadMap.specialAreas[i].road == RoadMap.roads[j].id) {
|
break;
|
}
|
}
|
|
if (j < RoadMap.roads.size()) {
|
|
PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
|
RoadMap.specialAreas[i].area[0]);
|
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
|
RoadMap.specialAreas[i].area[1]);
|
|
redArea.num = 4;
|
redArea.point = (PointF *) malloc(4 * sizeof(PointF));
|
|
redArea.point[0] = RoadMap.specialAreas[i].area[0];
|
redArea.point[1] = RoadMap.specialAreas[i].area[1];
|
redArea.point[2] = p2;
|
redArea.point[3] = p1;
|
}
|
}
|
|
if (redArea.num > 0) {
|
for (int i = 0; i < car->bodyNum; ++i) {
|
if (IntersectionOf(car->carXY[car->body[i]], &redArea) == GM_Containment) {
|
free(redArea.point);
|
return true;
|
}
|
}
|
free(redArea.point);
|
}
|
}
|
|
return false;
|
}
|
|
bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
if (CarModelList.size() < 2)
|
return false;
|
|
list<car_model *>::iterator iter = CarModelList.begin();
|
|
Line trace;
|
PointF p1, p2;
|
|
p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
|
++iter;
|
p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
|
MakeLine(&trace, &p1, &p2);
|
|
if (IntersectionOf(trace, line) == GM_Intersection &&
|
IntersectionOfLine(p1, line) == -1) {
|
return true;
|
}
|
return false;
|
}
|
|
static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
|
{
|
if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0 || CarModelList.size() < 2)
|
return -1;
|
|
list<car_model *>::iterator iter = CarModelList.begin();
|
|
Line trace;
|
PointF p1, p2;
|
|
p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
|
++iter;
|
p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
|
MakeLine(&trace, &p1, &p2);
|
|
double trace_length = DistanceOf(p1, p2);
|
|
if (TriggerDetect.size() > 0) {
|
auto it = TriggerDetect.begin();
|
|
if (it->second.road_id != RoadMap.roads[index].id) {
|
TriggerDetect.clear();
|
}
|
}
|
|
// DEBUG("检查触发点 道 %d 路 %d", RoadMap.examScheme[0].triggerLines[0].road, RoadMap.roads[index].id);
|
|
for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
|
if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
|
|
|
/*Line triggerLine;
|
|
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
|
|
MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &p2);
|
|
if (CrashTheLine(triggerLine, car, CarModelList)) {
|
DEBUG("触发项目 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
|
return RoadMap.triggerLines[i].active;
|
}*/
|
|
/* vector<double> vec;
|
|
PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
|
car->carXY[car->axial[AXIAL_FRONT]]);
|
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
|
RoadMap.examScheme[0].triggerLines[j].points[0]);
|
|
vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
|
RoadMap.examScheme[0].triggerLines[j].points[0]));
|
vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
|
vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));
|
|
sort(vec.begin(), vec.end());
|
|
if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
|
DEBUG("触发项目 %d", RoadMap.examScheme[0].triggerLines[j].active);
|
return RoadMap.examScheme[0].triggerLines[j].active;
|
}*/
|
|
// double angleOf = DeltaYaw(YawOf(RoadMap.examScheme[0].triggerLines[j].points[0], car->carXY[car->axial[AXIAL_FRONT]]), car->yaw);
|
// auto it = TriggerDetect.find(j);
|
//
|
// if (it != TriggerDetect.end()) {
|
// if (fabs(angleOf - it->second.angleOf) > 2.0) {
|
// bool trigger = false;
|
//
|
// trigger_detect_t up;
|
//
|
// if (moveDirect == 1 && forward == 1 && it->second.angleOf <= 90 && angleOf > 90) {
|
// trigger = true;
|
// }
|
// DEBUG("观察子<%d> 更新 %f -> %f", j, it->second.angleOf, angleOf);
|
//
|
// up.angleOf = angleOf;
|
// up.road_id = RoadMap.roads[index].id;
|
//
|
// TriggerDetect[j] = up;
|
//
|
//
|
//
|
// if (trigger) {
|
// DEBUG("pos %d 触发项目 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
|
// return RoadMap.examScheme[0].triggerLines[j].active;
|
// }
|
// }
|
// } else {
|
// trigger_detect_t up;
|
//
|
// up.road_id = RoadMap.roads[index].id;
|
// up.angleOf = angleOf;
|
//
|
// TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
|
//
|
// DEBUG("观察子<%d> 加入 %f", j, angleOf);
|
// }
|
|
// double dist = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
|
// RoadMap.examScheme[0].triggerLines[j].points[0]);
|
auto it = TriggerDetect.find(j);
|
|
if (it != TriggerDetect.end()) {
|
PointF verticalPoint;
|
|
if (moveDirect == 1 &&
|
forward == 1 &&
|
trace_length > 0.01 &&
|
VerticalPointOnLine(RoadMap.examScheme[0].triggerLines[j].points[0], trace, verticalPoint) &&
|
DistanceOf(verticalPoint, RoadMap.examScheme[0].triggerLines[j].points[0]) < it->second.distance) {
|
DEBUG("pos %d 触发项目 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
|
|
DEBUG("(%f, %f) ON (%f, %f)-(%f, %f) IS (%f, %f) trace %f",
|
RoadMap.examScheme[0].triggerLines[j].points[0].X,
|
RoadMap.examScheme[0].triggerLines[j].points[0].Y,
|
trace.X1,
|
trace.Y1,
|
trace.X2,
|
trace.Y2,
|
verticalPoint.X,
|
verticalPoint.Y,
|
trace_length);
|
|
return RoadMap.examScheme[0].triggerLines[j].active;
|
}
|
} else {
|
PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
|
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
|
|
double dist1 = DistanceOf(p1, RoadMap.examScheme[0].triggerLines[j].points[0]);
|
double dist2 = DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]);
|
|
trigger_detect_t up;
|
|
up.road_id = RoadMap.roads[index].id;
|
up.distance = MAX(dist1, dist2);
|
|
TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
|
|
DEBUG("观察子<%d> road_id %d 加入 (%f,%f)", j, RoadMap.roads[index].id, RoadMap.examScheme[0].triggerLines[j].points[0].X, RoadMap.examScheme[0].triggerLines[j].points[0].Y);
|
}
|
}
|
}
|
|
return -1;
|
}
|
|
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
|
{
|
double distance = 0;
|
|
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
|
return 0;
|
|
for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) {
|
for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) {
|
PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j];
|
distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].leftEdge);
|
if (distance > 1e-3) {
|
return distance;
|
}
|
}
|
}
|
|
return 0;
|
}
|
