红绿等停止线及开关逻辑

src/decition/common/common/car_status.h

@@ -257,6 +257,7 @@ namespace iv {
         QTime mRSUupdateTimer;
         QTime mRSUWarnUpdateTimer;
         QTime mRSUTrafficUpdateTimer;
+        bool mLightStartSensorBtn=false;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -284,10 +284,10 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.mLidarRotation = atof(xp.GetParam("LidarRotation","90.0").data());
     ServiceCarStatus.mLidarRangeUnit = atof(xp.GetParam("LidarRangeUnit","5.0").data());
 
-    std::string lightlon = xp.GetParam("lightlon","0");
-    std::string lightlat = xp.GetParam("lightlat","0");
-    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
-    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
+//    std::string lightlon = xp.GetParam("lightlon","0");
+//    std::string lightlat = xp.GetParam("lightlat","0");
+//    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
+//    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());  //20230310,红绿灯信息从V2R中传来
 
 
     //mobileEye
@@ -610,6 +610,7 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
+            xbs.set_mbtraficlightstart(ServiceCarStatus.mLightStartSensorBtn);
             ShareBrainstate(xbs);
             ShareDecition(decition_gps);
 
@@ -843,7 +844,7 @@ void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
                 <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
                 <<"  decideAngActive:"<<decition->angle_active
                 <<"  decideAutoMode:"<<decition->auto_mode
-              <<std::endl;
+                <<std::endl;
         std::cout<<"========================================="<<std::endl;
     }
     else
@@ -1207,6 +1208,10 @@ void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasi
     if(group_message.has_radiobroadcasttraffictype())
     {
         ServiceCarStatus.rsu_traffic_type = group_message.radiobroadcasttraffictype();
+        if(group_message.has_trafficlightstoplat())
+            ServiceCarStatus.iTrafficeLightLat = group_message.trafficlightstoplat();
+        if(group_message.has_trafficlightstoplon())
+            ServiceCarStatus.iTrafficeLightLon = group_message.trafficlightstoplon();
         ServiceCarStatus.mRSUTrafficUpdateTimer.start();
     }
     if(group_message.has_radiobroadcastspeedlimit())
@@ -1222,6 +1227,9 @@ void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasi
     {
         ServiceCarStatus.rsu_warnlimit_spd = group_message.radiowarningspeedlimit();
     }
+
+
+
 }
 
 void iv::decition::BrainDecition::UpdateSate(){

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -1099,7 +1099,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if(gpsTraceOri.empty())
     {
-        gps_decition->wheel_angle = 0;
+        gps_decition->wheel_angle = 0.2;
         gps_decition->speed = dSpeed;
 
         gps_decition->accelerator = -0.5;
@@ -1601,12 +1601,15 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
 
+        givlog->debug("decition_brain","obs_filter: %d,obs_filter_flag: %d,obs_filter_dis_memory: %f,obsDistance: %f,obsSpeed: %f,obs_speed_for_avoid: %f",
+                      obs_filter,obs_filter_flag,obs_filter_dis_memory,obsDistance,obsSpeed,obs_speed_for_avoid);
+
         obs_speed_for_avoid=0;//shenlan guosai zhangaiwu sudu zhijiegei 0
     }
     else
     {
         gpsTraceRear.clear();
-        for(int i=0;i<gpsTraceNow.size();i++)
+        for(unsigned int i=0;i<gpsTraceNow.size();i++)
         {
             Point2D pt;
             pt.x=0-gpsTraceNow[i].x;
@@ -2149,6 +2152,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     std::cout<<"juecesudu0="<<dSpeed<<std::endl;
 
+
     //----------------------------------------shenlan 采集车车路协同,add---------------------------------------------
     if(ServiceCarStatus.mRSUupdateTimer.elapsed()>10*1000)
      {
@@ -2162,6 +2166,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
      {
         ServiceCarStatus.rsu_traffic_type=200;//lvdeng
         ServiceCarStatus.rsu_trafficelimit_spd=200;
+
+        ServiceCarStatus.iTrafficeLightLon=0;//20230310
+        ServiceCarStatus.iTrafficeLightLat=0;//20230310
+
  //       ServiceCarStatus.rsu_warning_type=200;
  //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
  //       ServiceCarStatus.rsu_gps_lng = 0.0;
@@ -2288,6 +2296,25 @@ else
     //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
 
 
+
+    //20230310 add begin,shanlan 由决策决定什么时候开启红绿等检测
+    float distance_to_stopline;
+    distance_to_stopline=GetDistance(now_gps_ins,traffic_light_gps);
+    if(distance_to_stopline<20.0)
+    {
+        ServiceCarStatus.mLightStartSensorBtn=true;
+    }
+    else
+    {
+        ServiceCarStatus.mLightStartSensorBtn=false;
+    }
+
+    //20230310 add end,由决策决定什么时候开启红绿等检测
+
+
+
+
+
     transferGpsMode2(gpsMapLine);
 
     if(ServiceCarStatus.msysparam.mvehtype=="hapo")
@@ -2509,36 +2536,38 @@ else
                                  <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                                  <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                                  <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
-  //                            <<"readyParkMode"<< ","<<readyParkMode<< ","
-  //                              <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
-                                <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+  //                             <<"readyParkMode"<< ","<<readyParkMode<< ","
+  //                             <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                                 <<"OBS_Distance"<< ","<<obsDistance_log<< ","
                                  <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
-                                <<"obsSpeed_fusion"<<","<<obsSpeed<<","
-                                <<"SecSpeed"<<","<<secSpeed<<","
+                                 <<"obsSpeed_fusion"<<","<<obsSpeed<<","
+                                 <<"SecSpeed"<<","<<secSpeed<<","
                                  <<"Vehicle_state"<< ","<<vehState<< ","
-                                <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                                 <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
                                  <<"avoidXNew"<<","<< setprecision(3)<<avoidXNew<<","
                                  <<"avoidXNewPre"<<","<< setprecision(3)<<avoidXNewPre<<","
+                                 <<"Now_s_d_d"<< ","<< setprecision(3)<<now_s_d.d<< ","
+                                 <<"Now_s_d_s"<< ","<< setprecision(3)<<now_s_d.s<< ","
                                  //<<"avoidXPre"<<","<<avoidXPre<<","
                                  <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
                                  <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
                                  <<"Min_Decelation"","<<minDecelerate<< ","
-      //                           <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                                 <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
                                  <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
                                  <<"Vehicle_GPS_lat"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
                                  <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
-                                <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
-                                <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
-                                <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
-                                <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
-                                <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
+                                 <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
+                                 <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
+                                 <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
+                                 <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
+                                 <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
                                  <<"Trace_Point"<< ","<<PathPoint<< ","
                                  <<"changingDangWei"<< ","<<changingDangWei<< ","
                                  <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
-      //                           <<"TTC"<< ","<<ttc<< ","
-  //                               <<"Decide_torque"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
-  //                               <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
-  //                               <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                                 <<"TTC"<< ","<<ttc<< ","
+                                 <<"LightColor"  << ","  <<ServiceCarStatus.iTrafficeLightColor<< ","
+                                 <<"LightTime"<< ","  <<ServiceCarStatus.iTrafficeLightTime<< ","
+                                 <<"DisTostopline"<< ","  <<setprecision(2)<<distance_to_stopline<< ","
   //                               <<"OBS_Distance"<< ","<<obsDistance_log<< ","
   //                               <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
   //                               <<"Vehicle_state"<< ","<<vehState<< ","

src/include/proto/brainstate.proto

@@ -10,4 +10,5 @@ message brainstate
   optional bool mbBocheEnable = 4;
   optional double decision_period= 5; 
   optional bool mbBrainRunning = 6;
+  optional bool mbTraficLightStart=7;  //红绿灯开启检测标志，true 开启.false停止
 };

src/include/proto/v2r.proto

@@ -16,4 +16,7 @@ message v2r_send
 	optional int32	radioIdentiStart= 10;//拥堵识别开启指令	1：开启 254：异常 255：无效
         optional bool   mbpause=11;  //自动驾驶启动停止,true 停车，false 启动
 	optional float  speedlimit=12;//限速
+	optional double trafficLightStopLat	= 13;//实时路况广播lat
+	optional double trafficLightStopLon	= 14;//实时路况广播lon
+
 };