//
|
// 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 <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
|
};
|
|
static const int INVALID_ROAD = -1;
|
|
static const int TURN_THRESHOLD = 1;
|
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 double OVERTAKE_HOLD_RANGE = 30.0; // 在超车道行驶的一段距离
|
|
static const double LASTEST_BREAK_POINT = 30.0;
|
static const double NEXT_ROAD_TIP = 100.0; // 到达路口前提示下一个怎么走
|
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
|
static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh
|
|
static const int FIND_POSITION = -2;
|
static const int INVALID_POSITION = -1;
|
|
static bool occurCrashRedLine;
|
static bool occurCrashGreenLine;
|
static bool occurOverSpeed;
|
static bool occurSecondBreak;
|
static int checkCrashGreenTimeout;
|
static char carIntersectionOfGreenLine;
|
static int currTurnSignalStatus;
|
static int turnSignalStatusWhenCrashGreenLine;
|
static bool reportTurnSignalError;
|
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 drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
|
static struct drive_timer gearErrorTimePoint;
|
static struct drive_timer gearNSlideTimePoint;
|
static struct drive_timer startCarLeftTurnSignalTime;
|
|
static int gearErrorTime;
|
static int gearNSlideTime;
|
|
static int startCar;
|
static int currExamMapIndex;
|
static trigger_line_t *currRoadItem;
|
static int nextRoadId;
|
static PointF roadItemStartPoint;
|
static struct drive_timer roadItemStartTime;
|
static bool overtake = false;
|
|
static bool checkDoor = false;
|
static bool handBreakActive = false;
|
static bool reportRPMOver = false;
|
|
static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);
|
|
static bool turnError13Cold, turnError14Cold;
|
static struct car_on_lane {
|
int road;
|
int separate;
|
int lane;
|
int direct; // 车道方向限制
|
int type; // 实线，虚线
|
} CurrentLane;
|
static bool laneChanging;
|
|
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_ROAD_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 = 40.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 char isTurn(int currYaw, int prevYaw, int &ang);
|
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
|
|
static void TurnSignalError13ColdTimer(union sigval sig);
|
static void TurnSignalError14ColdTimer(union sigval sig);
|
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
|
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
|
static int CrashRoadLine(road_t &road, const car_model *car);
|
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
|
static bool LaneIsValid(struct car_on_lane lane);
|
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
|
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
|
static void ClearAll(road_exam_map &map);
|
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
|
double speed, int moveDirect, const struct RtkTime *rtkTime);
|
|
void InitRoadExam(road_exam_map &RoadMap)
|
{
|
DEBUG("Start road_exam");
|
|
crashGreenCmpTime.hour = -1;
|
occurCrashRedLine = false;
|
occurCrashGreenLine = false;
|
occurOverSpeed = false;
|
|
occurSecondBreak = false;
|
checkCrashGreenTimeout = 0;
|
carIntersectionOfGreenLine = 0;
|
|
reportTurnSignalError = false;
|
currTurnSignalStatus = OFF_LIGHT;
|
prevMoveDirect = 0;
|
|
reportStopCarOnRedArea = false;
|
occurSlide = false;
|
slideLongDistance = false;
|
slideNormalDistance = false;
|
prevGearError = false;
|
gearErrorTime = 0;
|
prevGearNSlide = false;
|
gearNSlideTime = 0;
|
|
currExamMapIndex = FIND_POSITION;
|
|
startCar = START_CAR_NOT_DO;
|
currRoadItem = NULL;
|
|
checkDoor = false;
|
handBreakActive = false;
|
reportRPMOver = false;
|
|
checkStartCarSignal = startCarLeftTurnSignal = false;
|
|
turnError13Cold = turnError14Cold = true;
|
|
CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
|
laneChanging = false;
|
|
nextRoadId = -1;
|
ClearAll(RoadMap);
|
}
|
|
void TerminateRoadExam(void)
|
{
|
TerminateDummyLightExam();
|
TerminateStopCarExam();
|
TerminateOperateGearExam();
|
|
AppTimer_delete(TurnSignalError13ColdTimer);
|
AppTimer_delete(TurnSignalError14ColdTimer);
|
}
|
|
static void TurnSignalError13ColdTimer(union sigval sig)
|
{
|
AppTimer_delete(TurnSignalError13ColdTimer);
|
|
turnError13Cold = true;
|
}
|
|
static void TurnSignalError14ColdTimer(union sigval sig)
|
{
|
AppTimer_delete(TurnSignalError14ColdTimer);
|
|
turnError14Cold = true;
|
}
|
|
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime)
|
{
|
if (err == 13 && turnError13Cold) {
|
DEBUG("起步不开转向灯");
|
AddExamFault(13, rtkTime);
|
|
turnError13Cold = false;
|
AppTimer_delete(TurnSignalError13ColdTimer);
|
AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME);
|
} else if (err == 14 && turnError14Cold) {
|
DEBUG("起步转向灯不足3秒");
|
AddExamFault(14, rtkTime);
|
|
turnError14Cold = false;
|
AppTimer_delete(TurnSignalError14ColdTimer);
|
AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME);
|
}
|
}
|
|
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) {
|
ReportTurnSignalError(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) {
|
ReportTurnSignalError(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;
|
}
|
|
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *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 {
|
// 前进
|
|
}
|
|
// 转向灯读取
|
switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
|
case LEFT_TURN_LIGHT:
|
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
|
currTurnSignalStatus = LEFT_TURN_LIGHT;
|
Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
|
}
|
break;
|
case RIGHT_TURN_LIGHT:
|
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
|
currTurnSignalStatus = RIGHT_TURN_LIGHT;
|
Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
|
}
|
break;
|
default:
|
currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
|
break;
|
}
|
|
// 检测通过路口、人行道等区域时，释放刹车或减速
|
CheckBreakActive(RoadMap, car, CarModelList,
|
speed, moveDirect, rtkTime);
|
|
// 检测离开此路段，全车需不在范围内
|
if (currExamMapIndex >= 0) {
|
Polygon area;
|
int n = 0;
|
|
area.num = 0;
|
|
for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
|
if (j > 0) {
|
area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size() - 1;
|
} else {
|
area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size();
|
}
|
}
|
for (int j = 0; j < RoadMap.roads[currExamMapIndex].rightEdge.size(); ++j) {
|
if (j > 0) {
|
area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1;
|
} else {
|
area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size();
|
}
|
}
|
|
area.point = (PointF *) malloc(area.num * sizeof(PointF));
|
|
for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
|
for (int k = (j>0?1:0); k < RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); ++k) {
|
area.point[n++] = RoadMap.roads[currExamMapIndex].leftEdge[j].points[k];
|
}
|
}
|
|
for (int j = RoadMap.roads[currExamMapIndex].rightEdge.size() - 1; j >= 0; --j) {
|
if (j == RoadMap.roads[currExamMapIndex].rightEdge.size() - 1) {
|
for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; k >= 0; --k) {
|
area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
|
}
|
} else {
|
for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 2; k >= 0; --k) {
|
area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
|
}
|
}
|
}
|
|
// 全车都需不在地图中
|
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]];
|
}
|
|
if (IntersectionOf(&carBody, &area) == GM_None) {
|
DEBUG("离开路段 id = %d", RoadMap.roads[currExamMapIndex].id);
|
|
RoadMap.roads[currExamMapIndex].arrivedTail = false;
|
|
currExamMapIndex = FIND_POSITION;
|
}
|
|
free(carBody.point);
|
free(area.point);
|
}
|
if (currExamMapIndex == FIND_POSITION) {
|
for (int i = 0; i < RoadMap.roads.size(); ++i) {
|
Polygon area;
|
int n = 0;
|
|
area.num = 0;
|
|
for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
|
if (j > 0) {
|
area.num += RoadMap.roads[i].leftEdge[j].points.size() - 1;
|
} else {
|
area.num += RoadMap.roads[i].leftEdge[j].points.size();
|
}
|
}
|
for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
|
if (j > 0) {
|
area.num += RoadMap.roads[i].rightEdge[j].points.size() - 1;
|
} else {
|
area.num += RoadMap.roads[i].rightEdge[j].points.size();
|
}
|
}
|
|
area.point = (PointF *) malloc(area.num * sizeof(PointF));
|
|
for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
|
for (int k = (j>0?1:0); k < RoadMap.roads[i].leftEdge[j].points.size(); ++k) {
|
area.point[n++] = RoadMap.roads[i].leftEdge[j].points[k];
|
}
|
}
|
|
for (int j = RoadMap.roads[i].rightEdge.size() - 1; j >= 0; --j) {
|
if (j == RoadMap.roads[i].rightEdge.size() - 1) {
|
for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 1; k >= 0; --k) {
|
area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
|
}
|
} else {
|
for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 2; k >= 0; --k) {
|
area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
|
}
|
}
|
}
|
|
if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
|
currExamMapIndex = i;
|
DEBUG("进入道路 id = %d", RoadMap.roads[i].id);
|
if (nextRoadId >= 0 && RoadMap.roads[i].id != nextRoadId) {
|
DEBUG("不按规矩行驶，进入错误路段");
|
AddExamFault(3, rtkTime);
|
}
|
nextRoadId = -1;
|
break;
|
}
|
|
free(area.point);
|
}
|
if (currExamMapIndex < 0) {
|
currExamMapIndex = INVALID_POSITION;
|
DEBUG("搜寻未果");
|
}
|
} else if (currExamMapIndex == INVALID_POSITION) {
|
for (int i = 0; i < RoadMap.roads.size(); ++i) {
|
if (CrashTheLine(RoadMap.roads[i].startLine, car, CarModelList)) {
|
currExamMapIndex = i;
|
DEBUG("进入道路 id = %d", RoadMap.roads[i].id);
|
break;
|
}
|
}
|
}
|
|
// 检测压线状态
|
bool crashRedLineNow = false;
|
bool crashGreenLineNow = false;
|
|
if (currExamMapIndex >= 0) {
|
int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);
|
|
if (crl == CRL_NONE) {
|
// DEBUG("什么都没压");
|
} else if (crl == CRL_SEP_DOTTED) {
|
DEBUG("压分道虚线");
|
crashGreenLineNow = true;
|
} else if (crl == CRL_SEP_SOLID) {
|
DEBUG("压分道实线");
|
crashRedLineNow = true;
|
} else if (crl == CRL_EDGE_DOTTED) {
|
DEBUG("压边沿虚线");
|
crashGreenLineNow = true;
|
} else if (crl == CRL_EDGE_SOLID) {
|
DEBUG("压边沿实线");
|
crashRedLineNow = true;
|
}
|
|
struct car_on_lane lane;
|
if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {
|
|
if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
|
lane.lane == CurrentLane.lane) {
|
laneChanging = false;
|
}
|
|
if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
|
lane.lane != CurrentLane.lane) {
|
if (crl == CRL_SEP_DOTTED) {
|
// 变道进行中
|
if (!laneChanging) {
|
laneChanging = true;
|
DEBUG("发生变道");
|
// 比较上次跨线时间
|
if (crashGreenCmpTime.hour >= 0) {
|
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
|
rtkTime->mss * 10,
|
crashGreenCmpTime.hour,
|
crashGreenCmpTime.min,
|
crashGreenCmpTime.sec,
|
crashGreenCmpTime.msec * 10);
|
|
if (diff < CHANGE_ROAD_MIN_INTERVAL) {
|
DEBUG("===================== 连续变道 ============!!");
|
// 连续变道，不合格
|
AddExamFault(15, rtkTime);
|
}
|
}
|
|
// 检查变道前，是否提前转向灯
|
if (lane.lane < CurrentLane.lane) {
|
// 向左侧变道
|
DEBUG("向左侧变道");
|
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
ReportTurnSignalError(13, rtkTime);
|
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
|
rtkTime->mss * 10,
|
turnSignalChangeTime.hour,
|
turnSignalChangeTime.min,
|
turnSignalChangeTime.sec,
|
turnSignalChangeTime.msec * 10) <
|
TURN_SIGNAL_LAMP_ADVANCE) {
|
if (!reportTurnSignalError) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
reportTurnSignalError = true;
|
ReportTurnSignalError(14, rtkTime);
|
}
|
}
|
} else {
|
// 向右侧变道
|
DEBUG("向右侧变道");
|
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
ReportTurnSignalError(13, rtkTime);
|
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
|
rtkTime->mss * 10,
|
turnSignalChangeTime.hour,
|
turnSignalChangeTime.min,
|
turnSignalChangeTime.sec,
|
turnSignalChangeTime.msec * 10) <
|
TURN_SIGNAL_LAMP_ADVANCE) {
|
if (!reportTurnSignalError) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
reportTurnSignalError = true;
|
ReportTurnSignalError(14, rtkTime);
|
}
|
}
|
}
|
}
|
} else {
|
// 变道完成
|
DEBUG("变道完成");
|
|
if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
|
DEBUG("变更车道项目完成");
|
currRoadItem = NULL;
|
} else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) {
|
if (CurrentLane.lane > lane.lane) {
|
DEBUG("超车变道完成");
|
|
} else {
|
DEBUG("右道超车，错误");
|
AddExamFault(3, rtkTime);
|
currRoadItem = NULL;
|
}
|
}
|
|
CurrentLane = lane;
|
laneChanging = false;
|
// 记录本次变道时间点
|
Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
|
}
|
}
|
|
if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
|
// 路或段变更，撤销变道跟踪
|
DEBUG("============== 路或段变更 CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
|
CurrentLane.separate, CurrentLane.direct,
|
lane.road, lane.separate, lane.direct);
|
|
CurrentLane = lane;
|
laneChanging = false;
|
}
|
|
if (CurrentLane.direct != 0 && !(CurrentLane.direct & RoadMap.roads[currExamMapIndex].active)) {
|
if (!RoadMap.roads[currExamMapIndex].errorLane) {
|
RoadMap.roads[currExamMapIndex].errorLane = true;
|
DEBUG("不按规定车道标线方向行驶 %d", CurrentLane.direct);
|
AddExamFault(9, rtkTime);
|
}
|
}
|
}
|
|
ArrivedRoadEnd(RoadMap.roads[currExamMapIndex], car, CarModelList);
|
|
if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) {
|
DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad);
|
nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad;
|
}
|
}
|
|
// 撞红线
|
if (crashRedLineNow) {
|
if (!occurCrashRedLine) {
|
// 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
|
DEBUG("撞道路边缘线");
|
AddExamFault(11, rtkTime);
|
occurCrashRedLine = true;
|
}
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
// 压虚线
|
if (crashGreenLineNow) {
|
// 压虚线
|
if (moveDirect != 0) {
|
if (checkCrashGreenTimeout == 0) {
|
checkCrashGreenTimeout = 1;
|
Rtk2DriveTimer(crashGreenRunTime, rtkTime); // 运动中压虚线的开始时间点
|
} else if (checkCrashGreenTimeout == 1) {
|
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
|
|
if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
|
DEBUG("长时间压虚线");
|
checkCrashGreenTimeout = 2;
|
// 长时间骑轧车道分界线行驶，不合格
|
AddExamFault(12, rtkTime);
|
}
|
}
|
} else {
|
// 停车就不计时了
|
checkCrashGreenTimeout = 0;
|
}
|
|
// 检测当前车辆于虚线的位置，做变道检测;
|
// 检测是否3秒前有开启对应之转向灯
|
if (!occurCrashGreenLine) {
|
occurCrashGreenLine = true;
|
// 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
|
Rtk2DriveTimer(crashGreenStartTime, rtkTime);
|
turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
|
}
|
} else {
|
// 不再压虚线
|
occurCrashGreenLine = false;
|
checkCrashGreenTimeout = 0;
|
}
|
|
TestRoadStartCar(car, speed, moveDirect, rtkTime);
|
|
if (startCar != START_CAR_DONE)
|
return;
|
|
// 检测由触发线控制的项目
|
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 && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_HOLD_RANGE) {
|
DEBUG("回原车道");
|
PlayTTS("请返回原车道");
|
currRoadItem = NULL;
|
}
|
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
|
ExecuteOperateGearExam(rtkTime);
|
}
|
} else if (currExamMapIndex >= 0) {
|
currRoadItem = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
|
if (currRoadItem != NULL) {
|
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);
|
}
|
}
|
}
|
}
|
|
/*************************************************************
|
* 检测当前车道，以车头中点为基准
|
* @param road
|
* @param car
|
*/
|
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car)
|
{
|
bool leftExt = false, rightExt = false;
|
Line leftExtLine, rightExtLine;
|
struct car_on_lane lane;
|
|
for (int i = 0; i < road.leftEdge.size(); ++i) {
|
PointF p1, p2;
|
Line edge;
|
|
p1 = road.leftEdge[i].points[0];
|
for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
|
p2 = road.leftEdge[i].points[j];
|
MakeLine(&edge, &p1, &p2);
|
|
PointF vp;
|
if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
|
leftExt = true;
|
MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
|
goto LEFT_EXT_CMP;
|
}
|
p1 = p2;
|
}
|
}
|
LEFT_EXT_CMP:
|
for (int i = 0; i < road.rightEdge.size(); ++i) {
|
PointF p1, p2;
|
Line edge;
|
|
p1 = road.rightEdge[i].points[0];
|
for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
|
p2 = road.rightEdge[i].points[j];
|
MakeLine(&edge, &p1, &p2);
|
|
PointF vp;
|
if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
|
rightExt = true;
|
MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
|
goto RIGHT_EXT_CMP;
|
} else {
|
DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
|
}
|
p1 = p2;
|
}
|
}
|
RIGHT_EXT_CMP:
|
|
if (!leftExt || !rightExt) {
|
DEBUG("左右边界不匹配");
|
return false;
|
}
|
|
bool orthogonalInSegment = false;
|
|
for (int i = 0; i < road.separate.size(); ++i) { // 段
|
PointF p1, p2;
|
Line sep;
|
|
map<int, vector<int>> orthogonal;
|
|
// 一段分道组内，有一条正交，就必须保证同组的全都正交，否则直接退出
|
for (int j = 0; j < road.separate[i].lines.size(); ++j) { // 线组
|
bool intersection = false;
|
|
for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) { // 节
|
|
p1 = road.separate[i].lines[j][k].points[0];
|
for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
|
p2 = road.separate[i].lines[j][k].points[m];
|
MakeLine(&sep, &p1, &p2);
|
|
if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
|
vector<int> stor(4);
|
|
stor[0] = 1;
|
stor[1] = road.separate[i].lines[j][k].character;
|
stor[2] = road.separate[i].lines[j][k].left_lane_direct;
|
stor[3] = road.separate[i].lines[j][k].right_lane_direct;
|
|
orthogonal.insert(pair<int, vector<int>>(j, stor));
|
orthogonalInSegment = true;
|
intersection = true;
|
|
// DEBUG("分道线 %d 左正交", j);
|
break;
|
} else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
|
vector<int> stor(4);
|
|
stor[0] = 2;
|
stor[1] = road.separate[i].lines[j][k].character;
|
stor[2] = road.separate[i].lines[j][k].left_lane_direct;
|
stor[3] = road.separate[i].lines[j][k].right_lane_direct;
|
|
orthogonal.insert(pair<int, vector<int>>(j, stor));
|
orthogonalInSegment = true;
|
intersection = true;
|
|
// DEBUG("分道线 %d 右正交", j);
|
break;
|
}
|
p1 = p2;
|
}
|
}
|
}
|
// DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size());
|
|
if (orthogonal.size() > 0) {
|
if (orthogonal.size() == road.separate[i].lines.size()) {
|
// 得到当前在第几个车道
|
int x = 0;
|
int left_char = LINE_SOLID; // 道路左边线
|
int right_direct = 0;
|
|
for (x = 0; x < orthogonal.size(); ++x) {
|
auto itx = orthogonal.find(x);
|
if (itx != orthogonal.end()) {
|
if (itx->second[0] != 1) { // 在车辆右侧
|
lane.road = road.id;
|
lane.separate = i;
|
lane.lane = itx->first;
|
if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
|
(itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT)) // 车道左右均是实线
|
lane.direct = itx->second[2];
|
else
|
lane.direct = 0;
|
// DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
|
break;
|
} else {
|
right_direct = itx->second[3];
|
left_char = itx->second[2];
|
}
|
}
|
}
|
if (x >= orthogonal.size()) {
|
lane.road = road.id;
|
lane.separate = i;
|
lane.lane = orthogonal.size();
|
// 最后车道的右侧限定
|
if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
|
lane.direct = right_direct;
|
else
|
lane.direct = 0;
|
// DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
|
}
|
out = lane;
|
return true;
|
} else {
|
// 不完全正交，直接退出
|
}
|
DEBUG("分道线长短不同，退出");
|
return false;
|
}
|
}
|
DEBUG("段匹配失败");
|
return false;
|
}
|
|
/*************************************************************************
|
* 碰触车道线型
|
* @param road
|
* @param car
|
*/
|
static int CrashRoadLine(road_t &road, const car_model *car)
|
{
|
Line frontAxle, rearAxle;
|
|
MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
// 检查道路边缘
|
for (int i = 0; i < road.leftEdge.size(); ++i) {
|
PointF p1, p2;
|
Line edge;
|
|
p1 = road.leftEdge[i].points[0];
|
for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
|
p2 = road.leftEdge[i].points[j];
|
MakeLine(&edge, &p1, &p2);
|
|
if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
|
IntersectionOf(edge, rearAxle) == GM_Intersection) {
|
// 压道路左边线
|
if (road.leftEdge[i].character != LINE_SOLID) {
|
return CRL_EDGE_DOTTED;
|
}
|
return CRL_EDGE_SOLID;
|
}
|
p1 = p2;
|
}
|
}
|
|
for (int i = 0; i < road.rightEdge.size(); ++i) {
|
PointF p1, p2;
|
Line edge;
|
|
p1 = road.rightEdge[i].points[0];
|
for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
|
p2 = road.rightEdge[i].points[j];
|
MakeLine(&edge, &p1, &p2);
|
|
if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
|
IntersectionOf(edge, rearAxle) == GM_Intersection) {
|
// 压道路右边线
|
if (road.rightEdge[i].character != LINE_SOLID) {
|
return CRL_EDGE_DOTTED;
|
}
|
return CRL_EDGE_SOLID;
|
}
|
p1 = p2;
|
}
|
}
|
|
// 检查分道线
|
for (int i = 0; i < road.separate.size(); ++i) {
|
// 段
|
PointF p1, p2;
|
Line sep;
|
|
for (int j = 0; j < road.separate[i].lines.size(); ++j) {
|
// 线
|
for (int k = 0; k < road.separate[i].lines[j].size(); ++k) {
|
// 节
|
p1 = road.separate[i].lines[j][k].points[0];
|
|
for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
|
p2 = road.separate[i].lines[j][k].points[m];
|
MakeLine(&sep, &p1, &p2);
|
|
if (IntersectionOf(sep, frontAxle) == GM_Intersection ||
|
IntersectionOf(sep, rearAxle) == GM_Intersection) {
|
// 检查道路分隔线类型
|
if (road.separate[i].lines[j][k].character == LINE_DOTTED) {
|
// 压虚线
|
return CRL_SEP_DOTTED;
|
} else if (road.separate[i].lines[j][k].character == LINE_SOLID) {
|
// 压实线
|
return CRL_SEP_SOLID;
|
} else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
|
if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) {
|
return CRL_SEP_SOLID;
|
}
|
return CRL_SEP_DOTTED;
|
} else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
|
if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) {
|
return CRL_SEP_SOLID;
|
}
|
return CRL_SEP_DOTTED;
|
}
|
}
|
|
p1 = p2;
|
}
|
}
|
}
|
}
|
|
return CRL_NONE;
|
}
|
|
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2)
|
{
|
if (lane1.road == lane2.road && lane1.separate == lane2.separate && lane1.lane == lane2.lane) {
|
return true;
|
}
|
return false;
|
}
|
|
static bool LaneIsValid(struct car_on_lane lane)
|
{
|
if (lane.road >= 0 && lane.separate >= 0 && lane.lane >= 0) {
|
return true;
|
}
|
return false;
|
}
|
|
#if 0
|
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
// 起步检测
|
TestRoadStartCar(car, speed, moveDirect, rtkTime);
|
|
// 超速检测
|
if (moveDirect != 0 && speed > MAX_SPEED) {
|
if (!occurOverSpeed) {
|
occurOverSpeed = true;
|
// 超速，不合格
|
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);
|
} 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;
|
}
|
}
|
} else {
|
if (slideNormalDistance) {
|
// 后滑，扣10分
|
AddExamFault(18, rtkTime);
|
DEBUG("后滑超过10厘米, 但不超过30厘米");
|
}
|
|
slideNormalDistance = false;
|
slideLongDistance = false;
|
occurSlide = false;
|
}
|
|
switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
|
case LEFT_TURN_LIGHT:
|
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
|
currTurnSignalStatus = LEFT_TURN_LIGHT;
|
Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
|
}
|
break;
|
case RIGHT_TURN_LIGHT:
|
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
|
currTurnSignalStatus = RIGHT_TURN_LIGHT;
|
Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
|
}
|
break;
|
default:
|
currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
|
break;
|
}
|
|
// 检查是否持续转向
|
char turnDirect = CheckCarTurn(CarModelList);
|
if (turnDirect == 'L') {
|
// PlayTTS("左1");
|
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
|
if (!reportTurnSignalError) {
|
DEBUG("没打左转灯");
|
// 没打左转灯，不合格
|
reportTurnSignalError = true;
|
ReportTurnSignalError(13, rtkTime);
|
}
|
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
|
if (!reportTurnSignalError) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
reportTurnSignalError = true;
|
ReportTurnSignalError(14, rtkTime);
|
}
|
}
|
} else if (turnDirect == 'R') {
|
// PlayTTS("右1");
|
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
|
if (!reportTurnSignalError) {
|
DEBUG("没打右转灯");
|
// 没打右转灯，不合格
|
reportTurnSignalError = true;
|
ReportTurnSignalError(13, rtkTime);
|
}
|
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
|
if (!reportTurnSignalError) {
|
DEBUG("转向灯时间不足");
|
// 不足3秒，不合格
|
reportTurnSignalError = true;
|
ReportTurnSignalError(14, rtkTime);
|
}
|
}
|
} else {
|
reportTurnSignalError = false;
|
}
|
|
// 撞红线
|
if (CrashRedLine(RoadMapList, car)) {
|
if (!occurCrashRedLine) {
|
// 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
|
DEBUG("撞道路边缘线");
|
AddExamFault(11, rtkTime);
|
occurCrashRedLine = true;
|
}
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
// 撞绿线
|
static PointF p1, p2;
|
if (CrashGreenLine(RoadMapList, car, p1, p2)) {
|
// 压虚线
|
if (moveDirect != 0) {
|
if (checkCrashGreenTimeout == 0) {
|
checkCrashGreenTimeout = 1;
|
Rtk2DriveTimer(crashGreenRunTime, rtkTime); // 运动中压虚线的开始时间点
|
} else if (checkCrashGreenTimeout == 1) {
|
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
|
crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
|
|
if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
|
DEBUG("长时间压虚线");
|
checkCrashGreenTimeout = 2;
|
// 长时间骑轧车道分界线行驶，不合格
|
AddExamFault(12, rtkTime);
|
}
|
}
|
} else {
|
// 停车就不计时了
|
checkCrashGreenTimeout = 0;
|
}
|
|
// 检测当前车辆于虚线的位置，做变道检测;
|
// 检测是否3秒前有开启对应之转向灯
|
if (!occurCrashGreenLine) {
|
occurCrashGreenLine = true;
|
// 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
|
Rtk2DriveTimer(crashGreenStartTime, rtkTime);
|
turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
|
}
|
|
// p1 ---------------> p2
|
double angle = car->yaw - YawOf(p2, p1);
|
if (angle < 0 || angle > 180) {
|
// 右侧
|
carIntersectionOfGreenLine = 'R';
|
} else {
|
// 左侧
|
carIntersectionOfGreenLine = 'L';
|
}
|
} else {
|
// 不再压虚线
|
if (occurCrashGreenLine) {
|
int inter = IntersectionOfLine(p1, p2, car->basePoint);
|
|
// 完成跨线动作
|
if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
|
(inter == -1 && carIntersectionOfGreenLine == 'L')) {
|
// 比较上次跨线时间
|
if (crashGreenCmpTime.hour >= 0) {
|
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
|
rtkTime->mss * 10,
|
crashGreenCmpTime.hour, crashGreenCmpTime.min,
|
crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);
|
|
if (diff < CHANGE_ROAD_MIN_INTERVAL) {
|
DEBUG("===================== 连续变道 ============!!");
|
// 连续变道，不合格
|
AddExamFault(15, rtkTime);
|
}
|
}
|
|
// 记录本次变道时间点
|
Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
|
|
// 检查变道前，是否提前转向灯
|
if (inter == 1) {
|
// PlayTTS("左2");
|
// 向左侧变道
|
DEBUG("向左侧变道");
|
if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
ReportTurnSignalError(13, rtkTime);
|
}
|
} else {
|
// PlayTTS("右2");
|
// 向右侧变道
|
DEBUG("向右侧变道");
|
if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
|
DEBUG("变调未打灯!!");
|
// 没打灯，不合格
|
ReportTurnSignalError(14, rtkTime);
|
}
|
}
|
}
|
}
|
occurCrashGreenLine = false;
|
checkCrashGreenTimeout = 0;
|
}
|
|
// 完成起步后，触发线检测
|
if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
|
currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
|
if (currExamMapIndex != -1) {
|
DEBUG("碰撞触发线");
|
|
MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
|
|
if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
|
RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
|
StartThroughExam(currExamMapIndex, RoadMapList);
|
} else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
|
|
} else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
|
|
} else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
|
|
}
|
}
|
} else if (startCar == START_CAR_DONE) {
|
int prevIdx = currExamMapIndex;
|
|
if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
|
currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
|
CarModelList, speed, moveDirect, rtkTime);
|
}
|
else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
|
|
} else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
|
|
} else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
|
|
}
|
|
if (currExamMapIndex == -1) {
|
DEBUG("离开区域 index %d id %d type %d", prevIdx, RoadMapList[prevIdx].id, RoadMapList[prevIdx].type);
|
|
MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 0);
|
}
|
}
|
}
|
|
|
bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car)
|
{
|
bool ret = false;
|
|
if (index < 0 || index > RoadMapList.size())
|
return true;
|
|
if (RoadMapList[index].area.point != NULL) {
|
// 需要车身全部离开
|
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]];
|
}
|
|
if (IntersectionOf(&carBody, &RoadMapList[index].area) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
}
|
|
return ret;
|
}
|
|
bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
|
{
|
bool ret = false;
|
|
if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size())
|
return ret;
|
|
Polygon trace;
|
|
trace.num = 5; // 最近5个轮轨迹
|
trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
|
|
list<car_model *>::iterator iter = CarModelList.begin();
|
|
int pn = 0;
|
while (iter != CarModelList.end() && pn < trace.num) {
|
trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
|
++iter;
|
}
|
|
// 每条线都检测
|
for (int j = 0; j < RoadMapList[index].redLineNum; ++j) {
|
Line red_line;
|
|
int kp = 0;
|
|
// 触发线一般应该只有首尾2点（id, p1, p2）
|
for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) {
|
MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp],
|
&RoadMapList[index].redLine[j].point[k]);
|
|
int pp = 0;
|
for (int p = 1; p < pn; ++p) {
|
Line trace_line;
|
MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
|
|
if (IntersectionOf(trace_line, red_line) == GM_Intersection) {
|
// 碰到触发线
|
ret = true;
|
goto SEARCH_SON_RED_LINE_END;
|
}
|
|
pp = p;
|
}
|
|
kp = k;
|
}
|
}
|
|
SEARCH_SON_RED_LINE_END:
|
free(trace.point);
|
|
return ret;
|
}
|
#endif
|
|
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;
|
}
|
|
static char isTurn(int currYaw, int prevYaw, int &ang)
|
{
|
// 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);
|
}
|
|
ang = deltaAng;
|
|
// DEBUG("角度差值 %d", deltaAng);
|
|
if (deltaAng >= TURN_THRESHOLD) {
|
if((( currYaw + 360 - prevYaw) % 360) < 180) {
|
// DEBUG("右转");
|
return 'R';
|
} else {
|
// DEBUG("左转");
|
return 'L';
|
}
|
}
|
|
return 'N';
|
}
|
|
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 edge
|
* @param road
|
* @param point
|
* @return
|
*/
|
PointF GetSELine(vector<edge_t> &edge, PointF point)
|
{
|
PointF p1, p2;
|
PointF px;
|
|
vector<PointF> vps;
|
|
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)) {
|
vps.push_back(vp);
|
}
|
|
p1 = p2;
|
}
|
}
|
|
if (vps.size() == 0) {
|
if (DistanceOf(point, edge[0].points[0]) < DistanceOf(point, edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1])) {
|
px = GetVerticalPoint(edge[0].points[0], edge[0].points[1], point);
|
} else {
|
px = GetVerticalPoint(edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 2], edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1], point);
|
}
|
} else if (vps.size() == 1) {
|
px = vps[0];
|
} else {
|
px = vps[0];
|
for (int i = 1; i < vps.size(); ++i) {
|
if (DistanceOf(point, vps[i]) < DistanceOf(point, px)) {
|
px = vps[i];
|
}
|
}
|
}
|
|
return px;
|
}
|
|
/***************************************************
|
* 接近路口时，提示下一步怎么走
|
* @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(GetSELine(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
|
}
|
}
|
|
MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]);
|
|
if (CrashTheLine(triggerLine, car, CarModelList)) {
|
return &RoadMap.triggerLines[i];
|
}
|
}
|
}
|
|
return NULL;
|
}
|
|
/************************************************************************
|
* 开始新的考试后，清除地图所有的刹车、停车记录
|
* @param map
|
*/
|
static void ClearAll(road_exam_map &map)
|
{
|
for (int i = 0; i < map.roads.size(); ++i) {
|
map.roads[i].activeStop = map.roads[i].activeBreak = false;
|
map.roads[i].arrivedTail = false;
|
map.roads[i].errorLane = false;
|
}
|
for (int i = 0; i < map.specialAreas.size(); i++) {
|
map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
|
}
|
}
|
|
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
|
double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
int BreakDone = ReadCarStatus(BREAK);
|
|
// 计算车前进轨迹延长线
|
double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
|
PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw);
|
|
Line extLine;
|
MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
|
|
// 路口刹车点
|
for (int i = 0; i < map.roads.size(); ++i) {
|
// 车头和路口距离不足30米
|
if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection &&
|
IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) {
|
DEBUG("进入减速区");
|
if (BreakDone == BREAK_ACTIVE) {
|
map.roads[i].activeBreak = true;
|
}
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
|
map.roads[i].stopLine) < DISTANCE_STOP_CAR_TO_STOP_LINE && moveDirect == 0) {
|
map.roads[i].activeStop = true;
|
}
|
}
|
// 跨线后，检查刹车动作
|
if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
|
if (map.roads[i].stopFlag != 0 && !map.roads[i].activeStop) {
|
// 不停车瞭望，不合格
|
DEBUG("不停车瞭望");
|
if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
|
AddExamFault(42, rtkTime);
|
} else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
|
AddExamFault(44, rtkTime);
|
} else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
|
AddExamFault(47, rtkTime);
|
}
|
}
|
if (!map.roads[i].activeBreak) {
|
// 不按规定减速，不合格
|
DEBUG("不按规定减速");
|
if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
|
AddExamFault(41, rtkTime);
|
} else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
|
AddExamFault(43, rtkTime);
|
} else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
|
AddExamFault(46, rtkTime);
|
}
|
}
|
}
|
}
|
// 人行道、公交站刹车点；学校限速区
|
for (int i = 0; i < map.specialAreas.size(); i++) {
|
if (map.specialAreas[i].type == GRID_AREA)
|
continue;
|
|
if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
|
// 计算点到左侧路边线的垂点
|
int road = 0;
|
for (road = 0; road < map.roads.size(); ++road) {
|
if (map.roads[road].id == map.specialAreas[i].road)
|
break;
|
}
|
|
PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
|
DEBUG("计算垂点1 (%f, %f)", vPoint.X, vPoint.Y);
|
|
map.specialAreas[i].leftPoints.push_back(vPoint);
|
|
vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
|
DEBUG("计算垂点2 (%f, %f)", vPoint.X, vPoint.Y);
|
map.specialAreas[i].leftPoints.push_back(vPoint);
|
}
|
|
if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
|
Line startLine;
|
|
MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);
|
|
// 车头和斑马线距离不足30米
|
if (IntersectionOf(extLine, startLine) == GM_Intersection &&
|
IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
|
DEBUG("进入减速区 %d", map.specialAreas[i].type);
|
if (BreakDone == BREAK_ACTIVE) {
|
map.specialAreas[i].activeBreak = true;
|
}
|
}
|
|
// 跨线后，检查刹车动作
|
if (CrashTheLine(startLine, car, CarModelList)) {
|
if (!map.specialAreas[i].activeBreak) {
|
// 不按规定减速，不合格
|
DEBUG("不按规定减速");
|
if (map.specialAreas[i].type == ZEBRA_CROSSING) {
|
AddExamFault(48, rtkTime);
|
} else {
|
AddExamFault(50, rtkTime);
|
}
|
} else {
|
DEBUG("按规定减速");
|
}
|
|
}
|
} else if (map.specialAreas[i].type == SCHOOL_AREA) {
|
Polygon school;
|
|
school.num = 4;
|
school.point = (PointF *) malloc(school.num * sizeof(PointF));
|
|
school.point[0] = map.specialAreas[i].area[0];
|
school.point[1] = map.specialAreas[i].area[1];
|
school.point[2] = map.specialAreas[i].leftPoints[1];
|
school.point[3] = map.specialAreas[i].leftPoints[0];
|
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
|
if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
|
if (!map.specialAreas[i].overSpeed) {
|
DEBUG("通过学校区域超速");
|
AddExamFault(49, rtkTime);
|
map.specialAreas[i].overSpeed = true;
|
}
|
}
|
}
|
|
free(school.point);
|
}
|
}
|
}
|
|
#if 0
|
|
typedef struct {
|
int road;
|
int segment;
|
int lane;
|
} CarOnTrackInfo_t;
|
|
static CarOnTrackInfo_t CarOnTrackInfo;
|
|
void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
// 检测车道变更
|
CarOnTrackInfo_t newCarOnTrackInfo;
|
|
if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
|
newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
|
newCarOnTrackInfo.lane != CarOnTrackInfo.lane) {
|
|
}
|
|
CarOnTrackInfo = newCarOnTrackInfo;
|
|
|
}
|
|
/**************************************************
|
* 车辆当前所在路段，车头需越过起始线一定距离
|
* @param currIndex
|
* @param map
|
* @param car
|
*/
|
static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
|
{
|
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]];
|
}
|
|
Polygon roadArea;
|
roadArea.num = 0;
|
roadArea.point = NULL;
|
|
for (int i = 0; i < map.roads.size(); ++i) {
|
if (roadArea.point != NULL) {
|
free(roadArea.point);
|
}
|
|
for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
|
roadArea.num += map.roads[i].leftEdge[x].points.size();
|
}
|
for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
|
roadArea.num += map.roads[i].rightEdge[x].points.size();
|
}
|
roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));
|
|
int n = 0;
|
for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
|
for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
|
if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
|
isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
|
// 第一个点已经存在于上一条线
|
} else {
|
roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
|
}
|
}
|
}
|
for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
|
for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
|
if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
|
isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
|
isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
|
// 第一个点已经存在于上一条线
|
} else {
|
roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
|
}
|
}
|
}
|
roadArea.num = n;
|
|
if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
|
currIndex = i;
|
goto CHECK_CAR_ON_ROAD_END;
|
}
|
}
|
currIndex = INVALID_ROAD;
|
|
CHECK_CAR_ON_ROAD_END:
|
if (roadArea.point != NULL) {
|
free(roadArea.point);
|
}
|
free(carBody.point);
|
}
|
|
/******************************************************
|
* 全车都需离开这个区域
|
* @param currIndex
|
* @param map
|
* @param car
|
* @return
|
*/
|
static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
|
{
|
// 全车都需不在地图中
|
bool ret = false;
|
|
if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
|
return ret;
|
|
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]];
|
}
|
|
if (IntersectionOf(&carBody, &map.roads[currIndex].area) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
return ret;
|
}
|
|
static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
|
{
|
Line frontAxle, rearAxle;
|
|
if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
|
return false;
|
|
MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
// 分段
|
for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
|
// 分段中的每条线
|
for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
|
Line theLine;
|
int p1 = 0;
|
for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
|
MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
|
if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
|
IntersectionOf(theLine, rearAxle) == GM_Intersection) {
|
return true;
|
}
|
p1 = p2;
|
}
|
}
|
}
|
return false;
|
}
|
|
|
static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
|
{
|
int segment;
|
int lane = -1;
|
|
CarOnTrackInfo_t newInfo;
|
|
if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
|
return;
|
|
// 遍历每一分段
|
for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
|
int separate_line_num = map.roads[currIndex].separate[i].lines.size();
|
|
struct vrecord_ {
|
int valid;
|
int p1;
|
int p2;
|
};
|
|
vector<struct vrecord_> vrecord;
|
vrecord.clear();
|
|
bool match1 = false;
|
// 遍历当前分段的每一条线
|
for (int j = 0; j < separate_line_num; ++j) {
|
Line theLine;
|
int p1 = 0;
|
|
struct vrecord_ v;
|
v.valid = 0;
|
|
// 单独的一条虚线
|
for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) {
|
MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1],
|
&map.roads[currIndex].separate[i].lines[j].point[p2]);
|
if (VerticalPointOnLine(car->basePoint, theLine)) {
|
v.valid = 1;
|
v.p1 = p1;
|
v.p2 = p2;
|
match1 = true;
|
break;
|
}
|
p1 = p2;
|
}
|
|
vrecord.push_back(v);
|
|
lane = separate_line_num;//
|
}
|
|
|
|
if (match1) {
|
for (int x = 0; x < vrecord.size(); ++x) {
|
if (vrecord[i].valid == 0) {
|
// 首尾两段线的延申必有一个垂点
|
if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) <
|
DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) {
|
vrecord[x].p1 = 0;
|
vrecord[x].p2 = 1;
|
} else {
|
vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2;
|
vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].num - 1;
|
}
|
}
|
|
int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
|
map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
|
car->basePoint);
|
if (rel != -1) {
|
newInfo.road = currIndex;
|
newInfo.segment = i;
|
newInfo.lane = x;
|
break;
|
}
|
}
|
|
newInfo.road = currIndex;
|
newInfo.segment = i;
|
newInfo.lane = vrecord.size();
|
|
break;
|
}
|
}
|
}
|
|
|
|
#endif
|
