Browse Source

add obstacle scan boundary

zhangjia 3 years ago
parent
commit
8fbcde352a

+ 1 - 0
src/decition/common/common/car_status.h

@@ -146,6 +146,7 @@ namespace iv {
         iv::ultrasonic_obs multrasonic_obs;
         std::vector<iv::TracePoint> obsTraceLeft,obsTraceRight;
 
+
         double mfGPSAcc = 0;
 
 //        iv::brain::decitionview mdecitionview;

+ 8 - 0
src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -599,6 +599,8 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
     }
 
+    ServiceCarStatus.obsTraceLeft.clear();
+    ServiceCarStatus.obsTraceRight.clear();
 
     for (int j = 0; j < gpsTrace.size(); j++)
     {
@@ -633,6 +635,12 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         gpsTraceLeft.push_back(ptLeft);
         gpsTraceRight.push_back(ptRight);
 
+
+        TracePoint obsptleft(ptLeft.x,ptLeft.y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+
+        TracePoint obsptright(ptRight.x,ptRight.y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptright);
     }
 
 

+ 5 - 0
src/decition/decition_brain_sf/decition/brain.cpp

@@ -132,6 +132,9 @@ iv::decition::BrainDecition::BrainDecition()
     void * pa = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
 
     mpaPlanTrace = iv::modulecomm::RegisterSend("plantrace",100000,1);
+    mpaObsTraceLeft = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
+    mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);
+
 
     mTime.start();
     mnOldTime = mTime.elapsed();
@@ -760,6 +763,8 @@ void iv::decition::BrainDecition::run() {
             }
             iv::modulecomm::ModuleSendMsg(mpaPlanTrace,(char *)decitionGps00->planTrace.data(),decitionGps00->planTrace.size()*sizeof(iv::TracePoint));
 
+            iv::modulecomm::ModuleSendMsg(mpaObsTraceLeft,(char *)(ServiceCarStatus.obsTraceLeft.data()),ServiceCarStatus.obsTraceLeft.size()*sizeof(iv::TracePoint));
+            iv::modulecomm::ModuleSendMsg(mpaObsTraceRight,(char *)(ServiceCarStatus.obsTraceRight.data()),ServiceCarStatus.obsTraceRight.size()*sizeof(iv::TracePoint));
 
 
             //      decition_gps = decitionLine00->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);

+ 2 - 0
src/decition/decition_brain_sf/decition/brain.h

@@ -136,6 +136,8 @@ namespace iv {
             void * mpaVechicleState;
             void * mpaToPlatform;
             void * mpaPlanTrace;
+            void * mpaObsTraceLeft;
+            void * mpaObsTraceRight;
 
 
             void ShareDecition(iv::decition::Decition decition);