将障碍物距离换算成S向的距离

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/transfer.cpp

@@ -228,7 +228,7 @@ iv::Point2D iv::decition::frenet_to_cartesian1D(std::vector<GPSData> gpsMap,doub
     int s_index=(int)(s_condition-(*gpsMap[index]).frenet_s)*10+index;
     int start_index=max(0,s_index-500);
     int map_size=gpsMap.size();
-    int end_index=min(s_index+500,map_size);
+    int end_index=min(s_index+500,map_size-1);
 
     int map_max=(*gpsMap[map_size-1]).frenet_s;
     double s_index_min=(*gpsMap[start_index]).frenet_s;

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_gps_00.cpp

@@ -1603,7 +1603,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
     if(!ServiceCarStatus.daocheMode){
         //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        computeObsOnRoadXY(lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
 
 
         if(obs_filter_flag==0)
@@ -3399,13 +3399,19 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
 
 }
 
-void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
                                                  const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
 
     //    QTime xTime;
     //   xTime.start();
     //    qDebug("time 0 is %d ",xTime.elapsed());
     double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
 
     if (lidarGridPtr == NULL)
     {
@@ -3420,7 +3426,15 @@ void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr
         if(!ServiceCarStatus.useMobileEye){
             lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
         }
-        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+
+        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);
+        givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
         //    lidarDistance=-1;
         if (lidarDistance<0)
         {

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_gps_00.h

@@ -197,7 +197,7 @@ public:
     bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
 
     void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
-    void computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    void computeObsOnRoadXY(iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
     static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
 
     void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);