|
@@ -3168,6 +3168,7 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
|
|
|
obsSpeed=obsSd;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
if(obsDistance<500&&obsDistance>0){
|
|
|
std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
|
|
|
}
|
|
@@ -3259,6 +3260,10 @@ void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr
|
|
|
obsSpeed=obsSd;
|
|
|
}
|
|
|
|
|
|
+ if(obsPoint.obs_type==2){
|
|
|
+ obsSpeed=-secSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
if(obsDistance<500&&obsDistance>0){
|
|
|
std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
|
|
|
}
|