// See docs in header
void COccupancyGridMap2D::laserScanSimulator(
mrpt::obs::CObservation2DRangeScan& inout_Scan, const CPose2D& robotPose,
float threshold, size_t N, float noiseStd, unsigned int decimation,
float angleNoiseStd) const
{
MRPT_START
ASSERT_(decimation >= 1);
ASSERT_(N >= 2);
// Sensor pose in global coordinates
CPose3D sensorPose3D = CPose3D(robotPose) + inout_Scan.sensorPose;
// Aproximation: grid is 2D !!!
CPose2D sensorPose(sensorPose3D);
// Scan size:
inout_Scan.resizeScan(N);
double A = sensorPose.phi() +
(inout_Scan.rightToLeft ? -0.5 : +0.5) * inout_Scan.aperture;
const double AA =
(inout_Scan.rightToLeft ? 1.0 : -1.0) * (inout_Scan.aperture / (N - 1));
const float free_thres = 1.0f - threshold;
for (size_t i = 0; i < N; i += decimation, A += AA * decimation)
{
bool valid;
float out_range;
simulateScanRay(
sensorPose.x(), sensorPose.y(), A, out_range, valid,
inout_Scan.maxRange, free_thres, noiseStd, angleNoiseStd);
inout_Scan.setScanRange(i, out_range);
inout_Scan.setScanRangeValidity(i, valid);
}
MRPT_END
}
void CHokuyoURG::doProcessSimple(
bool& outThereIsObservation,
mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
{
outThereIsObservation = false;
hardwareError = false;
// Bound?
if (!ensureStreamIsOpen())
{
m_timeStartUI = 0;
m_timeStartSynchDelay = 0;
hardwareError = true;
return;
}
// Wait for a message:
char rcv_status0, rcv_status1;
int nRanges = m_lastRange - m_firstRange + 1;
int expectedSize = nRanges * 3 + 4;
if (m_intensity) expectedSize += nRanges * 3;
m_rcv_data.clear();
m_rcv_data.reserve(expectedSize + 1000);
m_state = ssWorking;
if (!receiveResponse(rcv_status0, rcv_status1))
{
// No new data
return;
}
// DECODE:
if (rcv_status0 != '0' && rcv_status0 != '9')
{
hardwareError = true;
return;
}
// -----------------------------------------------
// Extract the observation:
// -----------------------------------------------
outObservation.timestamp = mrpt::system::now();
if ((size_t)expectedSize != m_rcv_data.size())
{
MRPT_LOG_ERROR_STREAM(
"[CHokuyoURG::doProcess] ERROR: Expected "
<< expectedSize << " data bytes, received " << m_rcv_data.size()
<< "instead!");
hardwareError = true;
return;
}
// Delay the sync of timestamps due to instability in the constant rate
// during the first few packets.
bool do_timestamp_sync = !m_disable_firmware_timestamp;
if (do_timestamp_sync &&
m_timeStartSynchDelay < MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE)
{
do_timestamp_sync = false;
m_timeStartSynchDelay++;
}
if (do_timestamp_sync)
{
// Extract the timestamp of the sensor:
uint32_t nowUI = ((m_rcv_data[0] - 0x30) << 18) +
((m_rcv_data[1] - 0x30) << 12) +
((m_rcv_data[2] - 0x30) << 6) + (m_rcv_data[3] - 0x30);
uint32_t AtUI = 0;
if (m_timeStartUI == 0)
{
m_timeStartUI = nowUI;
m_timeStartTT = mrpt::system::now();
}
else
AtUI = nowUI - m_timeStartUI;
mrpt::system::TTimeStamp AtDO = mrpt::system::secondsToTimestamp(
AtUI * 1e-3 /* Board time is ms */);
outObservation.timestamp = m_timeStartTT + AtDO;
}
// And the scan ranges:
outObservation.rightToLeft = true;
outObservation.aperture =
nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg;
outObservation.maxRange = m_sensor_info.d_max;
outObservation.stdError = 0.010f;
outObservation.sensorPose = m_sensorPose;
outObservation.sensorLabel = m_sensorLabel;
outObservation.resizeScan(nRanges);
char* ptr = (char*)&m_rcv_data[4];
if (m_intensity) outObservation.setScanHasIntensity(true);
for (int i = 0; i < nRanges; i++)
{
int b1 = (*ptr++) - 0x30;
int b2 = (*ptr++) - 0x30;
int b3 = (*ptr++) - 0x30;
int range_mm = ((b1 << 12) | (b2 << 6) | b3);
outObservation.setScanRange(i, range_mm * 0.001f);
outObservation.setScanRangeValidity(
i, range_mm >= 20 &&
(outObservation.scan[i] <= outObservation.maxRange));
if (m_intensity)
{
int b4 = (*ptr++) - 0x30;
int b5 = (*ptr++) - 0x30;
int b6 = (*ptr++) - 0x30;
outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6));
}
}
// Do filter:
C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
// Do show preview:
C2DRangeFinderAbstract::processPreview(outObservation);
outThereIsObservation = true;
}