#include <ulaserscan.h>

Inheritance diagram for ULaserScan:

Public Member Functions
void	clear ()

int	countValidPoints (int fromThis, int upToThis)

bool	findNearObstacles (UObstaclePool *obsts, UPoseTime odoPose, double maxRngL, double maxRngR, double minRange, bool outdoorContext, bool horizontalScan, bool ignoreIfFixed, double searchExt)

ULaserPoint *	getData (int index)

int	getDataCnt ()

int	getDataMax ()

ULineSegment	getLineSegmentFit (int left, int right, double variance, UPosition center)

UPosition	getPos (int index)

UPassQual	getQ (int index)

double	getRange (double angle)

int	getRight (int index)

UPose *	getRobotPose ()

UPosRot *	getSensorPose ()

unsigned long	getSerial ()

double	getVarHere (int index)

double	getVarLeft (int index)

double	getVarRight (int index)

void	setMaxValidRange (double value)

void	setPoseAndTime (UTime laserTime, double odoX, double odoY, double odoTheta)

void	setQ (int index, UPassQual value)

void	setQ (int index, int through, UPassQual value)

bool	setScan (ULaserData source, UPose odoPose, UPosRot laserPose)

void	setSerial (const unsigned long scanSerial)

void	setValid (bool value)

void	setVariance (double width, int minCnt)

	ULaserScan ()

Public Member Functions inherited from ULock
bool	lock ()

void	lockInit ()

void	post ()

bool	tryLock ()

bool	tryWait ()

	ULock ()

void	unlock ()

bool	wait ()

	~ULock ()

Public Attributes
ULaserPoint	data [MAX_RANGE_DATA_CNT]

int	dataCnt

UTime	time

bool	valid

Static Public Attributes
static const int	MAX_RANGE_DATA_CNT = 800

Private Attributes
FILE *	logpi

double	rangeMax

UPose	robotPose

UPosRot	sensorPose

unsigned long	serial

Detailed Description

A full laser scan

Constructor & Destructor Documentation

ULaserScan::ULaserScan ( )

Constructor

References clear().

Member Function Documentation

void ULaserScan::clear ( void )

Clear scan to empty

References dataCnt, serial, and valid.

Referenced by ULaserScan().

int ULaserScan::countValidPoints	(	int	fromThis,
		int	upToThis
	)

Count number of valid measurements from 'fromThis' up to, but not including the 'upToThis' measurement. Valid measurements is found by calling 'isValid()'. Returns n or above. 'fromThis' must be lower or equal to 'upToThis', or no count is erformed

References data, and ULaserPoint::isValid().

Referenced by setSerial().

bool ULaserScan::findNearObstacles	(	UObstaclePool *	obsts,
		UPoseTime	odoPose,
		double	maxRngL,
		double	maxRngR,
		double	minRange,
		bool	outdoorContext,
		bool	horizontalScan,
		bool	ignoreIfFixed,
		double	searchExt
	)

Find near obstacles - closer than maxRng

Parameters

obsts	obstacle pool, where obstacles are to be delivered.
odopose	pose of robot at scantime
maxRngL	range to use in left side of scan
maxRngR	range to use in right side of scan
minRange	minimum range to be considered an obstacle - could remove glass refelctions
outdoorContext	use outdoor settings in detecting obstacle separations
horizontalScan	assume scan is from a horizontal laserscan source
ignoreIfFixed	ignore obstacles that overlap fixed (mapped) obstacles.
searchExt	extra search range added to the range limits

Returns: true

References UObstaclePool::addObst(), data, dataCnt, UObstaclePool::getGroup(), UObstaclePool::getGroupsCnt(), UObstaclePool::getObstGrp(), ULaserPoint::getQ(), ULaserPoint::isValid(), UObstaclePool::obstDataUpdated(), PQ_ROUGH, ULaserPoint::range, UObstacleGroup::removeInvalid(), UPoseTime::t, and time.

Referenced by UResPassable::doFullAnalysis(), UResPassable::doObstAnalysis(), and setSerial().

ULaserPoint* ULaserScan::getData ( int index )

inline

Get pointer to laser measured data. NB! no range check

References data.

Referenced by UResPassable::findTopOfRoad(), UResPassable::makePassableIntervals2(), UResPassable::makePassableIntervalsFit(), and setScan().

int ULaserScan::getDataCnt ( )

inline

Get number og measurements

References dataCnt.

Referenced by UResPassable::makePassableIntervalsFit(), and UFunctionPassable::sendFullScan().

int ULaserScan::getDataMax ( )

inline

Get maximum number of slots in point buffer

References MAX_RANGE_DATA_CNT.

Referenced by ULaserObst::getIndexMax().

ULineSegment ULaserScan::getLineSegmentFit	(	int	left,
		int	right,
		double *	variance,
		UPosition *	center
	)

Find line segment returning a best-fit line (2D in x,y) with the endpoints taken as the left and right measuremnt projected to the line. If the variance is not NULL, the variance of the fit is returned.

References data, U2Dline::getOnLine(), getPos(), LASER_MAX_RANGE, ULineSegment::length, MAX_RANGE_DATA_CNT, ULaserPoint::pos, ULaserPoint::range, UPosition::scale(), UPosition::scaled(), U2Dline::set(), UPosition::set(), ULineSegment::setFromPoints(), UPosition::x, and UPosition::y.

Referenced by ULaserObst::findEdgeObstacle(), ULaserPi::setInterval(), and setSerial().

UPosition ULaserScan::getPos ( int index )

inline

Get 3D position of a given data point - by index number

References data, dataCnt, getRange(), and ULaserPoint::pos.

Referenced by UObstacleGroupLaser::addObst(), UResPassable::combineNearIntervals(), ULaserObst::findEdgeObstacle(), getLineSegmentFit(), UObstaclePool::pointsToPolygon(), and ULaserPi::setCenter().

UPassQual ULaserScan::getQ ( int index )

Get passable quality for this measurement

References data, dataCnt, ULaserPoint::getQ(), and PQ_UNKNOWN.

Referenced by ULaserPi::findEdgeObstacles(), and UFunctionPassable::handlePass().

double ULaserScan::getRange ( double angle )

Get range for this angle. angle is in radians and 0.0 is in front, positive is CCV.

References data, dataCnt, ULaserPoint::range, and roundi().

Referenced by getPos().

int ULaserScan::getRight ( int index )

Get index into scan that has this index as left limit

References data, dataCnt, maxi(), and ULaserPoint::varToL.

Referenced by UFunctionPassable::handlePass(), and setSerial().

UPose* ULaserScan::getRobotPose ( )

inline

Get robot pose

References robotPose.

Referenced by UFunctionPassable::handlePass(), and UFunctionPassable::sendFullScan().

UPosRot* ULaserScan::getSensorPose ( )

inline

Get sensor pose

References sensorPose.

Referenced by UObstacleGroupLaser::addObst(), and UObstaclePool::pointsToPolygon().

unsigned long ULaserScan::getSerial ( )

inline

Get scan serial number

References serial.

Referenced by UFunctionPassable::handlePass(), UResPassable::makePassableIntervalsFit(), ULaserPi::print(), and UFunctionPassable::sendFullScan().

double ULaserScan::getVarHere ( int index )

Returns average of variance to th left and right of this point. That is approx in a robot width area.

References getVarLeft(), and getVarRight().

double ULaserScan::getVarLeft ( int index )

Get variance in the area to the right

References data, dataCnt, and ULaserPoint::varL.

Referenced by getVarHere(), and setSerial().

double ULaserScan::getVarRight ( int index )

Get variance in the area to the right

References data, dataCnt, maxi(), ULaserPoint::varL, and ULaserPoint::varToL.

Referenced by getVarHere(), and setSerial().

void ULaserScan::setMaxValidRange ( double value )

inline

References rangeMax, and setScan().

Referenced by setScan().

void ULaserScan::setPoseAndTime	(	UTime	laserTime,
		double	odoX,
		double	odoY,
		double	odoTheta
	)

Set position, speed and detection time

void ULaserScan::setQ	(	int	index,
		UPassQual	value
	)

Set passage quality for one position.

to one of PQ_EASY, PQ_ROUGH, PQ_NOT, PQ_OBST, PQ_UNKNOWN

References data, dataCnt, and ULaserPoint::setQ().

Referenced by UResPassable::addPassableInterval(), ULaserPi::findEdgeObstacles(), UFunctionPassable::handlePass(), and UResPassable::makePassableIntervals2().

void ULaserScan::setQ	(	int	index,
		int	through,
		UPassQual	value
	)

Set passage quality for one position.

to one of PQ_EASY, PQ_ROUGH, PQ_NOT, PQ_OBST, PQ_UNKNOWN. in the interval to and including 'throught'

References data, dataCnt, mini(), and ULaserPoint::setQ().

bool ULaserScan::setScan	(	ULaserData *	source,
		UPose	odoPose,
		UPosRot *	laserPose
	)

Set scan measurements from this source structure.

References a, dataCnt, ULaserData::getAngleRad(), ULaserData::getAngleStart(), getData(), ULaserData::getMaxValidRange(), UPosRot::getPhi(), ULaserData::getRangeCnt(), ULaserData::getRangeMeter(), ULaserData::getScanTime(), ULaserData::getSerial(), UPosRot::getZ(), ULock::lock(), robotPose, sensorPose, serial, setMaxValidRange(), time, ULock::unlock(), and valid.

Referenced by setMaxValidRange(), and UResPassable::setScan().

void ULaserScan::setSerial ( const unsigned long scanSerial )

inline

Get scan serial number

References countValidPoints(), findNearObstacles(), getLineSegmentFit(), getRight(), getVarLeft(), getVarRight(), and serial.

void ULaserScan::setValid ( bool value )

Set vscan is valid and increment the scan-counter

References valid.

void ULaserScan::setVariance	(	double	width,
		int	minCnt
	)

Calculate variance of an interval using line-fit method. The variance is saved at the right extreme of the interval, and will not interfere with the fixed-mask variance. An integrated (running average) of a larger interval is an attempt to get a better estimate of the smootheness of a larger passable (e.g. asphalt) area. This value starts decreasing from maxIntegratedVariance, and is never below the found fit variance.

References U2Dline::A(), U2Dline::B(), data, dataCnt, UPosition::dist(), U2Dline::distanceSigned(), ULaserPoint::distLeft, ULaserPoint::distLeftPkt, ULaserPoint::distRight, ULaserPoint::distRightPkt, ULaserPoint::isValid(), MAX_RANGE_DATA_CNT, ULaserPoint::pos, ULaserPoint::range, U2Dline::setXline(), U2Dline::setYline(), sqr(), ULaserPoint::tilt, U2Dline::variance(), ULaserPoint::varL, ULaserPoint::varLine, ULaserPoint::varToL, UPosition::x, and UPosition::y.