add lidar grid obs in brain

include/ivopencv.pri

@@ -1,5 +1,4 @@
-
-unix:INCLUDEPATH += /opt/ros/kinetic/include/opencv-3.3.1-dev
+#unix:INCLUDEPATH += /opt/ros/kinetic/include/opencv-3.3.1-dev
 unix:INCLUDEPATH += /usr/include/opencv4
 unix:LIBS += -lopencv_highgui -lopencv_core -lopencv_imgproc -lopencv_imgcodecs -lopencv_video -lopencv_videoio -lpthread  #-lopencv_shape
 

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

@@ -40,59 +40,59 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
     }
     if (accAim < 0)
     {
-        controlSpeed=0;
-        controlBrake=u; //102
-//        if(obsDistance>0 && obsDistance<10)
-        if(obsDistance>0 && obsDistance<6)
-        {
-            controlBrake= u*1.0;     //1.5    zj-1.2
-            controlSpeed=0;
-        }
-//        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around)
-        if(ttc<5)
-        {
-            controlBrake=0;
-            controlSpeed=0;
-//            controlSpeed=max(0.0,realSpeed-1.0);
-        }
-//        else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
-//                 && dSpeed>0 && lastBrake==0)
-        else if(ttc<10)
-        {
-            controlBrake=0;
-            controlSpeed=max(0.0,realSpeed-2.0);
-        }
-//        else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around)
-        else
-        {
-            controlBrake=0;
-            controlSpeed=max(0.0,realSpeed-1.0);
-        }
-
-//       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
-//                  && !turn_around )
+//        controlSpeed=0;
+//        controlBrake=u; //102
+////        if(obsDistance>0 && obsDistance<10)
+//        if(obsDistance>0 && obsDistance<6)
 //        {
-//            controlBrake=min(controlBrake,0.3f);
+//            controlBrake= u*1.0;     //1.5    zj-1.2
 //            controlSpeed=0;
 //        }
-//        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
-//                && dSpeed>0 && !turn_around )
+////        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around)
+//        if(ttc<5)
 //        {
-//            controlBrake=min(controlBrake,0.5f);
+//            controlBrake=0;
 //            controlSpeed=0;
+////            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+////        else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+////                 && dSpeed>0 && lastBrake==0)
+//        else if(ttc<10)
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-2.0);
+//        }
+////        else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around)
+//        else
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-1.0);
 //        }
 
+////       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                  && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.3f);
+////            controlSpeed=0;
+////        }
+////        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                && dSpeed>0 && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.5f);
+////            controlSpeed=0;
+////        }
+
+
+//        //0110
+//        if(changingDangWei){
+//            controlBrake=max(0.5f,controlBrake);
+//        }
 
-        //0110
-        if(changingDangWei){
-            controlBrake=max(0.5f,controlBrake);
-        }
-
-        //斜坡刹车加大 lsn 0217
-        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
-            controlBrake=max(2.0f,controlBrake);
-        }
-        //斜坡刹车加大 end
+//        //斜坡刹车加大 lsn 0217
+//        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+//            controlBrake=max(2.0f,controlBrake);
+//        }
+//        //斜坡刹车加大 end
     }
     else
     {
@@ -107,7 +107,7 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
 //             controlSpeed = min(realSpeed+1,dSpeed);
 //        }
 
-        controlSpeed= dSpeed;
+//        controlSpeed= dSpeed;
 
         // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
 
@@ -129,9 +129,9 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
 //    if(controlSpeed>0){
 //        controlSpeed=max(controlSpeed,4.6f);
 //    }
-
+    controlSpeed= dSpeed;
     //0227 10m nei xianzhi shache
-    if(obsDistance<5 &&obsDistance>0){
+    if(obsDistance<3 &&obsDistance>0){
         controlSpeed=0;
         controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
     }
@@ -141,9 +141,9 @@ iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS no
         controlSpeed=0;
     }
 
-    if(DecideGps00::minDecelerate==-0.4){
-        controlBrake =0.4;
-    }
+//    if(DecideGps00::minDecelerate==-0.4){
+//        controlBrake =0.4;
+//    }
 
     controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
@@ -157,7 +157,11 @@ 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)->torque = controlSpeed;//hunter目标速度大于0前进，小于0后退，hunter只有速度，角度和驻车控制
+    if(controlSpeed==0.0)
+    {
+        givlog->debug("brain_decition","controlSpeed: %f", controlSpeed);
+    }
+    (*decition)->torque =controlSpeed;//hunter目标速度大于0前进，小于0后退，hunter只有速度，角度和驻车控制
 
     lastBrake= (*decition)->brake;
     lastTorque=(*decition)->torque;

src/decition/decition_brain_sf_1x/decition/brain.cpp

@@ -58,6 +58,29 @@ iv::decition::BrainDecition * gbrain;
            gbrain->GetFusion(strdata,nSize);
         }
 
+        void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+
+        //    qDebug("size is %d",nSize);
+
+            std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
+
+
+            iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
+            int nCount = nSize/sizeof(iv::ObstacleBasic);
+            int i;
+            for(i=0;i<nCount;i++)
+            {
+                iv::ObstacleBasic temp;
+                memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
+                lidar_obs->push_back(temp);
+                pdata++;
+            }
+            gbrain->UpdateOBS(lidar_obs);
+
+          //  gw->UpdateOBS(lidar_obs);
+        }
+
     /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             iv::formation_map_index::map map;
@@ -113,7 +136,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
 
-    mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
+    mpfusion = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
+//    mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
 
     mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
@@ -1402,6 +1426,99 @@ void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasi
 
     iv::decition::BrainDecition::UpdateFusion(xfusion);
 }
+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();
+
+//    ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+//    memset(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++)
+//        {
+////            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
+////            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
+//            ptr[i * (iv::gry) + j].ob = 0;
+////            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
+////            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
+//        }
+//    }
+//    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))
+//        {
+//            ptr[dx*(iv::gry) +dy].high = xobs.high;
+//            ptr[dx*(iv::gry) +dy].low = xobs.low;
+//            ptr[dx*(iv::gry) + dy].ob = 2;
+//            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
+//            ptr[dx*(iv::gry) + dy].pointcount = 5;
+//        }
+
+//    }
+//    /* 激光雷达障碍物*/
+////    typedef void
+////    (sig_cb_lidar_sensor_obstacle_grid)(iv::LidarGridPtr);
+
+////    boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_lidar_obstacle_grid;	//激光雷达栅格
+
+////    signal_lidar_obstacle_grid->operator()(ptr);//触发
+
+//    ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+//    int i;
+//    int nSize = mobs->size();
+//    for(i=0;i<nSize;i++)
+//        _obs_grid_->push_back(mobs->at(i));
+//    ServiceLidar.copyfromlidarobs(_obs_grid_);
+
+//    ServiceCarStatus.mLidarS = 10;
+
+
+
+
+    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;
+            }
+
+        }
+    mMutex_.unlock();
+}
 
 void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
 {

src/decition/decition_brain_sf_1x/decition/brain.h

@@ -164,6 +164,7 @@ namespace iv {
             void UpdateVbox(const char * pdata,const int ndatasize);
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
+            void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);
 
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;

src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -256,7 +256,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         DecideGps00::lastGpsIndex = PathPoint;
 
 
-        if(ServiceCarStatus.speed_control==true){
+        if(ServiceCarStatus.
+                speed_control==true){
             //Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
             Compute00().getPlanSpeed(gpsMapLine);
         }
@@ -2054,23 +2055,23 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     transferGpsMode2(gpsMapLine);
 
-    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
-        if(obsDistance>0 && obsDistance<8){
-                dSpeed=0;
-            }
-    }else if(obsDistance>0 && obsDistance<15){
-        dSpeed=0;
-    }
+//    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+//        if(obsDistance>0 && obsDistance<8){
+//                dSpeed=0;
+//            }
+//    }else if(obsDistance>0 && obsDistance<15){
+//        dSpeed=0;
+//    }
 
-    if(ServiceCarStatus.group_control){
-        dSpeed = ServiceCarStatus.group_comm_speed;
-    }
-    if(dSpeed==0){
-        if(realspeed<6)
-            minDecelerate=min(-0.5f,minDecelerate);
-        else
-            minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
-    }
+//    if(ServiceCarStatus.group_control){
+//        dSpeed = ServiceCarStatus.group_comm_speed;
+//    }
+//    if(dSpeed==0){
+//        if(realspeed<6)
+//            minDecelerate=min(-0.5f,minDecelerate);
+//        else
+//            minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
+//    }
 
     gps_decition->wheel_angle = 0.0 - controlAng;
 
@@ -2238,12 +2239,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                       <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                       <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                       <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                      <<"OBS_Distance"<< ","<<obsDistance<< ","
+                      <<"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<< ","
                       <<"Trace_Point"<< ","<<PathPoint<< ","
-                      <<"OBS_Distance"<< ","<<obsDistance<< ","
                       <<"OBS_Speed"<< ","<<obsSpeed<< ","
                       <<"TTC"<< ","<<ttc<< ","
                       <<endl;

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -458,6 +458,7 @@ static void ListenPointCloud(const char * strdata,const unsigned int nSize,const
             temp.nomal_y = 1000;
             lidar_obs->push_back(temp);
         }
+        qDebug("obs size %d", lidar_obs->size());
         if(lidar_obs->size() >0)iv::modulecomm::ModuleSendMsg(glidar_obs,(char *)(lidar_obs->data()),lidar_obs->size()*sizeof(iv::ObstacleBasic));
     }
     else

src/detection/detection_lidar_st/detection_lidar_st.pro

@@ -32,7 +32,6 @@ DEPENDPATH += $$PWD/rs/include
 
 
 INCLUDEPATH += $$PWD/../../../include/
-LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm
 
 # You can also make your code fail to compile if you use deprecated APIs.
 # In order to do so, uncomment the following line.
@@ -43,5 +42,26 @@ SOURCES += main.cpp \
     perceptionoutput.cpp
 
 HEADERS += \
-    perceptionoutput.h \
-    xmlparam.h
+    perceptionoutput.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}

thirdpartylib/Readme.md

@@ -1 +0,0 @@
-第三方库到modularization_thirdpartylib仓库下载。