/*-------------------------------------------------------------
doProcessSimple
-------------------------------------------------------------*/
void CRoboPeakLidar::doProcessSimple(
bool &outThereIsObservation,
mrpt::obs::CObservation2DRangeScan &outObservation,
bool &hardwareError )
{
#if MRPT_HAS_ROBOPEAK_LIDAR
outThereIsObservation = false;
hardwareError = false;
// Bound?
if (!checkCOMMs())
{
hardwareError = true;
return;
}
rplidar_response_measurement_node_t nodes[360*2];
size_t count = sizeof(nodes)/sizeof(nodes[0]);
// Scan:
const mrpt::system::TTimeStamp tim_scan_start = mrpt::system::now();
u_result op_result = RPLIDAR_DRV->grabScanData(nodes, count);
//const mrpt::system::TTimeStamp tim_scan_end = mrpt::system::now();
//const double scan_duration = mrpt::system::timeDifference(tim_scan_start,tim_scan_end);
// Fill in scan data:
if (op_result == RESULT_OK)
{
op_result = RPLIDAR_DRV->ascendScanData(nodes, count);
if (op_result == RESULT_OK)
{
const size_t angle_compensate_nodes_count = 360;
const size_t angle_compensate_multiple = 1;
int angle_compensate_offset = 0;
rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
outObservation.resizeScanAndAssign(angle_compensate_nodes_count, 0, false);
for(size_t i=0 ; i < count; i++ )
{
if (nodes[i].distance_q2 != 0)
{
float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
int angle_value = (int)(angle * angle_compensate_multiple);
if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
for (size_t j = 0; j < angle_compensate_multiple; j++)
{
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
}
}
}
for(size_t i=0 ; i < angle_compensate_nodes_count; i++ )
{
const float read_value = (float) angle_compensate_nodes[i].distance_q2/4.0f/1000;
outObservation.setScanRange(i, read_value );
outObservation.setScanRangeValidity(i, (read_value>0) );
}
}
// 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;
}