modify avoid pro

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -4209,14 +4209,16 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
                     givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
           }
    }
-   if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
-            signed_record_avoidX=front_car.avoidX;
-            return signed_record_avoidX;
-   }
+   if(front_car_id>0){
+           if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
+                    signed_record_avoidX=front_car.avoidX;
+                    return signed_record_avoidX;
+           }
 
-   if((obs_cur_distance<50)&&(front_car.front_car_dis<150)&&(front_car.avoidX!=0||front_car.vehState!=0)){
-            signed_record_avoidX=front_car.avoidX;
-            return signed_record_avoidX;
+           if((obs_cur_distance<50)&&(front_car.front_car_dis<150)&&(front_car.avoidX!=0||front_car.vehState!=0)){
+                    signed_record_avoidX=front_car.avoidX;
+                    return signed_record_avoidX;
+           }
    }
 
     if((signed_record_avoidX!=0)&&(obs_cur_distance<50))
@@ -4304,17 +4306,20 @@ int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr
 
 //    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
 //                        return 0;
-    if((front_car.avoidX==0)&&(front_car.vehState==0)&&(front_car.front_car_dis<150)&&(vehState==normalRun))
+    if(front_car_id>0)
     {
-        if(PathPoint+300<gpsMapLine.size()){
-            for(int k=PathPoint;k<PathPoint+300;k++){
-                    if((gpsMapLine[k]->mfCurvature>0.02))
-                                     return 20;
+        if((front_car.avoidX==0)&&(front_car.vehState==0)&&(front_car.front_car_dis<150)&&(vehState==normalRun))
+        {
+            if(PathPoint+300<gpsMapLine.size()){
+                for(int k=PathPoint;k<PathPoint+300;k++){
+                        if((gpsMapLine[k]->mfCurvature>0.02))
+                                         return 20;
+                }
+                return 0;
             }
-            return 0;
         }
     }
-    if((obs_cur_distance>100)&&(vehState==normalRun))
+    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
     {
         if(PathPoint+300<gpsMapLine.size()){
             for(int k=PathPoint;k<PathPoint+300;k++){

src/fusion/lidar_radar_fusion/fusion.hpp

@@ -87,10 +87,10 @@ void Get_AssociationMat(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radar
         if(radar_object_array.obj(k).bvalid() == false) continue;
         if(std::find(index.begin(),index.end(),k) !=index.end())
         {
-            std::cout<<"  radar    only "<<std::endl;
+//            std::cout<<"  radar    only "<<std::endl;
             radar_idx.push_back(k);
         } else {
-            std::cout<<"   int   it    "<<std::endl;
+//            std::cout<<"   int   it    "<<std::endl;
         }
     }
     index.clear();
@@ -204,9 +204,9 @@ void RLfusion(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarra
                     iv::fusion::NomalXYZ *nomal_centroid_;
                     float nomal_x = ix*0.2 - xp*0.2;
                     float nomal_y = iy*0.2 - yp*0.2;
-                    float nomal_z = lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.z;
+                    float nomal_z = lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.z;
                     float s = nomal_x*cos(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw);
-                    float t = nomal_x*sin(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector_ptr->at(i).yaw);
+                    float t = nomal_x*sin(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw);
                     nomal_centroid.set_nomal_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.x + s);
                     nomal_centroid.set_nomal_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.y+ t);
                     nomal_centroid_ = fusion_object.add_nomal_centroid();
@@ -271,7 +271,7 @@ void RLfusion(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarra
             }
             iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
             obj->CopyFrom(fusion_obj);
-        }  
+        }
 //        std::cout<<"     RLfusion       end  "<<std::endl;
 }