/*------------------------------------------------------------- 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 }
/*------------------------------------------------------------- filterByExclusionAngles -------------------------------------------------------------*/ void C2DRangeFinderAbstract::filterByExclusionAngles( mrpt::obs::CObservation2DRangeScan &obs) const { obs.filterByExclusionAngles( m_lstExclusionAngles ); }
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; }