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