void AudioPolicyService::AudioCommandThread::AudioCommand::dump(char* buffer, size_t size)
{
snprintf(buffer, size, " %02d %06d.%03d %01u %p\n",
mCommand,
(int)ns2s(mTime),
(int)ns2ms(mTime)%1000,
mWaitStatus,
mParam);
}
bool RealLidar2D::measure(const nanosecond& time, const Point2D& pose, Measure &valueSensed, Measure &valueIdeal) noexcept {
if (time - timeLastMeasure_ < samplingTime_)
return false;
auto applyRes = [](double const &value, double const &resolution) -> double {
if (resolution == .0) return value;
return resolution*int(value/resolution);
};
auto rangeLimit = [&](double const &value) -> double {
if (value < rangeMin_) return rangeMin_;
if (rangeMax_ < value) return rangeMax_;
return value;
};
auto angleLimit = [&](double const &value) -> double {
return value<.0? 2.*pi+value : std::fmod(value,2.*pi) ;
};
double const rangeNoise{rangeDistribution(generator)};
double const angleNoise{angleDistribution(generator)};
valueIdeal.m[1] = angleLimit(ns2s(time)*scanSpeed_);
if (scanAngle_ == .0 || valueIdeal.m[1] <= scanAngle_ || 2.*pi-scanAngle_ <= valueIdeal.m[1]) {
Lidar2D const *lidar = this;
valueIdeal.m[0] = lidar->measure(valueIdeal.m[1],pose);
valueSensed.m[0] = applyRes(rangeLimit(valueIdeal.m[0] + rangeNoise),rangeResolution_);
valueSensed.m[1] = applyRes(angleLimit(valueIdeal.m[1] + angleNoise),angleResolution_);
return true;
}
valueIdeal.m[0] = .0;
valueSensed.m[0] = applyRes(rangeLimit(rangeNoise),rangeResolution_);
valueSensed.m[1] = applyRes(angleLimit(valueIdeal.m[1] + angleNoise),angleResolution_);
return true;
}
