|
@@ -1907,10 +1907,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
|
|
cout<<"\n====================preAvoid time count finish==================\n"<<endl;
|
|
cout<<"\n====================preAvoid time count finish==================\n"<<endl;
|
|
}
|
|
}
|
|
|
|
|
|
- if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(road_permit_avoid==false)||(front_car_decide_avoid==false)){
|
|
|
|
|
|
+// if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(road_permit_avoid==false)||(front_car_decide_avoid==false)){ //xunluoche,20211203,cxw
|
|
|
|
+ if((obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis+20)) {
|
|
ObsTimeStart=-1;
|
|
ObsTimeStart=-1;
|
|
ObsTimeEnd=-1;
|
|
ObsTimeEnd=-1;
|
|
ObsTimeSpan=-1;
|
|
ObsTimeSpan=-1;
|
|
|
|
+
|
|
avoid_speed_flag=false;
|
|
avoid_speed_flag=false;
|
|
}
|
|
}
|
|
|
|
|
|
@@ -2292,7 +2294,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
|
|
|
|
|
|
transferGpsMode2(gpsMapLine);
|
|
transferGpsMode2(gpsMapLine);
|
|
//巡逻车临时停车逻辑,begin,20211203,cxw
|
|
//巡逻车临时停车逻辑,begin,20211203,cxw
|
|
- unsigned int parkpointnum=-1;
|
|
|
|
|
|
+ int parkpointnum=-1;
|
|
double min_distoparkpoint =8;
|
|
double min_distoparkpoint =8;
|
|
bool parktimeEN = false;
|
|
bool parktimeEN = false;
|
|
QTime parktimer;
|
|
QTime parktimer;
|
|
@@ -3618,7 +3620,8 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
|
|
return -1;
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
- if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
|
|
|
|
|
|
+// if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
|
|
|
|
+ if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],15.0)) && //cunluoche,20211203,cxw
|
|
(checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
|
|
(checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
|
|
{
|
|
{
|
|
roadPre = roadOri;
|
|
roadPre = roadOri;
|
|
@@ -3630,7 +3633,8 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
|
|
for (int i = roadOri + 1;i < roadNow;i++) {
|
|
for (int i = roadOri + 1;i < roadNow;i++) {
|
|
if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
(GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> avoidMinDistanceVector[i])&&
|
|
(GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> avoidMinDistanceVector[i])&&
|
|
- (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
|
|
|
+// (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 15.0)) //20211203,cunluoche ,cxw
|
|
{
|
|
{
|
|
roadPre = i;
|
|
roadPre = i;
|
|
return roadPre;
|
|
return roadPre;
|
|
@@ -3643,7 +3647,8 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
|
|
for (int i = roadOri - 1;i > roadNow;i--) {
|
|
for (int i = roadOri - 1;i > roadNow;i--) {
|
|
if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
(GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
|
|
(GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
|
|
- (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
|
|
|
+// (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 15.0)) //20211203,cunluoche ,cxw
|
|
{
|
|
{
|
|
roadPre = i;
|
|
roadPre = i;
|
|
return roadPre;
|
|
return roadPre;
|