|
|
@@ -11,6 +11,8 @@
|
|
|
|
|
|
#include "gpsimu.pb.h"
|
|
|
|
|
|
+#include "v2x.pb.h"
|
|
|
+
|
|
|
#include "modulecomm.h"
|
|
|
|
|
|
#include "xmlparam.h"
|
|
|
@@ -36,6 +38,7 @@ void * gpasrc;
|
|
|
void * gpmap;
|
|
|
void * gpagps;
|
|
|
void * gpasimple;
|
|
|
+void * gpav2x;
|
|
|
|
|
|
iv::Ivfault *gfault = nullptr;
|
|
|
iv::Ivlog *givlog = nullptr;
|
|
|
@@ -408,6 +411,8 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
|
|
|
|
|
|
ShareMap(mapdata);
|
|
|
}
|
|
|
+
|
|
|
+
|
|
|
void SetPlan(xodrobj xo)
|
|
|
{
|
|
|
|
|
|
@@ -482,6 +487,111 @@ void SetPlan(xodrobj xo)
|
|
|
s = 1;
|
|
|
}
|
|
|
|
|
|
+void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
|
|
|
+{
|
|
|
+
|
|
|
+ std::vector<PlanPoint> xPlan;
|
|
|
+
|
|
|
+ int i;
|
|
|
+
|
|
|
+ double fLastHdg = 0;
|
|
|
+
|
|
|
+ int ndeflane =nlane;
|
|
|
+
|
|
|
+ for(i=0;i<pxv2x->stgps_size();i++)
|
|
|
+ {
|
|
|
+
|
|
|
+ double x_src,y_src,x_dst,y_dst;
|
|
|
+
|
|
|
+ if(i==0)
|
|
|
+ {
|
|
|
+ CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
|
|
|
+ }
|
|
|
+ CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);
|
|
|
+
|
|
|
+
|
|
|
+ std::vector<PlanPoint> xPlanPart;
|
|
|
+
|
|
|
+ double s;
|
|
|
+
|
|
|
+ // x_src = -5;y_src = 6;
|
|
|
+ // x_dst = -60;y_src = -150;
|
|
|
+ int nRtn = -1;
|
|
|
+ if(i==0)
|
|
|
+ {
|
|
|
+ nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(nRtn < 0)
|
|
|
+ {
|
|
|
+ qDebug("plan fail.");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ int j;
|
|
|
+ for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
|
|
|
+ fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ int nSize = xPlan.size();
|
|
|
+
|
|
|
+ std::vector<iv::GPSData> mapdata;
|
|
|
+
|
|
|
+ QFile xfile;
|
|
|
+ QString strpath;
|
|
|
+ strpath = getenv("HOME");
|
|
|
+ strpath = strpath + "/map/maptrace.txt";
|
|
|
+ xfile.setFileName(strpath);
|
|
|
+ xfile.open(QIODevice::ReadWrite);
|
|
|
+
|
|
|
+ for(i=0;i<nSize;i++)
|
|
|
+ {
|
|
|
+ iv::GPSData data(new iv::GPS_INS);
|
|
|
+ CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
|
|
|
+ data->gps_lng,data->ins_heading_angle);
|
|
|
+ data->index = i;
|
|
|
+ data->speed = xPlan[i].speed;
|
|
|
+ // ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
|
|
|
+ GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
|
|
|
+
|
|
|
+ givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
|
|
|
+ data->gps_lng,data->ins_heading_angle);
|
|
|
+
|
|
|
+ data->roadSum = 1;
|
|
|
+ data->roadMode = 0;
|
|
|
+ data->roadOri = 0;
|
|
|
+
|
|
|
+ if(xPlan[i].lanmp == -1)data->roadMode = 15;
|
|
|
+ if(xPlan[i].lanmp == 1)data->roadMode = 14;
|
|
|
+
|
|
|
+ mapdata.push_back(data);
|
|
|
+
|
|
|
+ char strline[255];
|
|
|
+ snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
|
|
|
+ i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
|
|
|
+ xfile.write(strline);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ xfile.close();
|
|
|
+
|
|
|
+ ShareMap(mapdata);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
{
|
|
|
|
|
|
@@ -505,6 +615,25 @@ void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int
|
|
|
|
|
|
}
|
|
|
|
|
|
+void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+ iv::v2x::v2x xv2x;
|
|
|
+ if(!xv2x.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+ givlog->warn("ListernV2X Parse Error.");
|
|
|
+ std::cout<<"ListenV2X Parse Error."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(xv2x.stgps_size()<1)
|
|
|
+ {
|
|
|
+ givlog->debug("ListenV2X no gps station.");
|
|
|
+ std::cout<<"ListenV2X no gps station."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
|
|
|
+}
|
|
|
+
|
|
|
void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
{
|
|
|
|
|
|
@@ -580,6 +709,8 @@ int main(int argc, char *argv[])
|
|
|
|
|
|
std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
|
|
|
|
|
|
+ std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
|
|
|
+
|
|
|
|
|
|
glat0 = atof(strlat0.data());
|
|
|
glon0 = atof(strlon0.data());
|
|
|
@@ -592,6 +723,7 @@ int main(int argc, char *argv[])
|
|
|
gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
|
|
|
gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
|
|
|
gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
|
|
|
+ gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
|
|
|
|
|
|
gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
|
|
|
|
jiaolili