C++ (Cpp) CObservation2DRangeScan::resizeScan Examples

Example #1

File: COccupancyGridMap2D_simulate.cpp Project: EduFdez/mrpt

// 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 #2

File: CHokuyoURG.cpp Project: EduFdez/mrpt

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;
}