|
@@ -898,6 +898,37 @@ void SetPlan(xodrobj xo)
|
|
|
|
|
|
iv::GPSData data(new iv::GPS_INS);
|
|
|
data->roadMode = 0;
|
|
|
+
|
|
|
+ // std::cout<<"i:"<<i<<" lr:"<<xPlan[i].nlrchange<<std::endl;
|
|
|
+ if(xPlan[i].nlrchange == 1)
|
|
|
+ {
|
|
|
+ data->roadMode = 14;
|
|
|
+ }
|
|
|
+ if(xPlan[i].nlrchange == 2)
|
|
|
+ {
|
|
|
+ data->roadMode = 15;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(i>50)
|
|
|
+ {
|
|
|
+ if((xPlan[i-1].nlrchange == 0)&&(xPlan[i].nlrchange != 0))
|
|
|
+ {
|
|
|
+ int j;
|
|
|
+ for(j=(i-1);j>=(i-50);j--)
|
|
|
+ {
|
|
|
+ if(xPlan[i].nlrchange == 1)
|
|
|
+ {
|
|
|
+ mapdata[j]->roadMode = 14;
|
|
|
+ }
|
|
|
+ if(xPlan[i].nlrchange == 2)
|
|
|
+ {
|
|
|
+ mapdata[j]->roadMode = 15;
|
|
|
+ }
|
|
|
+ if(j <=0)break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
data->gps_lat = gps_lat;
|
|
|
data->gps_lat_avoidleft = gps_lat_avoidleft;
|
|
|
data->gps_lat_avoidright = gps_lat_avoidright;
|