add obs avoid in laneline decition

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -379,8 +379,15 @@ void iv::decition::BrainDecition::run() {
     std::string laneline_speed = xp.GetParam("laneline_speed","5.0");
     ServiceCarStatus.laneline_speed = atof(laneline_speed.data());
 
+    std::string socfDis = xp.GetParam("socfDis","15");
+    std::string aocfDis = xp.GetParam("aocfDis","25");
+    std::string aocfTimes = xp.GetParam("aocfTimes","3");
 
 
+    ServiceCarStatus.socfDis = atof(socfDis.data());
+    ServiceCarStatus.aocfDis = atof(aocfDis.data());
+    ServiceCarStatus.aocfTimes = atof(aocfTimes.data());
+
     givfault->SetFaultState(0,0,"OK.");
 
     while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
@@ -580,7 +587,7 @@ void iv::decition::BrainDecition::run() {
             }else{
 
                 std::cout << "----------------------------------------------getDecideFromLaneline-----------------"<<std::endl;
-                decition_gps = decitionfromlaneline->getDecideFromLaneline(*gps_data_, navigation_data);
+                decition_gps = decitionfromlaneline->getDecideFromLaneline(*gps_data_, navigation_data,templidar);
             }          
             mMutexMap.unlock();
 

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/decide_from_laneline.cpp

@@ -44,6 +44,10 @@ double iv::decition::DecideFromLaneline::obsSpeed=0;
 bool iv::decition::DecideFromLaneline::isFirstRun = true;
 float iv::decition::DecideFromLaneline::minDecelerate=0;
 
+double iv::decition::DecideFromLaneline::lidarDistance = -1;
+double iv::decition::DecideFromLaneline::lastLidarDis = -1;
+double iv::decition::DecideFromLaneline::lastDistance = 500;
+
 extern double offset_real;
 extern double realspeed;
 extern double dSpeed;
@@ -51,8 +55,9 @@ extern double dSecSpeed;
 extern double secSpeed;
 
 extern std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY,gpsTraceMapOri;
+extern std::vector<iv::Point2D> traceOriLeft,traceOriRight;//get by vehWidth and heading angle
 
-iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine)
+iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr)
 {
     Decition laneline_decition(new DecitionBasic);
 
@@ -126,6 +131,8 @@ iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(G
     if(gpsTraceNow.size()==0)
     {
         gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+        gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+        gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
     }
 
 
@@ -220,6 +227,63 @@ iv::decition::Decition iv::decition::DecideFromLaneline::getDecideFromLaneline(G
     transferGpsMode2(gpsMapLine);
     dSpeed = min(dSpeed, ServiceCarStatus.laneline_speed);
 
+    // obstacle avoid start
+    computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,gpsMapLine);
+    static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
+    static double obs_filter_speed_memory=0;
+    if(obs_filter_flag==0)
+    {
+        if(obsDistance>0&&obsDistance<60)
+        {
+            obs_filter++;
+            if(obs_filter<20)                          //80
+            {
+                obsDistance=-1;
+                obsSpeed=0;
+            }
+            else
+            {
+                obs_filter_flag=1;
+                obs_filter_dis_memory=obsDistance;
+                obs_filter_speed_memory=obsSpeed;
+                obs_filter=0;
+            }
+        }
+        else
+        {
+            obs_filter=0;
+        }
+    }
+    else
+    {
+        if(obsDistance<0||obsDistance>60)
+        {
+            obs_filter++;
+            if(obs_filter<40)                           //80
+            {
+                obsDistance=obs_filter_dis_memory;
+                obsSpeed=obs_filter_speed_memory;
+            }
+            else
+            {
+                obs_filter_flag=0;
+                obs_filter=0;
+            }
+        }
+        else
+        {
+            obs_filter=0;
+            obs_filter_dis_memory=obsDistance;
+            obs_filter_speed_memory=obsSpeed;
+        }
+    }
+    ServiceCarStatus.mObsDistance=obsDistance;
+    if(obsDistance>0 && obsDistance<  ServiceCarStatus.socfDis)
+    {
+        dSpeed=0;
+    }
+    // obstacle avoid end
+
     laneline_decition->speed = dSpeed;
     laneline_decition->wheel_angle = 0.0 - controlAng;
 
@@ -425,7 +489,8 @@ std::vector<iv::Point2D> iv::decition::DecideFromLaneline::getLanelinePoints(con
 
 std::vector<iv::Point2D> iv::decition::DecideFromLaneline::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
     vector<iv::Point2D> trace;
-
+    traceOriLeft.clear();
+    traceOriRight.clear();
     if (gpsMapLine.size() > 0 && PathPointNew >= 0) {
         int aimIndex;
         if(circleMode){
@@ -490,6 +555,8 @@ std::vector<iv::Point2D> iv::decition::DecideFromLaneline::getGpsTrace(iv::GPS_I
             double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
             Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
 
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
 
         }
     }
@@ -616,3 +683,60 @@ void iv::decition::DecideFromLaneline::phaseSpeedDecition(iv::decition::Decition
         changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
 }
+
+void iv::decition::DecideFromLaneline::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,
+                                                    const std::vector<GPSData> gpsMapLine) {
+
+    double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+}

src/decition/laneline_decition_brain_sf_changan_shenlan/decition/decide_from_laneline.h

@@ -32,6 +32,11 @@ public:
     DecideFromLaneline();
     ~DecideFromLaneline();
 
+    static double lidarDistance;
+    static double lastLidarDis;
+    iv::Point2D obsPoint;
+    static double lastDistance;
+
     static double minDis;
     static double maxAngle;
     static int lastGpsIndexNew;
@@ -71,7 +76,7 @@ public:
 
 
 
-    Decition getDecideFromLaneline(GPS_INS gpsIns, const std::vector<GPSData> gpsMapLine);
+    Decition getDecideFromLaneline(GPS_INS gpsIns, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr);
 
     void initMethods();
     static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
@@ -88,6 +93,8 @@ public:
     double limitSpeed(double angle, double speed);
 
     void  getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace);
+    void computeObsOnRoadXY(iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,
+                            std::vector<Point2D> gpsTraceRight,const std::vector<GPSData> gpsMapLine);
     
 private:
     //  void changeRoad(int roadNum);

src/include/msgtype/Readme.md

@@ -1 +0,0 @@
-这个目录内的文件有protoc生成。

src1/decision/build-decision_brain-Debug/.qmake.stash

@@ -1,24 +0,0 @@
-QMAKE_CXX.QT_COMPILER_STDCXX = 201402L
-QMAKE_CXX.QMAKE_GCC_MAJOR_VERSION = 7
-QMAKE_CXX.QMAKE_GCC_MINOR_VERSION = 5
-QMAKE_CXX.QMAKE_GCC_PATCH_VERSION = 0
-QMAKE_CXX.COMPILER_MACROS = \
-    QT_COMPILER_STDCXX \
-    QMAKE_GCC_MAJOR_VERSION \
-    QMAKE_GCC_MINOR_VERSION \
-    QMAKE_GCC_PATCH_VERSION
-QMAKE_CXX.INCDIRS = \
-    /usr/include/c++/7 \
-    /usr/include/x86_64-linux-gnu/c++/7 \
-    /usr/include/c++/7/backward \
-    /usr/lib/gcc/x86_64-linux-gnu/7/include \
-    /usr/local/include \
-    /usr/lib/gcc/x86_64-linux-gnu/7/include-fixed \
-    /usr/include/x86_64-linux-gnu \
-    /usr/include
-QMAKE_CXX.LIBDIRS = \
-    /usr/lib/gcc/x86_64-linux-gnu/7 \
-    /usr/lib/x86_64-linux-gnu \
-    /usr/lib \
-    /lib/x86_64-linux-gnu \
-    /lib