modify lidar obs detection

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);//20210929,cxw,单纯使用激光雷达不用融合后的结果
+//    iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);//20210929,cxw,单纯使用激光雷达不用融合后的结果
 //    mpa = new adcmemshare("lidar_obs",20000000,3);
 //    mpa->listenmsg(ListenOBS);
 
-    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);//20210929,cxw
+//    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/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);//20210929，cxw,buyong zhege
+    mpfusion = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);//20210929，cxw,buyong zhege
 //    mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
 
@@ -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/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -203,6 +203,7 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
         {
             continue;
         }
+        if (z < -0.5 && z > 1.0) continue;  // pingbidianyun Z<-0.5 he Z>1
         float dis = sqrt(x*x + y*y);
 
 //        if((dis>0.9)&&(z>(-SENSOR_HEIGHT-dis*General_Thresh_Ratio))&&(z<(-SENSOR_HEIGHT+dis*General_Thresh_Ratio)))
@@ -654,14 +655,14 @@ int main(int argc, char *argv[])
         return 0;
     }
 
-    snprintf(gstr_sensorheight,255,"2.0");
-    snprintf(gstr_vehicleheight,255,"2.3");
+    snprintf(gstr_sensorheight,255,"0.8");
+    snprintf(gstr_vehicleheight,255,"0.8");
     snprintf(gstr_inputmemname,255,"lidar_pc");
     snprintf(gstr_outputmemname,255,"lidar_obs");
-    snprintf(gstr_skipxmin,255,"-0.9");
-    snprintf(gstr_skipxmax,255,"0.9");
-    snprintf(gstr_skipymin,255,"-2.3");
-    snprintf(gstr_skipymax,255,"2.3");
+    snprintf(gstr_skipxmin,255,"-0.4");
+    snprintf(gstr_skipxmax,255,"0.4");
+    snprintf(gstr_skipymin,255,"-0.8");
+    snprintf(gstr_skipymax,255,"0.8");
 
 
     givlog->verbose("yaml is %s ",gstr_yaml);

src/driver/driver_lidar_leishen16/lidar_driver_leishen16.cpp

@@ -393,6 +393,8 @@ static void process_leishen16obs(char * strdata,int nLen)
                     point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
                     point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
                 }
+//                point.z = point.z +1.0;
+//                if(point.z < 0.0 && point.z >0.5) continue;
                 point_cloud->points.push_back(point);
 
 
@@ -432,6 +434,8 @@ static void process_leishen16obs(char * strdata,int nLen)
                     point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
                     point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
                 }
+//                point.z = point.z +1.0;
+//                if(point.z < 0.0 && point.z >0.5) continue;
                 point_cloud->points.push_back(point);