|
@@ -4,8 +4,10 @@
|
|
|
#include <iostream>
|
|
|
#include <memory>
|
|
|
|
|
|
+#ifndef NOTINPILOT
|
|
|
extern iv::Ivlog *givlog;
|
|
|
extern iv::Ivfault *gfault;
|
|
|
+#endif
|
|
|
|
|
|
namespace iv {
|
|
|
struct xodrtreeunit
|
|
@@ -136,7 +138,9 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
|
{
|
|
|
if(px->GetLaneSection(j)->GetLaneCount()<2)
|
|
|
{
|
|
|
+#ifndef NOTINPILOT
|
|
|
givlog->info("road %s lanesection %d: no lane",px->GetRoadId().data(),j);
|
|
|
+#endif
|
|
|
continue;
|
|
|
}
|
|
|
|
|
@@ -283,8 +287,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
|
// road_to,pjll->GetTo(),fromedge,toedge,pjc->GetContactPoint().data());
|
|
|
if((fromedge == -1)||(toedge == -1))
|
|
|
{
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("from %d %d to %d %d fromedge %d toedge %d %s",road_from, pjll->GetFrom(),
|
|
|
road_to,pjll->GetTo(),fromedge,toedge,pjc->GetContactPoint().data());
|
|
|
+ #endif
|
|
|
continue;
|
|
|
}
|
|
|
|
|
@@ -434,7 +440,9 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("After Junction vertex same is %d ",xvertexsame.size());
|
|
|
+ #endif
|
|
|
|
|
|
|
|
|
|
|
@@ -1017,12 +1025,16 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
|
|
|
|
}
|
|
|
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("vertex same is %d ",xvertexsame.size());
|
|
|
+ #endif
|
|
|
|
|
|
for(i=0;i<xvertexsame.size();i++)
|
|
|
{
|
|
|
qDebug("%d: %d %d",i,xvertexsame[i].ma,xvertexsame[i].mb);
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("%d: %d %d",i,xvertexsame[i].ma,xvertexsame[i].mb);
|
|
|
+ #endif
|
|
|
}
|
|
|
|
|
|
|
|
@@ -1136,7 +1148,9 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
|
{
|
|
|
qDebug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
|
|
|
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
|
|
|
+ #endif
|
|
|
}
|
|
|
|
|
|
|
|
@@ -1218,7 +1232,9 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
|
for(i=0;i<mroadedge.size();i++)
|
|
|
{
|
|
|
qDebug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
|
|
|
+ #endif
|
|
|
}
|
|
|
|
|
|
|
|
@@ -1390,8 +1406,11 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
|
|
|
// int dstvertex = mroadedge[dstedge].mvertexend;
|
|
|
int dstvertex = mroadedge[dstedge].mvertexstart;
|
|
|
if(srcedge == dstedge)dstvertex = mroadedge[dstedge].mvertexstart;
|
|
|
+
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("src edge is %d vertex is %d",srcedge,vs);
|
|
|
givlog->debug("dst vertex is %d ",dstvertex);
|
|
|
+ #endif
|
|
|
|
|
|
// 初始化
|
|
|
for (i = 0; i < nvertexnum; i++)
|
|
@@ -1431,7 +1450,9 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
|
|
|
if(k == -1)
|
|
|
{
|
|
|
qDebug("i = %d not found k",i);
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("i = %d not found k",i);
|
|
|
+ #endif
|
|
|
break;
|
|
|
}
|
|
|
// 标记"顶点k"为已经获取到最短路径
|
|
@@ -1484,7 +1505,9 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
|
|
|
int nedge = getroadedgefromvertex(nstart,nend);
|
|
|
if(nedge<0)
|
|
|
{
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->error("prev path error.");
|
|
|
+ #endif
|
|
|
break;
|
|
|
}
|
|
|
rtnpath.push_back(nedge);
|
|
@@ -1506,8 +1529,10 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
|
|
|
|
|
|
for(i=0;i<rtnpath.size();i++)
|
|
|
{
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
|
|
|
mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
|
|
|
+ #endif
|
|
|
// qDebug("%d %d %d %f",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
|
|
|
// mroadedge[rtnpath[rtnpath.size()-1-i]].mlen);
|
|
|
rtnpath2.push_back(rtnpath[rtnpath.size()-1-i]);
|
|
@@ -1515,7 +1540,9 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
|
|
|
|
|
|
/* if(srcedge == dstedge)*/rtnpath2.push_back(dstedge);
|
|
|
|
|
|
+ #ifndef NOTINPILOT
|
|
|
givlog->debug("rt = %d ",rtnpath.size());
|
|
|
+ #endif
|
|
|
//gfault->SetFaultState(0,0,"ok");
|
|
|
|
|
|
return rtnpath2;
|