modify pure lidar for obs detection and log save logic

src/decition/common/common/car_status.h

@@ -166,12 +166,12 @@ namespace iv {
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90
         int iTrafficeLightID = 0;       //红绿灯ID
-        int iTrafficeLightColor = 2;    //0x0: 红色
-        //0x1: 黄色
-        //0x2: 绿色
-        int iTrafficeLightTime = 0;     //红绿灯剩余时间
-        double iTrafficeLightLat = 0;
-        double iTrafficeLightLon = 0;
+//        int iTrafficeLightColor = 2;    //0x0: 红色
+//        //0x1: 黄色
+//        //0x2: 绿色
+//        int iTrafficeLightTime = 0;     //红绿灯剩余时间
+//        double iTrafficeLightLat = 0;
+//        double iTrafficeLightLon = 0;
 
         bool daocheMode=false;
 
@@ -199,6 +199,19 @@ namespace iv {
         bool inRoadAvoid = false;
         //hapo station 2021/02/05
         iv::StationCmd   stationCmd;
+
+
+        //对于1+x车辆V2X信息，cxw
+        unsigned char rsu_traffic_type=0;//路况信息
+        unsigned char rsu_warning_type=0;//碰撞预警信息
+        float rsu_radiation_distance=0;//事件辐射范围
+        float rsu_limit_spd=200;  //200代表没有限速
+        int iTrafficeLightColor = 2;    //0x0: 红色
+        //0x1: 黄色
+        //0x2: 绿色
+        int iTrafficeLightTime = 0;     //红绿灯剩余时间
+        double iTrafficeLightLat = 0;
+        double iTrafficeLightLon = 0;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/common/perception_sf/sensor_lidar.cpp

@@ -102,11 +102,11 @@ void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int
 void iv::perception::LiDARSensor::start() {
 
 
- //   iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
+    iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);//20210929,cxw,单纯使用激光雷达不用融合后的结果
 //    mpa = new adcmemshare("lidar_obs",20000000,3);
 //    mpa->listenmsg(ListenOBS);
 
-//    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
+    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);//20210929,cxw
 //    mpb = new adcmemshare("lidar_per",20000000,3);
 //    mpb->listenmsg(ListenPer);
 

src/decition/decition_brain_sf_1x/decition/adc_adapter/hunter_adapter.cpp

@@ -38,8 +38,8 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
     if(ttc<0){
         ttc=15;
     }
-    if (accAim < 0)
-    {
+//    if (accAim < 0)
+//    {
 //        controlSpeed=0;
 //        controlBrake=u; //102
 ////        if(obsDistance>0 && obsDistance<10)
@@ -93,10 +93,10 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
 //            controlBrake=max(2.0f,controlBrake);
 //        }
 //        //斜坡刹车加大 end
-    }
-    else
-    {
-        controlBrake = 0;
+//    }
+//    else
+//    {
+//        controlBrake = 0;
 
 //        if(abs(dSpeed-realSpeed)<=2.0)
 //        {
@@ -107,36 +107,30 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
 //             controlSpeed = min(realSpeed+1,dSpeed);
 //        }
 
+
 //        controlSpeed= dSpeed;
 
         // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
 
-//        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-//            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-//                && abs(realSpeed)<1.0){
-//            controlSpeed=max((float)5.0,controlSpeed);
-//            controlSpeed=min((float)4.0,controlSpeed);
-//        }
-//        // 斜坡加大油门  end
-//        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-//                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-//                && abs(realSpeed)<10.0){
-//            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
-//        }
-
-    }
-
-//    if(controlSpeed>0){
-//        controlSpeed=max(controlSpeed,4.6f);
 //    }
+
     controlSpeed= dSpeed;
+    controlSpeed=max(controlSpeed,5.0f); //对车子进行限速，车子最大速度是5km/h
+
     //0227 10m nei xianzhi shache
-    if(obsDistance<3 &&obsDistance>0){
+    //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车
+    if(obsDistance<10 &&obsDistance>3){
+        controlSpeed=max(1.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+    else if(obsDistance<=3 &&obsDistance>0)
+    {
         controlSpeed=0;
         controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
     }
 
-    if(DecideGps00::minDecelerate<0){
+    if(DecideGps00::minDecelerate<0)  //在到达地图终点的时候，停车
+    {
         controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
         controlSpeed=0;
     }
@@ -155,8 +149,7 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
 //    }
 
 
-    (*decition)->brake = controlBrake*9;//9     zj-6
- //   (*decition)->torque=controlSpeed; //20210702,cxw,yika shi mubiaosudu kognzhi ,qie mubiaosudu yao pignhua
+    (*decition)->brake = controlBrake*9;//9     zj-6//对于hunter车辆，brake 没有用
     if(controlSpeed==0.0)
     {
         givlog->debug("brain_decition","controlSpeed: %f", controlSpeed);

src/decition/decition_brain_sf_1x/decition/brain.cpp

@@ -136,7 +136,7 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
 
-    mpfusion = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
+//    mpfusion = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);//20210929，cxw,buyong zhege
 //    mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
 
@@ -343,21 +343,21 @@ void iv::decition::BrainDecition::run() {
         ServiceCarStatus.txt_log_on=false;
     }
 
-    if(ServiceCarStatus.txt_log_on==true){
-        QDateTime dt = QDateTime::currentDateTime();
-        QString date =dt.toString("yyyy_MM_dd_hhmm");
-        QString strsen = "datalog_";
-        date.append(strsen);
-        //将数据写入文件开始
-        ofstream outfile;
-        outfile.open("control_log.txt", ostream::ate);
-        outfile.flush();
-        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
-              <<"vehState"<<'\t'<<"PathPoint"<<'\t'<<setprecision(4)<<"dSpeed"<<'\t'<<"obsDistance"<<'\t'<<"obsSpeed"<<'\t'<<"realspeed"<<'\t'<<"minDecelerate"<<'\t'
-             <<"torque"<<'\t'<<"brake"<<'\t'<<"wheel_angle"<<endl;
-        outfile.close();
-        //将数据写入文件结束
-    }
+//    if(ServiceCarStatus.txt_log_on==true){
+//        QDateTime dt = QDateTime::currentDateTime();
+//        QString date =dt.toString("yyyy_MM_dd_hhmm");
+//        QString strsen = "datalog_";
+//        date.append(strsen);
+//        //将数据写入文件开始
+//        ofstream outfile;
+//        outfile.open("control_log.txt", ostream::ate);
+//        outfile.flush();
+//        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
+//              <<"vehState"<<'\t'<<"PathPoint"<<'\t'<<setprecision(4)<<"dSpeed"<<'\t'<<"obsDistance"<<'\t'<<"obsSpeed"<<'\t'<<"realspeed"<<'\t'<<"minDecelerate"<<'\t'
+//             <<"torque"<<'\t'<<"brake"<<'\t'<<"wheel_angle"<<endl;
+//        outfile.close();
+//        //将数据写入文件结束
+//    }
 
     std::string strparklat = xp.GetParam("parklat","39.0624557");
     std::string strparklng = xp.GetParam("parklng","117.3575493");
@@ -1428,11 +1428,11 @@ void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasi
 }
 void iv::decition::BrainDecition::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
 {
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
-    iv::LidarGridPtr ptr;
-    mMutex_.lock();
-    lidar_obs.swap(mobs);
-    mMutex_.unlock();
+//    std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
+//    iv::LidarGridPtr ptr;
+//    mMutex_.lock();
+//    lidar_obs.swap(mobs);
+//    mMutex_.unlock();
 
 //    ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
 //    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
@@ -1483,41 +1483,41 @@ void iv::decition::BrainDecition::UpdateOBS(std::shared_ptr<std::vector<iv::Obst
 
 
 
-    mMutex_.lock();
-//    fusion_obs.swap(mfusion_obs_);
-    if(fusion_ptr_ != NULL)
-    {
-        free(fusion_ptr_);
-        fusion_ptr_ = NULL;
-    }
-    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
-    for(int i =0; i<iv::grx; i++)     //复制到指针数组
-    {
-        for(int j =0; j<iv::gry; j++)
-        {
-            fusion_ptr_[i*(iv::gry)+j].ob = 0;
-        }
+//    mMutex_.lock();
+////    fusion_obs.swap(mfusion_obs_);
+//    if(fusion_ptr_ != NULL)
+//    {
+//        free(fusion_ptr_);
+//        fusion_ptr_ = NULL;
+//    }
+//    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+//    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
+//    for(int i =0; i<iv::grx; i++)     //复制到指针数组
+//    {
+//        for(int j =0; j<iv::gry; j++)
+//        {
+//            fusion_ptr_[i*(iv::gry)+j].ob = 0;
+//        }
 
-    }
+//    }
 
-        for(int i=0;i<mobs->size();i++)
-        {
-            iv::ObstacleBasic xobs = mobs->at(i);
-            int dx,dy;
-            dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
-            dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
-            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
-            {
-                fusion_ptr_[dx*(iv::gry) +dy].high = xobs.high;
-                fusion_ptr_[dx*(iv::gry) +dy].low = xobs.low;
-                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
-                fusion_ptr_[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
-                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
-            }
+//        for(int i=0;i<mobs->size();i++)
+//        {
+//            iv::ObstacleBasic xobs = mobs->at(i);
+//            int dx,dy;
+//            dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
+//            dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
+//            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+//            {
+//                fusion_ptr_[dx*(iv::gry) +dy].high = xobs.high;
+//                fusion_ptr_[dx*(iv::gry) +dy].low = xobs.low;
+//                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
+//                fusion_ptr_[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
+//                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
+//            }
 
-        }
-    mMutex_.unlock();
+//        }
+//    mMutex_.unlock();
 }
 
 void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)

src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -963,7 +963,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //        if((ServiceCarStatus.msysparam.mvehtype=="pixloop")||(ServiceCarStatus.msysparam.mvehtype=="yika")
 //               || (ServiceCarStatus.msysparam.mvehtype=="hunter"))
 //        {
-//            dSpeed=0.0;   //20210903,cxw,这三种车型都是用决策速度控制的
+//            dSpeed=0.0;   //20210903,cxw,这三种车型都是用决策速度控制的，在adapter中判断minDecelerate小于0直接将目标速度归0
 //        }
         phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
         return gps_decition;
@@ -1986,11 +1986,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //-----------------------------------------1+X采集车车路协同,add---------------------------------------------
-    unsigned char traffic_type=0;//路况信息
-    unsigned char warning_type=0;//预警信息，RSU获得
+    unsigned char traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
+    unsigned char warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
     float distance_to_center=0;
-    float radiation_distance=0;//事件辐射范围，从RSU获得
-    float limit_spd=0; //从RSU获得的限速值
+    float radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
+    float limit_spd=ServiceCarStatus.rsu_limit_spd; //从RSU获得的限速值
     //以上变量信息都需要存储到log文件中
 //    distance_to_center=GetDistance(now_gps_ins,gps_center);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
     if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
@@ -2023,6 +2023,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         dSpeed=0.0;
     }
+//红绿灯信息处理，computeTrafficSpeedLimt由于车子最高5所以要对速度做调整
     //-----------------------------------------1+X采集车车路协同，end---------------------------------------------
     //-------------------------------traffic light----------------------------------------------------------------------------------------
 
@@ -2048,14 +2049,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     dSpeed = min(v2xTrafficSpeed,dSpeed);//???这里是不是应该是max,因为V2Xspeed应该是穿过红绿灯的最低速度吧？速度还不能超过当前的限速，如果V2Xspeed超过限速，需要判断车辆是否要
-    //停车？
+    //停车，只会减速或当前决策速度不会加速
 
     //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
 
 
     transferGpsMode2(gpsMapLine);
 
-//    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+//    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){   //这一部分代码对1+x小车没有用，因为小车的速度比较慢，如果障碍物距离小于15的时候就将目标速度给0,停障距离太远，停障逻辑在ADAPTER中做
 //        if(obsDistance>0 && obsDistance<8){
 //                dSpeed=0;
 //            }
@@ -2087,23 +2088,26 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
    //shuju cunchu gognnenng add,20210624,cxw
    if(ServiceCarStatus.txt_log_on==true)
    {
-   //    static int file_num=0;
-//       double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[0]);
-//       double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[gpsMapLine.size()-1]);
-       if(first_start_en)
+      if(first_start_en)
        {
           first_start_en=false;
           start_tracepoint = PathPoint;
-          if(start_tracepoint==0)
+          if(circleMode)
           {
-
-              end_tracepoint =gpsMapLine.size()-1;
+              if(start_tracepoint==0)
+              {
+
+                  end_tracepoint =gpsMapLine.size()-1;  //这种计算终点坐标的序号只适合与闭环地图
+              }
+              else
+              {
+                  end_tracepoint=start_tracepoint-1;
+              }
           }
           else
           {
-              end_tracepoint=start_tracepoint-1;
+             end_tracepoint =gpsMapLine.size()-1; //20210929,不是闭环地图的时候终点参考坐标就是地图数组的最大序号
           }
-
        }
        double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
        double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
@@ -2139,7 +2143,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
            }
            QDateTime dt=QDateTime::currentDateTime();
            QString date =dt.toString("yyyy_MM_dd_hhmm");
-           QString strsen = "datalog_";
+           QString strsen = "1xdatalog_";
            date.append(strsen);
            QString s = QString::number(file_num);
            date.append(s);
@@ -2171,6 +2175,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                   <<"Trace_Point"<< ","<<PathPoint<< ","
                   <<"OBS_Distance"<< ","<<obsDistance<< ","
                   <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                  <<"Traffic_Type"<< ","<<traffic_type<< ","
+                  <<"Warn_Type"<< ","<<warning_type<< ","
+                  <<"Limit_Speed"<< ","<<limit_spd<< ","
                   <<endl;
            outfile.close();
        }