add traffic light logic for 1x

src/decition/decition_brain_sf_1x/decition/brain.cpp

@@ -384,8 +384,8 @@ void iv::decition::BrainDecition::run() {
 
     std::string lightlon = xp.GetParam("lightlon","0");
     std::string lightlat = xp.GetParam("lightlat","0");
-    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
-    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
+    //ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data()); //cxw,红绿灯的经纬度从地图中直接读取
+    //ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
 
 
 

src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -279,7 +279,17 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         //     circleMode = true;
 
 
-        getV2XTrafficPositionVector(gpsMapLine);
+//        getV2XTrafficPositionVector(gpsMapLine);//20211029,cxw, 1+x中只有一个红绿灯信息所以还是用原来的，直接取经纬度
+        for (int var = 0; var < gpsMapLine.size(); var++)
+        {
+            if(gpsMapLine[var]->roadMode==6|| gpsMapLine[var]->mode2==1000001)
+            {
+                //v2xTrafficVector.push_back(var);
+                ServiceCarStatus.iTrafficeLightLat=gpsMapLine[var]->gps_lat;
+                ServiceCarStatus.iTrafficeLightLon=gpsMapLine[var]->gps_lng;
+                break;
+            }
+        }
         //        group_ori_gps=*gpsMapLine[0];
         ServiceCarStatus.bocheMode=false;
         ServiceCarStatus.daocheMode=false;
@@ -2287,39 +2297,39 @@ else
                    obsDistance_log=obsDistance;
                QDateTime dt2=QDateTime::currentDateTime();
                outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
-                      <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
-                      <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
-                      <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
-                      <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
-                      <<"OBS_Distance"<< ","<<obsDistance_log<< ","
-                      <<"Min_Decelation"","<<minDecelerate<< ","
-                      <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
-                      <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
-                      <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
-                      <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
-                     <<"center_GPS_X"<< ","<< setprecision(10)<<traffic_center_gps.gps_lat<< ","
-                     <<"center_GPS_Y"<< ","<< setprecision(10)<<traffic_center_gps.gps_lng<< ","
-                      <<"Trace_Point"<< ","<<PathPoint<< ","
-                      <<"OBS_Speed"<< ","<<obsSpeed<< ","
-                      <<"TTC"<< ","<<ttc<< ","
-                      <<"Traffic_Type"<< ","<<traffic_type<< ","
-                      <<"Warn_Type"<< ","<<warning_type<< ","
-                      <<"TrafficLimit_Speed"<< ","<<trafficlimit_spd<< ","
-                      <<"Radation_Dis"<< ","<<radiation_distance<< ","
-                      <<"Dis_to_center"<< ","<<distance_to_center<< ","
+                       <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                       <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                       <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                       <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                       <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                       <<"Min_Decelation"","<<minDecelerate<< ","
+                       <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                       <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                       <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                       <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                       <<"Center_GPS_X"<< ","<< setprecision(10)<<traffic_center_gps.gps_lat<< ","
+                       <<"Center_GPS_Y"<< ","<< setprecision(10)<<traffic_center_gps.gps_lng<< ","
+                       <<"Trace_Point"<< ","<<PathPoint<< ","
+                       <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                       <<"TTC"<< ","<<ttc<< ","
+                       <<"Traffic_Type"<< ","<<traffic_type<< ","
+                       <<"Warn_Type"<< ","<<warning_type<< ","
+                       <<"TrafficLimit_Speed"<< ","<<trafficlimit_spd<< ","
+                       <<"Radation_Dis"<< ","<<radiation_distance<< ","
+                       <<"Dis_to_center"<< ","<<distance_to_center<< ","
+                       <<"Light_colour"<< ","<<ServiceCarStatus.iTrafficeLightColor<< ","
+                       <<"Light_time"<< ","<<ServiceCarStatus.iTrafficeLightTime<< ","
                       <<endl;
                outfile.close();
            }
+        }
+       givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f",
+                      vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate, gps_decition->torque,gps_decition->brake,
+                     gps_decition->wheel_angle);
+       givlog->debug("decition_brain","Traffic_Type: %d,Warn_Type: %d,TrafficLimit_Speed: %f,radiation_dis: %f,Dis_to_center: %f,WarnLimit_Speed: %f",
+                     traffic_type,warning_type,trafficlimit_spd,radiation_distance,distance_to_center,warnlimit_spd);
+       givlog->debug("decition_brain","Light_colour: %d,Light_time: %d",ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime);
    }
-       givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,"
-                     "obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,"
-                     "wheel_angle: %f","Traffic_Type: %d","Warn_Type: %d","TrafficLimit_Speed: %f",
-                     "radiation_dis: %f","Dis_to_center: %f","WarnLimit_Speed: %f",
-                      vehState,PathPoint,dSpeed,obsDistance,
-                     obsSpeed,realspeed,minDecelerate, gps_decition->torque,gps_decition->brake,
-                     gps_decition->wheel_angle,traffic_type,warning_type,trafficlimit_spd,
-                     radiation_distance,distance_to_center,warnlimit_spd);
-       }
 
 
     lastAngle=gps_decition->wheel_angle;