|
@@ -2449,7 +2449,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
|
|
|
}
|
|
|
}
|
|
|
double dis1 = GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
|
|
|
- double dis2 = GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
|
|
|
+ double dis2 = 10;// GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
|
|
|
+ if(brake_mode==true)
|
|
|
+ {
|
|
|
+ dis2=2;
|
|
|
+ }
|
|
|
if(circleMode)//闭环地图
|
|
|
{
|
|
|
if(dis1<3&&dis2<3&&circleMode) //距离不能太小,太小了可能会错过,所以适当将距离增大一些
|
|
@@ -4565,19 +4569,23 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
|
|
|
{
|
|
|
hasCheckedAvoidLidar = true;
|
|
|
}
|
|
|
-
|
|
|
+ int record_vector=0;
|
|
|
for(int j=0;j<obs_property.size();j++){
|
|
|
if(obs_property[j].obs_distance>100){
|
|
|
+ if((signed_record_avoidX<0)&&(obs_property[j].avoid_distance>0)){
|
|
|
+ break;
|
|
|
+ }
|
|
|
if(abs(obs_property[j].avoid_distance)<record_avoidX){
|
|
|
record_avoidX=abs(obs_property[j].avoid_distance);
|
|
|
signed_record_avoidX=obs_property[j].avoid_distance;
|
|
|
+ record_vector=j;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
|
|
|
{
|
|
|
- int obs_test=abs(signed_record_avoidX)-1;
|
|
|
+ int obs_test=record_vector-1;
|
|
|
|
|
|
if(obs_property[obs_test].obs_distance>100){
|
|
|
signed_record_avoidX-=1;
|