//
|
// 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 <cmath>
|
#include <vector>
|
#include <list>
|
#include <map>
|
#include <string>
|
#include <cstdlib>
|
|
#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
#define TURN_CHECK_CNT 6
|
|
enum {
|
START_CAR_NOT_DO,
|
START_CAR_DOING,
|
START_CAR_DONE
|
};
|
|
enum {
|
STOP_CAR_NOT_DO,
|
STOP_CAR_DOING,
|
STOP_CAR_DONE
|
};
|
|
enum {
|
CROSSING_NOT_HINT,
|
CROSSING_HAS_HINT,
|
};
|
|
typedef struct {
|
int road;
|
int sep;
|
int no;
|
int guide;
|
} lane_t;
|
|
typedef struct {
|
int edgeIndex;
|
int pointIndex;
|
PointF point;
|
} projection_t;
|
|
typedef struct
|
{
|
int name;
|
int value;
|
struct RtkTime time;
|
} car_sensor_value_t;
|
|
typedef struct {
|
int gain;
|
struct RtkTime time;
|
} change_lane_t;
|
|
static const int INVALID_ROAD = -1;
|
|
static const int TURN_THRESHOLD = 3;
|
static const int TURN_CHECK_INTERVAL = 500;
|
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
|
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
|
const double CHANGE_LANE_RANGE = 100.0;
|
const double OVERTAKE_RANGE = 150.0;
|
const int OVERTAKE_HOLD_TIME = D_SEC(3); // 在超车道行驶的一段时间
|
const double EXAM_RANGE = 3000.0; // 至少驾驶距离
|
|
static const double NEXT_ROAD_TIP = 100.0; // 到达路口前提示下一个怎么走
|
|
|
static bool occurCrashRedLine;
|
static bool occurCrashGreenLine;
|
static bool occurOverSpeed;
|
static bool occurSecondBreak;
|
|
static char carIntersectionOfGreenLine;
|
static int currTurnSignalStatus;
|
static int turnSignalStatusWhenCrashGreenLine;
|
|
static int prevMoveDirect;
|
static uint32_t stopTimepoint = 0;
|
static bool reportStopCarOnRedArea;
|
static PointF stopPoint, startPoint;
|
static bool prevGearError = false;
|
static bool prevGearNSlide = false;
|
|
static bool slideLongDistance;
|
static bool slideNormalDistance;
|
static bool occurSlide;
|
static bool startCarLeftTurnSignal, checkStartCarSignal;
|
|
static struct RtkTime crashGreenRunTime, crashGreenStartTime;
|
|
static struct drive_timer gearErrorTimePoint;
|
static struct drive_timer gearNSlideTimePoint;
|
static struct drive_timer startCarLeftTurnSignalTime;
|
static struct drive_timer overTakeCmpTime;
|
|
static int gearErrorTime;
|
static int gearNSlideTime;
|
|
static int startCar, stopCar;
|
static int currExamMapIndex;
|
static trigger_line_t *currRoadItem;
|
static PointF roadItemStartPoint;
|
static struct drive_timer roadItemStartTime;
|
static bool overtake = false;
|
static bool checkTurn = false;
|
static bool checkDoor = false;
|
static bool handBreakActive = false;
|
static bool reportRPMOver = false;
|
|
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;
|
|
static struct RtkTime beginTurnTime, prevDetectTurnTime;
|
static int startTurnYaw, prevYaw;
|
static int turnCnt, turnTimeCnt;
|
static int prevTurnWise;
|
|
static const int MAX_ENGINE_RPM = 2500;
|
static const double START_CAR_MOVE_DISTANCE = 10.0;
|
static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0;
|
static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
|
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
|
static const uint32_t STOP_CAR_TIME = D_SEC(2);
|
static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10);
|
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
|
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
|
|
static const int CRL_NONE = 0;
|
static const int CRL_SEP_DOTTED = 1;
|
static const int CRL_SEP_SOLID = 2;
|
static const int CRL_EDGE_DOTTED = 3;
|
static const int CRL_EDGE_SOLID = 4;
|
|
static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
|
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
|
|
static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
|
|
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 trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
|
|
static bool AllCmp(road_exam_map &map);
|
|
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
|
|
void InitRoadExam(road_exam_map &RoadMap)
|
{
|
DEBUG("Start road_exam");
|
|
occurCrashRedLine = false;
|
occurCrashGreenLine = false;
|
occurOverSpeed = false;
|
|
occurSecondBreak = false;
|
|
carIntersectionOfGreenLine = 0;
|
|
currTurnSignalStatus = OFF_LIGHT;
|
prevMoveDirect = 0;
|
|
reportStopCarOnRedArea = false;
|
occurSlide = false;
|
slideLongDistance = false;
|
slideNormalDistance = false;
|
prevGearError = false;
|
gearErrorTime = 0;
|
prevGearNSlide = false;
|
gearNSlideTime = 0;
|
|
currExamMapIndex = -1;
|
|
startCar = START_CAR_NOT_DO;
|
stopCar = STOP_CAR_NOT_DO;
|
|
currRoadItem = NULL;
|
|
checkDoor = false;
|
handBreakActive = false;
|
reportRPMOver = false;
|
|
checkStartCarSignal = startCarLeftTurnSignal = false;
|
|
Lane.road = Lane.sep = Lane.no = -1;
|
Lane.guide = 0;
|
|
checkTurn = false;
|
|
CrashLineType = -1;
|
turnCnt = -1;
|
|
ResetOdo();
|
ResetTarget(RoadMap);
|
// 初始化考项
|
}
|
|
void TerminateRoadExam(void)
|
{
|
TerminateDummyLightExam();
|
TerminateStopCarExam();
|
TerminateOperateGearExam();
|
TerminateDriveStraightExam();
|
}
|
|
/*********************************************************************
|
* 计算某点到道路边线的投影点
|
* @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();
|
}
|
|
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;
|
}
|
|
}
|
}
|
}
|
}
|
|
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();
|
|
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;
|
}
|
}
|
|
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));
|
}
|
}
|
|
static 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 int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
double moveDistance;
|
|
if (startCar == START_CAR_NOT_DO) {
|
startPoint = car->basePoint;
|
reportRPMOver = false;
|
startCar = START_CAR_DOING;
|
|
PlayTTS("请起步");
|
} else if (startCar == START_CAR_DOING) {
|
moveDistance = DistanceOf(startPoint, car->basePoint);
|
DEBUG("起步行驶距离 %f", moveDistance);
|
|
if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) {
|
startCarLeftTurnSignal = true;
|
Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime);
|
}
|
|
if (!checkStartCarSignal && moveDirect == 1) {
|
checkStartCarSignal = true;
|
if (!startCarLeftTurnSignal) {
|
AddExamFault(13, rtkTime);
|
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
|
AddExamFault(14, rtkTime);
|
}
|
}
|
|
if (moveDistance > START_CAR_MOVE_DISTANCE) {
|
if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
|
DEBUG("Handbreak active move over 10m");
|
// 手刹拉起状态下，行驶了10米以上，不合格
|
AddExamFault(25, rtkTime);
|
} else if (handBreakActive) {
|
// 手刹拉起状态下，行驶了1米以上，扣10分
|
DEBUG("Handbreak active move over 1M");
|
AddExamFault(26, rtkTime);
|
}
|
|
PlayTTS("完成起步");
|
DEBUG("############# 完成起步 ############");
|
startCar = START_CAR_DONE;
|
} else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
|
if (!checkDoor) {
|
checkDoor = true;
|
|
if (ReadCarStatus(DOOR) == DOOR_OPEN) {
|
// 车门未完全关闭，不合格
|
DEBUG("车门未关闭");
|
AddExamFault(23, rtkTime);
|
}
|
|
if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
|
handBreakActive = true;
|
}
|
}
|
}
|
|
if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
|
// 转速超标，不合格
|
DEBUG("转速超标");
|
AddExamFault(29, rtkTime);
|
reportRPMOver = true;
|
}
|
} else {
|
|
}
|
|
return startCar;
|
}
|
|
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(11, rtkTime);
|
}
|
if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) {
|
// 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
|
DEBUG("撞道路边缘线");
|
AddExamFault(11, rtkTime);
|
}
|
if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
|
if (!crashDottedLine) {
|
checkCrashGreenTimeout = 0;
|
// 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
|
crashGreenStartTime = *rtkTime;
|
}
|
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) >= CRASH_DOTTED_LINE_TIMEOUT) {
|
DEBUG("长时间压虚线");
|
checkCrashGreenTimeout = 2;
|
// 长时间骑轧车道分界线行驶，不合格
|
AddExamFault(12, 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 {
|
ChangeLane.gain = 0;
|
}
|
|
// 检查转向灯
|
if (gain != 0) {
|
DEBUG("变道 gain %d", gain);
|
car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
|
if (lamp.name == TURN_SIGNAL_LAMP) {
|
if (gain < 0) {
|
if (lamp.value != LEFT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(13, rtkTime);
|
} else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
AddExamFault(14, rtkTime);
|
}
|
} else {
|
if (lamp.value != RIGHT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(13, rtkTime);
|
} else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
AddExamFault(14, rtkTime);
|
}
|
}
|
}
|
|
if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < CHANGE_LANE_MIN_INTERVAL) {
|
DEBUG("连续变道");
|
AddExamFault(15, 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;
|
}
|
|
static void ResetCrossingStatus(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);
|
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;
|
}
|
}
|
|
static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
for (int i = 0; i < road.stopLine.size(); ++i) {
|
PointF point;
|
double distance;
|
|
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 > 10 && distance < 50) {
|
// 提示路口怎么走
|
if (GetCrossingStatus(roadIndex, i) == CROSSING_NOT_HINT) {
|
if (!road.stopLine[i].tts.empty()) {
|
DEBUG("路口提示 %s", road.stopLine[i].tts.c_str());
|
PlayTTS(road.stopLine[i].tts.c_str());
|
}
|
ChangeCrossingStatus(roadIndex, i, CROSSING_HAS_HINT);
|
}
|
} else if (distance > 55 && GetCrossingStatus(roadIndex, i) != CROSSING_NOT_HINT) {
|
ChangeCrossingStatus(roadIndex, i, CROSSING_NOT_HINT);
|
}
|
}
|
}
|
|
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
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 = road.stopLine[i].active;
|
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;
|
}
|
|
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
// uint32_t cts = AppTimer_GetTickCount();
|
// int ri = CalcRoadIndex(-1, RoadMap, car);
|
// bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
|
// lane_t laneInfo;
|
// double redist = -1;
|
//
|
// laneInfo.road = -1;
|
// laneInfo.sep = -1;
|
// laneInfo.no = -1;
|
//
|
// if (ri >= 0) {
|
// GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
|
//
|
// int m = RoadMap.roads[ri].rightEdge.size();
|
// int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
|
//
|
// PointF base;
|
//
|
// base.X = 428922.2985; base.Y = 3292119.5457;
|
//
|
// redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
|
// RoadMap.roads[ri].rightEdge);
|
// }
|
//
|
// DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);
|
|
UpdateCarSensor(rtkTime);
|
|
UpdataOdo(speed, moveDirect, rtkTime);
|
|
// 超速检测
|
if (moveDirect != 0 && speed > MAX_SPEED) {
|
if (!occurOverSpeed) {
|
occurOverSpeed = true;
|
// 超速，不合格
|
DEBUG("超速 %f", speed);
|
AddExamFault(10, rtkTime);
|
}
|
} else {
|
occurOverSpeed = false;
|
}
|
|
// 副刹车检测
|
if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
|
// 副刹车踩下，不合格
|
if (!occurSecondBreak) {
|
DEBUG("副刹车动作了");
|
occurSecondBreak = true;
|
AddExamFault(17, rtkTime);
|
}
|
} else {
|
occurSecondBreak = false;
|
}
|
|
// 挡位匹配检测
|
bool currGearError = false;
|
bool currGearNSlide = false;
|
|
switch (ReadCarStatus(GEAR)) {
|
case GEAR_N:
|
if (moveDirect != 0) {
|
// 空档滑行
|
currGearNSlide = true;
|
}
|
break;
|
case GEAR_1:
|
if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_2:
|
if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_3:
|
if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_4:
|
if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
|
currGearError = true;
|
}
|
break;
|
case GEAR_5:
|
if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
|
currGearError = true;
|
}
|
break;
|
default:break;
|
}
|
// 空档滑行超时
|
if (currGearNSlide && prevGearNSlide) {
|
gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
|
}
|
if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
|
// 空档滑行超5秒，不合格
|
DEBUG("挡位滑行，超过5秒");
|
AddExamFault(8, rtkTime);
|
gearNSlideTime = 0;
|
}
|
|
prevGearNSlide = currGearNSlide;
|
if (prevGearNSlide) {
|
Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
|
} else {
|
gearNSlideTime = 0;
|
}
|
// 挡位不匹配超时
|
if (currGearError && prevGearError) {
|
gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
|
}
|
if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
|
// 累计15秒，挡位-车速不匹配，不合格
|
DEBUG("挡位错误超过15秒");
|
AddExamFault(6, rtkTime);
|
gearErrorTime = 0;
|
}
|
|
prevGearError = currGearError;
|
if (prevGearError) {
|
Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
|
}
|
|
// 起步后滑
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
reportStopCarOnRedArea = 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(18, 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) {
|
// 持续停车
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
/*if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
|
// 停车超2秒，停在红区，不合格
|
AddExamFault(16, rtkTime);
|
DEBUG("禁停区停车");
|
reportStopCarOnRedArea = true;
|
}*/
|
} else if (moveDirect == -1) {
|
// 持续后滑
|
if (occurSlide) {
|
double slideDistance = DistanceOf(stopPoint, car->basePoint);
|
|
if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
|
slideNormalDistance = true;
|
}
|
|
if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
|
// 后滑超过30厘米, 不合格
|
AddExamFault(5, 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(oldid);
|
ResetErrorLaneRpt(oldid);
|
}
|
}
|
|
if (currExamMapIndex >= 0) {
|
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
|
HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
|
|
// 检测通过路口、人行道等区域时，释放刹车或减速
|
ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
|
}
|
ExitTarget(RoadMap, car, CarModelList, rtkTime);
|
|
oldid = CrashLineType;
|
|
DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
|
|
if (oldid != CrashLineType) {
|
DEBUG("压线类型切换 %d", CrashLineType);
|
}
|
|
|
oldid = Lane.guide;
|
DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
|
if (Lane.guide > 0 && currExamMapIndex >= 0) {
|
int stop_line;
|
|
if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
|
if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
|
DEBUG("不按规定车道标向行驶");
|
AddExamFault(9, rtkTime);
|
SetErrorLaneRpt(currExamMapIndex, stop_line, true);
|
}
|
}
|
}
|
|
if (Lane.guide != oldid) {
|
DEBUG("导向类型切换 %d", Lane.guide);
|
}
|
|
// 检测压线状态
|
TestRoadStartCar(car, speed, moveDirect, rtkTime);
|
|
// 额外的转向检测
|
DetectTurn(car, moveDirect, rtkTime);
|
|
if (startCar != START_CAR_DONE)
|
return;
|
|
if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
|
// 在合适条件下停车结束考试
|
StartStopCarExam("请靠边停车");
|
stopCar = STOP_CAR_DOING;
|
} else if (stopCar == STOP_CAR_DOING) {
|
if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0)
|
stopCar = STOP_CAR_DONE;
|
}
|
|
|
// 执行某个项目
|
if (currRoadItem != NULL) {
|
if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
|
if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
|
DEBUG("变道距离超标");
|
AddExamFault(3, rtkTime);
|
currRoadItem = NULL;
|
}
|
} else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
|
if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) {
|
DEBUG("超车距离超标");
|
AddExamFault(3, rtkTime);
|
currRoadItem = NULL;
|
} else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
|
rtkTime->mss * 10,
|
overTakeCmpTime.hour, overTakeCmpTime.min,
|
overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) {
|
DEBUG("回原车道");
|
PlayTTS("请返回原车道");
|
currRoadItem = NULL;
|
}
|
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
|
if (ExecuteOperateGearExam(rtkTime) < 0) {
|
currRoadItem = NULL;
|
}
|
} else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
|
if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) {
|
currRoadItem = NULL;
|
}
|
}
|
} // 检测由触发线控制的项目
|
else if (currExamMapIndex >= 0) {
|
trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
|
|
if (new_item != NULL && !new_item->cmp) {
|
currRoadItem = new_item;
|
if (!currRoadItem->tts.empty())
|
PlayTTS(currRoadItem->tts.c_str());
|
|
// 初始时间和距离限制
|
roadItemStartPoint = car->basePoint;
|
Rtk2DriveTimer(roadItemStartTime, rtkTime);
|
|
if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
|
overtake = false;
|
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
|
StartOperateGearExam(rtkTime);
|
} else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
|
StartDriveStraightExam(currRoadItem->tts);
|
}
|
}
|
}
|
}
|
|
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
|
{
|
tm.hour = rtkTime->hh;
|
tm.min = rtkTime->mm;
|
tm.sec = rtkTime->ss;
|
tm.msec = rtkTime->mss;
|
}
|
|
void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
|
{
|
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;
|
}
|
} else if (turnCnt == 1) {
|
if (angle != 0 && moveDirect != 0) {
|
if (angle * prevTurnWise > 0) {
|
DEBUG("确认转向");
|
// 同向转动
|
turnCnt++;
|
turnTimeCnt = 0;
|
} else {
|
beginTurnTime = *rtkTime;
|
startTurnYaw = (int) car->yaw;
|
}
|
} 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) > 60) {
|
// 确认转弯行为，检测开始刚转弯时转向灯情况
|
turnCnt = -1;
|
|
car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
|
if (lamp.name == TURN_SIGNAL_LAMP) {
|
if (wise < 0) {
|
if (lamp.value != LEFT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(13, rtkTime);
|
} else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
AddExamFault(14, rtkTime);
|
}
|
} else {
|
if (lamp.value != RIGHT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
AddExamFault(13, rtkTime);
|
} else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
AddExamFault(14, 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;
|
}
|
|
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
|
{
|
// 最近2秒内，每0.5秒的角度差大于5度，且方向相同，连续4次；或突现超30度的转向；认为转向。
|
if (CarModelList.size() < 1)
|
return false;
|
|
list<car_model *>::iterator iter = CarModelList.begin();
|
|
car_model *c1 = *iter, *c2;
|
|
++iter;
|
|
char turn[TURN_CHECK_CNT] = {0};
|
int checkCnt = 0;
|
|
// DEBUG("CheckCarTurn.........");
|
|
while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) {
|
c2 = *iter;
|
|
uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
|
|
if (tdiff >= TURN_CHECK_INTERVAL) {
|
int ang = 0;
|
turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
|
// DEBUG("%c 角度比较 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
|
|
if (turn[checkCnt] == 'N') {
|
break;
|
} else if (ang >= 30) {
|
DEBUG("左右转确认 %c", turn[checkCnt]);
|
return turn[checkCnt];
|
}
|
|
c1 = c2;
|
checkCnt++;
|
}
|
|
++iter;
|
}
|
|
int i = 0;
|
for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
|
if (turn[i] != turn[i+1])
|
break;
|
}
|
|
if (i == TURN_CHECK_CNT-1) {
|
DEBUG("左右转确认 %c", turn[0]);
|
return turn[0];
|
}
|
|
return 0;
|
}
|
|
/**********************************************************
|
* 按整个车身是否覆盖计算
|
* @param RoadMapList
|
* @param car
|
* @return
|
*/
|
/*static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
|
{
|
bool ret = false;
|
|
Polygon carBody;
|
|
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]];
|
}
|
|
for (int i = 0; i < RoadMapList.size(); ++i) {
|
if (RoadMapList[i].type == GENERAL_MAP) {
|
|
// 每条红区都检测
|
for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
|
if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
|
ret = true;
|
}
|
}
|
break;
|
}
|
}
|
|
free(carBody.point);
|
|
return ret;
|
}*/
|
|
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;
|
}
|
|
|
/***************************************************
|
* 接近路口时，提示下一步怎么走
|
* @param road
|
* @param car
|
* @param CarModelList
|
*/
|
/*static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
// 计算车前进轨迹延长线
|
double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
|
|
PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
|
Line extLine;
|
|
MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
|
|
if (IntersectionOf(extLine, road.stopLine) == GM_Intersection &&
|
IntersectionOfLine(extPoint, road.stopLine) == -1) {
|
if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
|
// 接近路口后，要检查车辆是否进入错误车道
|
DEBUG("接近路口");
|
road.arrivedTail = true;
|
if (!road.tts.empty())
|
PlayTTS(road.tts.c_str());
|
}
|
} else if (road.arrivedTail) {
|
road.arrivedTail = false;
|
}
|
}*/
|
|
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
|
if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
|
Line triggerLine;
|
|
if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) {
|
RoadMap.triggerLines[i].leftPoints.clear();
|
|
for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
|
RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
|
}
|
|
for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
|
DEBUG("触发线补齐 road %d id %d type %d (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.roads[index].id, RoadMap.triggerLines[i].id, RoadMap.triggerLines[i].active,
|
RoadMap.triggerLines[i].points[0].X, RoadMap.triggerLines[i].points[0].Y, RoadMap.triggerLines[i].leftPoints[0].X, RoadMap.triggerLines[i].leftPoints[0].Y);
|
}
|
}
|
|
MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]);
|
|
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];
|
}
|
}
|
}
|
|
return NULL;
|
}
|
|
static bool AllCmp(road_exam_map &map)
|
{
|
for (int i = 0; i < map.triggerLines.size(); ++i) {
|
if (!map.triggerLines[i].cmp)
|
return false;
|
}
|
return true;
|
}
|
