diff --git a/src/Lidar.cc b/src/Lidar.cc index c065084c..c88a0083 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -211,8 +211,11 @@ void Lidar::ApplyNoise() int index = j * this->RayCount() + i; double range = this->laserBuffer[index*3]; range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range); - range = ignition::math::clamp(range, + if (std::isfinite(range)) + { + range = ignition::math::clamp(range, this->RangeMin(), this->RangeMax()); + } this->laserBuffer[index*3] = range; } }