490 lines
15 KiB
C++
490 lines
15 KiB
C++
#include "Sub3Judge12Lkdt.h"
|
||
#include "HFactory.h"
|
||
|
||
Sub3Judge12Lkdt::Sub3Judge12Lkdt(TKM3Item* exam)
|
||
{
|
||
m_exam = exam;
|
||
}
|
||
|
||
Sub3Judge12Lkdt::~Sub3Judge12Lkdt()
|
||
{
|
||
m_exam = nullptr;
|
||
}
|
||
|
||
bool Sub3Judge12Lkdt::dealJudgeEnter()
|
||
{
|
||
if(m_exam->TestPro != ItemProFlagInit) return false;
|
||
// if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1)) return false;
|
||
|
||
const TChuanGan* cg = m_car->historyChuanGan();
|
||
const TRTKResult& RTKKM3 = cg->RTKKM3;
|
||
|
||
m_itemv = TSub3Item12Lkdt();
|
||
|
||
m_itemv.Enter_TK = cg->tkCnt;
|
||
m_itemv.LuK_Zx_Deg = m_exam->SetUp8 != "" ? std::atof(m_exam->SetUp8.c_str()) : 25;
|
||
|
||
//m_itemv.ItemPosI = xi;
|
||
m_itemv.Start_JL_CM = cg->ai_ljjl_cm;
|
||
|
||
const std::vector<std::string>& s508 = TableSysSet->asArray508();
|
||
//得到项目距离(如果没有读到停止线或者没有感应到出路口，行驶指定距离后，自动结束项目 )
|
||
m_itemv.XMJL_Mi = s508.size() > 0 && s508[0] != "" ? std::atoi(s508[0].c_str()) : 250;
|
||
//3、得到行驶方向点和停车让行标志
|
||
if(m_exam->SetUp3 != "")
|
||
{
|
||
std::vector<std::string> setup3 = Tools::split(m_exam->SetUp3, "-");
|
||
m_itemv.LuK_Fx_PointNo = setup3.size() > 0 && setup3[0] != "" ? std::atoi(setup3[0].c_str()) : 0;
|
||
}
|
||
else
|
||
{
|
||
m_itemv.LuK_Fx_PointNo = 0;
|
||
}
|
||
|
||
std::string roadData = cg->MapPoint_Road_Code;
|
||
if(roadData == "")
|
||
{
|
||
roadData = RTKKM3.MapRoad_Name;
|
||
}
|
||
roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
|
||
m_itemv.CSLuDuan = roadData;
|
||
|
||
m_exam->TestPro = ItemProFlagJudge;
|
||
//ToDo1:播报项目语音
|
||
//ToDo2:生成进项目事件
|
||
return true;
|
||
}
|
||
|
||
void Sub3Judge12Lkdt::dealJudgeItem()
|
||
{
|
||
HELP_COST_TIME("");
|
||
if(m_exam->TestPro != ItemProFlagJudge) return;
|
||
if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1)) return;
|
||
|
||
//setup1: 路口类型 1-平交 2-环道 3-三叉 缺省 1－平交
|
||
//setup2: 限速值，通过路口停车线时的车速 缺省 35
|
||
//setup3: 正确行驶方向点-方向-停车让行
|
||
//setup4: *语音提示方向，保存语音文件名 缺省 空
|
||
//setup5: 路口内禁止停车时间 缺省 10000秒 标识不限制
|
||
//setup6: 越过停车线后X米后，不允许停车超过setup5秒 缺省 10米
|
||
//setup7: 陀螺仪变化角度阀值，禁止变道
|
||
//setup8: 陀螺仪变化角度阀值，转向方向灯 左转+ -250 -220 右转- -220 -250 100 70
|
||
|
||
const TChuanGan* cg = m_car->historyChuanGan();
|
||
const TRTKResult& RTKKM3 = cg->RTKKM3;
|
||
const TGpsInfo& gps = cg->real.gps;
|
||
const TSensorInfo& sor = cg->real.sensor;
|
||
const TChuanGan* his1 = m_car->historyChuanGan(1);
|
||
const std::string& ksdd = TableSysSet->get211();
|
||
|
||
if(m_car->isExamMode()) //正式考试
|
||
{
|
||
if(!m_car->isQualified())
|
||
{
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
return;
|
||
}
|
||
}
|
||
|
||
|
||
double MaxAngleChange = 0;
|
||
for(int i = 1; i < 500; i++)
|
||
{
|
||
const TChuanGan* hisi = m_car->historyChuanGan(i);
|
||
if(hisi == nullptr) break;
|
||
|
||
const TGpsInfo& gpsi = hisi->real.gps;
|
||
|
||
if(!gpsi.rtkEnabled) break;
|
||
if (hisi->tkCnt <= m_itemv.Enter_TK) break;
|
||
double a = gps.hxj - gpsi.hxj;
|
||
//处理越界问题
|
||
if(a > 300)
|
||
{
|
||
a = a - 360;
|
||
}
|
||
else
|
||
{
|
||
if(a < -300)
|
||
{
|
||
a = a + 360;
|
||
}
|
||
}
|
||
|
||
a = std::abs(a);
|
||
if(Tools::greater(a, MaxAngleChange))
|
||
{
|
||
MaxAngleChange = a;
|
||
}
|
||
|
||
if(a > m_itemv.LuK_Zx_Deg && a < 210) //20180830 -150
|
||
{
|
||
m_itemv.LuK_FXD_AngleOKFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
Update_DiaoTou_JianSuRec(m_itemv.slowDown); //, -1
|
||
|
||
//(*2024-03-08*)
|
||
if(Tools::pos("+", cg->MapPoint_Road_Code) == true && Tools::pos("+", his1->MapPoint_Road_Code) == false) //包含路口端点的路口项目
|
||
{
|
||
if(m_exam->Gps_Itemno1 < 3)
|
||
{
|
||
m_exam->Gps_Itemno1 = 3;
|
||
}
|
||
}
|
||
|
||
if(m_exam->Gps_Itemno1 == 3 || m_itemv.Cross_TZX_Flag == true)
|
||
{
|
||
if(m_itemv.Step3Flag == false)
|
||
{
|
||
m_itemv.Step3Flag = true;
|
||
std::string roadData = cg->MapPoint_Road_Code;
|
||
if(roadData == "")
|
||
{
|
||
//这里要稍微讲解一下(有些地方，可能驾校在路段中间，这种情况下，初始读不到路段，这种情况比较少)
|
||
roadData = RTKKM3.MapRoad_Name;
|
||
}
|
||
roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
|
||
m_itemv.Step3RoadData = roadData;
|
||
}
|
||
|
||
std::string roadData = cg->MapPoint_Road_Code;
|
||
if(roadData == "")
|
||
{
|
||
roadData = RTKKM3.MapRoad_Name;
|
||
}
|
||
roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
|
||
if(roadData != "")
|
||
{
|
||
std::vector<std::string> s = Tools::split(roadData, "-");
|
||
//TempRoadData := GetDotStr(2, TempRoadData + '-', '-') + '-' + GetDotStr(1, TempRoadData + '-', '-');
|
||
roadData = std::string(s.size() > 1 ? s[1] : "") + "-" + std::string(s.size() > 0 ? s[0] : "");
|
||
}
|
||
|
||
if(m_itemv.CSLuDuan == roadData)
|
||
{
|
||
m_itemv.OKFangXiangPt_SuccessFlag = true;
|
||
m_itemv.ReadNextLuDuanFlag = true;
|
||
if(sor.zfxd == SNOT && sor.yfxd == SNOT)
|
||
{
|
||
TTestCtl* ctl = m_car->getTTestCtl();
|
||
ctl->ZXD_GuanBi = true;
|
||
}
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
return;
|
||
}
|
||
}
|
||
|
||
//读到下一个路段标志
|
||
if(m_itemv.Step3Flag == true)
|
||
{
|
||
std::string roadData = cg->MapPoint_Road_Code;
|
||
if(roadData == "")
|
||
{
|
||
roadData = RTKKM3.MapRoad_Name;
|
||
}
|
||
roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
|
||
if(m_itemv.Step3RoadData != roadData)
|
||
{
|
||
m_itemv.ReadNextLuDuanFlag = true;
|
||
}
|
||
|
||
//做完项目后，有没有关闭灯光(综合评判会用到，这里先赋值)
|
||
if(sor.zfxd == SNOT && sor.yfxd == SNOT)
|
||
{
|
||
TTestCtl* ctl = m_car->getTTestCtl();
|
||
ctl->ZXD_GuanBi = true;
|
||
}
|
||
}
|
||
|
||
//如果设置了路口掉头的方向灯立刻判（打错立刻判）
|
||
//const std::vector<std::string>& s508 = TableSysSet->asArray508();
|
||
//if(s508.size() > 2 && std::atoi(s508[2].c_str()) == 1)
|
||
//{
|
||
// const TSensorInfo& sor1 = m_car->historySensor(1);
|
||
// const TSensorInfo& sor2 = m_car->historySensor(2);
|
||
// if(sor.yfxd == SYES && sor1.yfxd == SYES && sor2.yfxd == SYES)
|
||
// {
|
||
// if(!m_itemv.Mark_12_42_Flag)
|
||
// {
|
||
// JUDGE_MARK_SUB3(12, "42", false);
|
||
// }
|
||
// }
|
||
//}
|
||
|
||
//方向灯评判
|
||
//方向灯标志
|
||
if(sor.zfxd == SYES && sor.yfxd == SNOT)
|
||
{
|
||
m_itemv.Temp_ZFXD = true;
|
||
}
|
||
|
||
std::string msg = "";
|
||
if(m_itemv.Cross_TZX_Flag == true)
|
||
{
|
||
if(m_itemv.StopLine_JLCM != 0)
|
||
{
|
||
//环岛业务逻辑翻译（一般很少有地方是环岛） //路口类型 1-平交 2-环岛
|
||
int disMI = m_itemv.LuK_Type == 2 ? 190 : 100;
|
||
int dis1 = cg->ai_ljjl_cm - m_itemv.StopLine_JLCM;
|
||
int dis2 = disMI * 100;
|
||
msg = JUDGE_UTF8S(",过停止线距离cm=") + std::to_string(dis1) + JUDGE_UTF8S(",结束=") + std::to_string(dis2);
|
||
if(cg->ai_ljjl_cm - m_itemv.StopLine_JLCM >= dis2)
|
||
{
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
// (*2024-03-07*)
|
||
//增加南京地区特殊判断
|
||
if(ksdd == siteof::nj && m_exam->Gps_Itemno1 == 3 && TableSysSet->get346() == "-1")
|
||
{
|
||
if(!m_itemv.Cross_TZX_Flag)
|
||
{
|
||
m_itemv.Cross_TZX_Flag = true;
|
||
//0-未越线 1-停车线 2-中心点右侧线 3-等待控制线 20150119
|
||
m_itemv.StopLine_JLCM = cg->ai_ljjl_cm; //停止线时刻的距离
|
||
}
|
||
}
|
||
|
||
//判断是否到达停止线
|
||
//0-未越线 1-停车线 2-中心点右侧线 3-等待控制线 20150119
|
||
if(m_itemv.Cross_TZX_Flag == false)
|
||
{
|
||
if(RTKKM3.CrossLineAttr == CrossLineAttr_1)
|
||
{
|
||
m_itemv.Cross_TZX_Flag = true;
|
||
|
||
if(m_exam->Gps_Itemno1 < 3)
|
||
{
|
||
m_exam->Gps_Itemno1 = 3;
|
||
}
|
||
// (*2024-03-07*)
|
||
//0-未越线 1-停车线 2-中心点右侧线 3-等待控制线 20150119
|
||
m_itemv.StopLine_JLCM = cg->ai_ljjl_cm; //停止线时刻的距离
|
||
//路段状况:基准天线所在车道方向: 1-右转 2-直行 3-左转 4-掉头
|
||
//1000 0100 0010 0001
|
||
//评判方向灯
|
||
NS3JudgeVision_41501(true);
|
||
}
|
||
}
|
||
|
||
if(m_itemv.Cross_TZX_Flag == true || m_itemv.LuK_FXD_AngleOKFlag == true)
|
||
{
|
||
if(m_itemv.Judge_TCX == false)
|
||
{
|
||
m_itemv.Judge_TCX = true;
|
||
//方向灯评判
|
||
JudgeFXD();
|
||
if(m_itemv.Cross_TZX_Flag == true && m_exam->EnterItemFromLuKou5)
|
||
{
|
||
//车头穿越时评判，没有停止控制线不判
|
||
Judge_LuKou_TZX_Night(itemNo());
|
||
}
|
||
}
|
||
}
|
||
|
||
//检查车道是否正确
|
||
if(RTKKM3.CrossLineAttr == CrossLineAttr_1 || m_exam->Gps_Itemno1 == 2 || m_exam->Gps_Itemno1 == 3)
|
||
{
|
||
#if JUDGE_USE_INSPECT
|
||
if(1)
|
||
{
|
||
//路段状况:基准天线所在车道方向: 1-右转 2-直行 3-左转 4-掉头
|
||
//1000 0100 0010 0001
|
||
if(forbidInLane(RTKKM3.BasePointInLaneDir, TURN_D))
|
||
{
|
||
JUDGE_MARK_SUB3(12, "45", true);
|
||
}
|
||
}
|
||
#else
|
||
//if(m_itemv.Check_CheDao == false)
|
||
{
|
||
//m_itemv.Check_CheDao = true;
|
||
//路段状况:基准天线所在车道方向: 1-右转 2-直行 3-左转 4-掉头
|
||
//1000 0100 0010 0001
|
||
if(forbidInLane(RTKKM3.BasePointInLaneDir, TURN_D))
|
||
{
|
||
JUDGE_MARK_SUB3(12, "45", true);
|
||
}
|
||
}
|
||
|
||
#endif
|
||
}
|
||
|
||
//不按规定考试（比如：走错车道了
|
||
//方向灯标志
|
||
|
||
if(m_itemv.Cross_TZX_Flag == false)
|
||
{
|
||
if(cg->ai_ljjl_cm - m_itemv.Start_JL_CM > m_itemv.XMJL_Mi * 100)
|
||
{
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
return;
|
||
}
|
||
}
|
||
|
||
if(m_itemv.Step3Flag == true)
|
||
{
|
||
//插入穿越点
|
||
int ptNo = m_car->lastCrossPtNo();
|
||
if(ptNo != 0)
|
||
{
|
||
bool findFlag = false;
|
||
int len = m_itemv.Step3PtArr.size();
|
||
for(int i = 0; i < len; i++)
|
||
{
|
||
if(m_itemv.Step3PtArr[i] == ptNo)
|
||
{
|
||
findFlag = true;
|
||
}
|
||
}
|
||
if(!findFlag)
|
||
{
|
||
m_itemv.Step3PtArr.push_back(ptNo);
|
||
}
|
||
|
||
findFlag = false;
|
||
len = m_itemv.Step3PtArr.size();
|
||
for(int i = 0; i < len; i++)
|
||
{
|
||
if(m_itemv.Step3PtArr[i] == m_itemv.LuK_Fx_PointNo)
|
||
{
|
||
findFlag = true;
|
||
}
|
||
}
|
||
if(!m_itemv.OKFangXiangPt_SuccessFlag)
|
||
{
|
||
m_itemv.OKFangXiangPt_SuccessFlag = findFlag;
|
||
}
|
||
}
|
||
std::string roadData = cg->MapPoint_Road_Code;
|
||
if(roadData == "")
|
||
{
|
||
roadData = RTKKM3.MapRoad_Name;
|
||
}
|
||
roadData = Tools::replace(roadData, "+", ""); // [rfReplaceAll]
|
||
if(roadData != "")
|
||
{
|
||
//如果路段发生变化了(如果持续3帧不同，可以认为路口项目结束了
|
||
if(roadData != m_itemv.Step3RoadData)
|
||
{
|
||
bool OKFlag = true;
|
||
for(int i = 1; i <= 2; i++)
|
||
{
|
||
TChuanGan* cgi = m_car->historyChuanGan(i);
|
||
roadData = cgi->MapPoint_Road_Code;
|
||
if(roadData == "")
|
||
{
|
||
roadData = cgi->RTKKM3.MapRoad_Name;
|
||
}
|
||
roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
|
||
if(roadData == m_itemv.Step3RoadData)
|
||
{
|
||
OKFlag = false;
|
||
break;
|
||
}
|
||
}
|
||
if(OKFlag == true)
|
||
{
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//ToDo:贵州地区的信号灯评判
|
||
if(ksdd == siteof::guizhoubj)
|
||
{
|
||
//ToDo:贵州地区的红绿灯通信数据
|
||
}
|
||
//ToDo: 等待控制线，需要结合红绿灯信号 （暂时不实现)
|
||
//0-未越线 1-停车线 2-中心点右侧线 3-等待控制线 20150119
|
||
//RTKKM3.CrossLineAttr = 3 。
|
||
////////////////////////////////////////////////////////////////////////////
|
||
//ToDo:暂时屏蔽观察类算法，后期实现
|
||
|
||
//状态提示
|
||
std::string str = "";
|
||
std::vector<std::string> xhd = Tools::split(sor.xhd, ",");
|
||
if(ksdd == siteof::nmgcfkm3 && xhd.size() > 0 && xhd[0] == m_exam->ItemSNO)
|
||
{
|
||
char buf[128] = {0};
|
||
SafeSprintf(buf, sizeof(buf), JUDGE_UTF8S("%s;St=%d;Cs=%0.2f,,角度变化=%0.1f,xhd=%s"),
|
||
m_exam->ItemSNO.c_str(), m_exam->Gps_Itemno1, gps.sd, MaxAngleChange, sor.xhd.c_str());
|
||
str += buf;
|
||
}
|
||
else
|
||
{
|
||
char buf[128] = {0};
|
||
SafeSprintf(buf, sizeof(buf), JUDGE_UTF8S("%s;St=%d;Cs=%0.2f,角度变化=%0.1f"),
|
||
m_exam->ItemSNO.c_str(), m_exam->Gps_Itemno1, MaxAngleChange, gps.sd);
|
||
str += buf;
|
||
}
|
||
|
||
if(m_exam->TestPro == ItemProFlagEnd)
|
||
{
|
||
if(m_itemv.LuK_Fx_PointNo > 0)
|
||
{
|
||
if(m_itemv.OKFangXiangPt_SuccessFlag == false && m_itemv.ReadNextLuDuanFlag == true)
|
||
{
|
||
JUDGE_MARK_SUB3(12, "41", true);
|
||
}
|
||
}
|
||
if(m_itemv.slowDown.Js_Passed == false)
|
||
{
|
||
//对可能出现危险的情形未采取减速、鸣喇叭等安全措施
|
||
JUDGE_MARK_SUB3(12, "46", false); //20170505
|
||
}
|
||
}
|
||
|
||
str += msg;
|
||
showStatus(JUDGE_UTF8S("[大路口掉头]") + str);
|
||
|
||
NS3JudgeVision_30107(m_itemv.green, TVisionTarg::XD_D);
|
||
NS3JudgeVision_41501(false);
|
||
}
|
||
|
||
void Sub3Judge12Lkdt::JudgeFXD()
|
||
{
|
||
//掉头
|
||
if(m_itemv.JudgeFXD_Flag == false)
|
||
{
|
||
m_itemv.JudgeFXD_Flag = true;
|
||
if(m_itemv.Temp_ZFXD == false)
|
||
{
|
||
JUDGE_MARK_SUB3(12, "42", false);
|
||
}
|
||
}
|
||
}
|
||
|
||
void Sub3Judge12Lkdt::NS3JudgeVision_41501(bool opportunity)
|
||
{
|
||
//不观察左、右方交通情况，转弯通过路口时，未观察侧前方交通情况,
|
||
//通过停止线前10s内，头部姿态没有大于左侧角度【18度】或大于右侧角度【20度】
|
||
if(!NS3UsingAndData(NS3UsingPose)) return;
|
||
|
||
int64 nowTime = m_car->GetCurrentTime2();
|
||
const TNS3PoseSysset& poseSys = m_nsub3->getPoseSysset();
|
||
const TVisionPose* pose0 = m_nsub3->hisPose(0);
|
||
if(pose0->zy < -poseSys.left || pose0->zy > poseSys.right)
|
||
{
|
||
m_itemv.observe.obsTime = nowTime;
|
||
}
|
||
|
||
if(opportunity && !m_itemv.observe.isJudge)
|
||
{
|
||
m_itemv.observe.isJudge = true;
|
||
if(nowTime - m_itemv.observe.obsTime > 10*SECOND)
|
||
{
|
||
JUDGE_MARK_NSUB3(12, "01", false);
|
||
//NS3video
|
||
}
|
||
}
|
||
}
|