#include <uresposehist.h>

Inheritance diagram for UResPoseHist:

Public Member Functions
bool	addIfNeeded (UPoseTVQ pose, int source)

void	addPoseHist (UPoseTVQ poseAtTime)

void	addPoseHist (UPose pose, UTime atTime, double v=0.0, double qual=-1.0)

void	clear ()

void	closeLogfile ()

void	createBaseVar ()

double	getHistHeading (double closeBy, double farAway, const UPoseTime ref, double histAge)

const char *	getLogfileName ()

double	getMinDist ()

double	getMinTheta ()

double	getMinTime ()

UPoseTime	getNewest (double *velocity)

UPoseTVQ	getNewest ()

int	getNewestIndex ()

int	getNewestSource ()

UPoseTime	getOdoPoseOrigin ()

UPoseTime	getOldest ()

UTime	getOldestTime ()

UPoseTVQ	getPose (int index)

const UPoseTVQ *	getPoseAtIndex (int index) const

UPoseTVQ	getPoseAtTime (UTime time)

double	getPoseFitAtDistance (double closeBy, double farAway, const UPose ref, UPose closeFitPose)

bool	getPoseNearDistance (const double away, const UPose ref, UPoseTVQ closer, UPoseTVQ *moreDistant)

bool	getPoseNearTime (UTime atTime, UPoseTVQ justBefore, UPoseTVQ justAfter)

int	getPosesCnt ()

char *	getReplayFileName (char *fn, const int fnCnt)

int	getReplayLogLine ()

UTime	getReplayTimeNext ()

UTime	getReplayTimeNow ()

bool	getReplayTimestamp2 ()

UTime	getTimeAtDistance (double away)

bool	gotAllResources (char *missingThese, int missingTheseCnt)

bool	isLogfileOpen ()

bool	isReplayFileOpen ()

virtual bool	methodCall (const char name, const char paramOrder, char *strings, const double doubles, double value, UDataBase returnStruct=NULL, int returnStructCnt=NULL)

bool	openLogfile ()

virtual const char *	print (const char preString, char buff, int buffCnt)

bool	processUtmFile (FILE fs, FILE fd, bool doLine=true)

bool	replayStep (int steps)

void	saveToLog (FILE *logf)

void	setMinDist (double value)

void	setMinTheta (double value)

void	setMinTime (double value)

bool	setReplay (bool value)

void	setReplayTimestamp2 (bool value)

virtual void	setResID (const char *aliasName)

virtual void	setResID (const char *id, const int version)

bool	setResource (UResBase *resource, bool remove)

bool	toKml (char *aKmlName, const int MRL)

	UResPoseHist ()

virtual	~UResPoseHist ()

Public Member Functions inherited from UResVarPool
int	addMethod (const char formalName, const char paramTypes, const char *comment)

int	addMethodV (const char formalName, const char paramTypes, const char *comment)

UVarPool *	addStruct (const char name, const char comment, bool copy=false)

UVariable *	addVar (const char name, const double initialValue, const char type, const char *comment)

UVariable *	addVar (const char name, const char initialValue, const char type, const char comment)

UVariable *	addVarA (const char name, const char initialValue, const char type, const char comment)

bool	callGlobal (const char name, const char paramOrder, char *strings, const double doubles, double value, UDataBase returnStruct, int returnStructCnt)

bool	callGlobalV (const char name, const char paramOrder, UVariable params, UDataBase returnStruct, int *returnStructCnt)
	Call a method from the root var pool scope It may be in the structure direct (no '.') or in any sub-structure that the 'name.subname' indicate. More...

bool	callLocal (const char name, const char paramOrder, char *strings, const double doubles, double value, UDataBase returnStruct, int returnStructCnt)

bool	callVS (const char function, const char stringParam)

int	callVSCD (const char function, const char strPar, UDataBase *data, int cooSys)

void	createBaseVar ()

virtual bool	createVarSpace (const int varCnt, const int structCnt, const int methCnt, const char *note, bool copy=false)

bool	getGlobalString (const char name, const char *value)

bool	getGlobalValue (const char name, double value)

bool	getGlobalValue (const char name, const char *value)

bool	getGlobalValue (const char name, bool value)

bool	getGlobalValue (const char name, UTime value)

bool	getLocalValue (const char name, double value)
	get a value in this variable tree More...

bool	getLocalValue (const char name, bool value)

bool	getLocalValue (const char name, UTime value)

double	getLocalValue (int idx)

UPosition	getLocalValue3D (int idx)

UPosRot	getLocalValue6D (int idx)

bool	getLocalValueBool (int idx)

int	getLocalValueInt (int idx)

UTime	getLocalValueTime (int idx)

const char *	getLogFileName ()

const char *	getVarDescription (int idx)

UVarCalc *	getVarPool ()

bool	isLogFileOpen ()

bool	isLogFileOpen (const char *structName)

bool	isVarPoolUpdated (int lastCnt, int *newCnt)

void	logFileClose ()

bool	logFileOpen ()

void	logFileStart (const char *structName, bool open)

bool	setGlobalVar (const char *name, const double value, bool mayAdd)
	Assigns a value to a variable. The variable may be a part of a structure. More...

bool	setLocalVar (int idx, double value, const int element)

bool	setLocalVar (const char *name, const double value, bool mayAdd)
	Assigns a value to a variable. The variable may be a part of a structure. More...

bool	setLocalVar3D (int idx, UPosition *value)

bool	setLocalVar6D (int idx, UPosRot *value)

bool	setLocalVarAdd (int idx, double value, const int element)

bool	setLocalVarPose (int idx, UPose *value)

void	setVarPool (UVarCalc *sharedVarPool)

int	updateQTvars (int structIdx)

	UResVarPool ()

virtual	~UResVarPool ()

Public Member Functions inherited from UResBase
void	createReplayVar (UVarPool *pool)

virtual void	createResources ()

UCmdExe *	getCorePointer ()

char *	getLogFileName (char *fn, const int fnCnt)

int	getResFuncIdx ()

const char *	getResID ()

int	getResVersion ()

bool	isA (const char *idStr)

bool	isAlsoA (const char *id)

virtual void	print (const char *preString)

virtual void	saveSettings ()

void	setCorePointer (UCmdExe *serverCore)

void	setResFuncIdx (int idx)

virtual void	stop (bool)

virtual void	updateReplayStatus ()

	UResBase ()

virtual	~UResBase ()

Public Member Functions inherited from UReplay
void	addChild (UReplay *child)

virtual bool	decodeReplayLine (char *)

char *	getLogFileName (char *fn, const int fnCnt)

char *	getReplayFileName (char *fn, const int fnCnt)

int	getReplayLogLine ()

bool	isReplay ()

bool	isReplayFileOpen ()

void	removeChild (UReplay *child)

virtual void	replayAdvanceTime (UTime untilTime)

void	replaySetBaseFileName (const char name, const char preName="")

void	replaySetFileName (const char *name)

int	replayStep (int steps)

virtual bool	replayToTime (UTime untilTime)

void	setParent (UReplay *parent)

bool	setReplay (bool value)

	UReplay ()

virtual	~UReplay ()

Public Member Functions inherited from ULock
bool	lock ()

void	lockInit ()

void	post ()

bool	tryLock ()

bool	tryWait ()

	ULock ()

void	unlock ()

bool	wait ()

	~ULock ()

Public Member Functions inherited from UVarMethodImplement
virtual bool	methodCallV (const char name, const char paramOrder, UVariable params[], UDataBase returnStruct, int returnStructCnt)

virtual	~UVarMethodImplement ()

Public Member Functions inherited from UServerPush
int	addPushCommand (UServerInMsg *msg)

virtual void	callGotNewDataWithObject ()

bool	doPushCall (UServerPushElement pe, const char value)

void	flushClientCmds (int clientIdx)

UServerPushImplement *	getImplementor ()

int	getPushCmdCnt (int cmdCnt, int callCnt)

UServerPushQueue *	getPushQueue ()

bool	gotCmdExe ()

void	gotNewData (void *pData)

bool	isUpdated (int lastCnt, int *newCnt)

bool	needNewData ()

void	print (const char *preString)

void	print (const char preString, char buff, int buffCnt)

void	servicePendingPushCmds ()

void	setCmdExe (UServerPushImplement *executor)

void	setImplementor (UServerPushImplement *executor)

bool	setResource (UResBase *resource, bool remove)

bool	setUpdated (const char *value)
	Add watch object if needed. An image or other object is updated. set object as updated, with an extra qualifier, that may be used as a parameter in a push call. this push list is then added to the to the push list examined by the server thread. when server thread reach this push list it will call to get a possible associated data structure by a call to needNewData() whish is expected to call gotNewData(void * data_structure) - but now in the server thread. More...

	UServerPush ()

virtual	~UServerPush ()

Static Public Member Functions
static const char *	getMapPoseID ()

static const char *	getOdoPoseID ()

static const char *	getUtmPoseID ()

Static Public Member Functions inherited from UResVarPool
static const char *	getResClassID ()

Static Public Attributes
static const int	SOURCE_OTHER_SERVER = -2

static const int	SOURCE_REPLAY = -3

Protected Member Functions
void	setPoseResult (bool found, UPoseTVQ pose1, double value, UDataBase returnStruct, int returnStructCnt)

void	updateVarPool (double tripDist)

Protected Member Functions inherited from UResVarPool
void	setDescription (const char *note, bool copy=false)

Protected Member Functions inherited from UResBase
UResBase *	getStaticResource (const char *resName, bool mayCreate, bool staticOnly=true)
	get a pointer to a static resource. More...

Protected Attributes
ULogFile	logPose

int	newest

int	newestSource

UResPoseHist *	odoPoseHist

UPoseTime	odoPoseOrigin

UPoseTVQ	poseNew

UPoseTVQ	poses [MAX_HIST_POSES]

int	posesCnt

FILE *	replayFile

char	replayLine [MAX_LOG_LINE_LENGTH]

int	replayLogLine

bool	replayTime2

UVariable *	varCalcPose

UVariable *	varEstHead
	index to estimate heading variable */ More...

UVariable *	varEstVel
	index to estimate velocity */ More...

UVariable *	varMinDist

UVariable *	varMinTime

UVariable *	varMinTurn

UVariable *	varOdoPoseOrigin
	origin of odometry pose in this coordinate system More...

UVariable *	varPose

UVariable *	varPoseh5m

UVariable *	varPoseh5mUse

UVariable *	varQ

UVariable *	varTime

UVariable *	varTrip

UVariable *	varTripA

UVariable *	varTripB

UVariable *	varTripTime

UVariable *	varTripTimeA

UVariable *	varTripTimeB

UVariable *	varVel

Protected Attributes inherited from UResBase
int	resFuncIdx

char	resID [MAX_RESOURCE_ID_LENGTH]

char	resIsAlso [MAX_RESOURCE_ISALSO_LENGTH]

int	resVersion

Protected Attributes inherited from UReplay
bool	replay

FILE *	replayFile

char	replayFileName [REPLAY_FILE_NAME_LENGTH]
	replay filename More...

char	replayLine [MAX_LOG_LINE_LENGTH]

int	replayLogLine

UReplay *	replayParent

UTime	replayTimeNext

Static Protected Attributes
static const int	MAX_LOG_LINE_LENGTH = 200

Static Protected Attributes inherited from UReplay
static const int	MAX_LOG_LINE_LENGTH = 10000

static const int	REPLAY_FILE_NAME_LENGTH = 264
	max length of replay filename More...

Private Member Functions
virtual bool	replayStep ()

Additional Inherited Members
Public Attributes inherited from UReplay
bool	replayTimeAdvancePending

UTime	replayTimeNow

Detailed Description

Class to hold some 50 meter of pose history for drive calculations

Author: Christian Andersen

Constructor & Destructor Documentation

UResPoseHist::UResPoseHist ( )

inline

Constructor

References UPoseTime::clear(), UPoseTVQ::clear(), createBaseVar(), newest, newestSource, odoPoseHist, odoPoseOrigin, poseNew, posesCnt, UResBase::replay, replayFile, replayLine, replayLogLine, replayTime2, setResID(), UTime::setTime(), UPoseTime::t, and ~UResPoseHist().

UResPoseHist::~UResPoseHist ( )

virtual

Constructor

References closeLogfile().

Referenced by UResPoseHist().

Member Function Documentation

bool UResPoseHist::addIfNeeded	(	UPoseTVQ	pose,
		int	source
	)

Add this pose to the pose history - if needed, i.e. distance or heading change is above a limit. Returns true if position is added to history poses.

References absf(), addPoseHist(), UPoseTime::clear(), UPose::getDistance(), UPose::getHeadingDiff(), UPose::getMapToPosePose(), getMinDist(), getMinTheta(), getMinTime(), getPose(), getPoseAtTime(), UPose::getPoseToMapPose(), UTime::getSec(), UVariable::getValueBool(), UPose::h, limitToPi(), newestSource, odoPoseHist, odoPoseOrigin, poseNew, posesCnt, UResBase::replay, UVariable::setPose(), SOURCE_REPLAY, UPoseTime::t, updateVarPool(), varEstHead, varEstVel, varOdoPoseOrigin, UPoseTVQ::vel, UPose::x, and UPose::y.

Referenced by UResSmrIf::eventGpsUpdate(), UResSmrIf::eventPoseUpdated(), getMapPoseID(), UResIfVar::handlePoseHist(), UFunctionPoseHist::handlePoseHistCommand(), UFunctionLaser::handleScanGetCommand(), UResLocater::locate(), replayStep(), UResLocater::set_state(), UResLocater::set_utmstate(), UResLocater::setUtmPoseToMapPose(), UServerClient::trapMessage(), UFuncLoca2::updateStatus(), and UFuncLocalize::updateStatus().

void UResPoseHist::addPoseHist ( UPoseTVQ poseAtTime )

Add a history pose (in cronological order)

References UPose::getDistance(), ULogFile::getF(), UTime::getMicrosec(), getPose(), UTime::getSec(), UPose::h, ULogFile::isOpen(), ULock::lock(), logPose, MAX_HIST_POSES, newest, UTime::now(), poseNew, poses, posesCnt, UPoseTVQ::q, UServerPush::setUpdated(), UPoseTime::t, ULock::unlock(), updateVarPool(), UPoseTVQ::vel, UPose::x, and UPose::y.

Referenced by addIfNeeded(), addPoseHist(), and getMapPoseID().

void UResPoseHist::addPoseHist	(	UPose	pose,
		UTime	atTime,
		double	v = `0.0`,
		double	qual = `-1.0`
	)

Add a history pose (in cronological order)

References addPoseHist().

void UResPoseHist::clear ( void )

Clear current pose history

References newest, and posesCnt.

Referenced by getMapPoseID(), UNavView::setRefSystemsHere(), and setReplay().

void UResPoseHist::closeLogfile ( )

Close the logfile

References ULogFile::closeLog(), ULock::lock(), logPose, and ULock::unlock().

Referenced by getReplayLogLine(), UFunctionPoseHist::handlePoseHistCommand(), and ~UResPoseHist().

void UResPoseHist::createBaseVar ( )

Create initial var-pool variables

References UResVarPool::addMethod(), UResVarPool::addVar(), varCalcPose, varEstHead, varEstVel, varMinDist, varMinTime, varMinTurn, varOdoPoseOrigin, varPose, varPoseh5m, varPoseh5mUse, varQ, varTime, varTrip, varTripA, varTripB, varTripTime, varTripTimeA, varTripTimeB, and varVel.

Referenced by getMapPoseID(), and UResPoseHist().

double UResPoseHist::getHistHeading	(	double	closeBy,
		double	farAway,
		const UPoseTime *	ref,
		double *	histAge
	)

Get heading over a distance in pose history from 'closeBy' to 'farAway' (in meters) relative to the 'ref' pose. Returns the general heading over this distance (in radians). If 'histAge' != NULL, then the age of the last

References UPose::h, ULock::lock(), newest, poses, posesCnt, UPoseTime::t, ULock::unlock(), UPose::x, and UPose::y.

Referenced by getPoseAtIndex(), UFuncLinefinder::houghTest(), and methodCall().

const char* UResPoseHist::getLogfileName ( )

inline

Get logfile name

References ULogFile::getLogFileName(), and logPose.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

static const char* UResPoseHist::getMapPoseID ( )

inlinestatic

Constant name for defined alias for pose hist - from localization based on map coordinate system - may differ from utm

References addIfNeeded(), addPoseHist(), clear(), createBaseVar(), getOldest(), getPoseAtTime(), and UResBase::time.

Referenced by UResIfVar::handleNewData(), UResIfVar::handlePoseHist(), UCmdExe::loadStaticModule(), UNavPaint::setResource(), UFuncView::setResource(), UResAvoid::setResource(), UResIfVar::setResource(), UResDisp::setResource(), UNavView::setResource(), UResGps::setResource(), and UFuncLaserBox::setResource().

double UResPoseHist::getMinDist ( )

inline

Get minimum distance required to store a pose history pose

References UVariable::getValued(), and varMinDist.

Referenced by addIfNeeded(), and UFunctionPoseHist::handlePoseHistCommand().

double UResPoseHist::getMinTheta ( )

inline

Get minimum distance required to store a pose history pose

References UVariable::getValued(), and varMinTurn.

Referenced by addIfNeeded(), and UFunctionPoseHist::handlePoseHistCommand().

double UResPoseHist::getMinTime ( )

inline

Get minimum distance required to store a pose history pose

References UVariable::getValued(), and varMinTime.

Referenced by addIfNeeded(), and UFunctionPoseHist::handlePoseHistCommand().

UPoseTime UResPoseHist::getNewest ( double * velocity )

inline

Get newest stored position. If storage is empty time is zero.

References UPoseTVQ::getPoseTime(), poseNew, and UPoseTVQ::vel.

Referenced by UNamedBoxes::addBox(), UFuncPpl::calculateVars(), UResAvoid::codeXmlRobotFront(), UResPassable::doObstAnalysis(), UResDrivePos::driveOdo(), UResRoadDrive::findExitPose(), UResAvoid::findPathToHere(), UResDrivePos::getCurrentPose(), UResAvoid::getCurrentPose(), UFunctionAvoid::handleAvoid(), Funcavoid::handleCommand(), UFuncObj3d::handleCommand(), UFuncPpl::handleCommand(), UFunctionPassable::handleObstGet(), UFunctionPoseHist::handlePoseHistCommand(), UFunctionAvoid::handlePoseToPose(), UFuncLinefinder::houghTest(), UFunctionPoseHist::listPoses(), UResDrivePos::methodCall(), UResAvoid::methodCall(), UResAvoid::methodCallV(), UNavPaint::paint(), UNavView::paint(), UResObj3d::paintGndPlane(), UNavPaint::paintOdoDataText(), UNavPaint::printRefSystems(), UFuncPpl::robotToOdoCooTransf(), UResSmrCtl::run(), UResSmrCtl::sendNewManoeuvreToSMR(), UFunctionPassable::sendRoadLines(), UResLocater::set_poselast(), UResLocater::set_state(), UResLocater::set_utmstate(), UNavPaint::setRefSystemsHere(), UResLocater::setUtmPoseToMapPose(), updateDisplacement(), UFuncLoca2::updateDisplacement(), and UResObstacle::updateMappedObstacles().

UPoseTVQ UResPoseHist::getNewest ( )

inline

Get newest available pose

References poseNew.

Referenced by methodCall().

int UResPoseHist::getNewestIndex ( )

inline

Get index to newest saved pose

References newest.

Referenced by UPaintPoseHist::paint().

int UResPoseHist::getNewestSource ( )

inline

Get source of newest stored position (a client number).

References getOldestTime(), getPoseNearTime(), and newestSource.

Referenced by UResIfVar::handlePoseHist(), and UFunctionPoseHist::handlePoseHistCommand().

static const char* UResPoseHist::getOdoPoseID ( )

inlinestatic

Constant name for defined alias for pose hist - odometry based on whell, INS or the like

UPoseTime UResPoseHist::getOdoPoseOrigin ( )

inline

get position of odometry origin in this coordinate system

References odoPoseOrigin, replayStep(), setPoseResult(), and updateVarPool().

Referenced by UFuncLoca2::correlateLines(), UResPassable::doObstAnalysis(), and UResObstacle::updateMappedObstacles().

UPoseTime UResPoseHist::getOldest ( )

Get time of oldest data

References ULock::lock(), MAX_HIST_POSES, newest, poses, posesCnt, and ULock::unlock().

Referenced by getMapPoseID(), getOldestTime(), and UFunctionPoseHist::handlePoseHistCommand().

UTime UResPoseHist::getOldestTime ( )

Get Oldest supportet time

References getOldest(), UTime::Now(), posesCnt, and UPoseTime::t.

Referenced by getNewestSource().

UPoseTVQ UResPoseHist::getPose ( int index )

Get a history pose by index from the newest and back - NB! unsafe (if not locked), as element at index may have changed - no lock using this function. (Relative safe for all the newer poses, but especially the older ones may be overwritten.)

References MAX_HIST_POSES, newest, and poses.

Referenced by addIfNeeded(), addPoseHist(), getPoseAtIndex(), UFunctionPoseHist::listPoses(), UPaintPoseHist::paint(), UNavPaint::paintPoseHistLine(), and saveToLog().

const UPoseTVQ* UResPoseHist::getPoseAtIndex ( int index ) const

inline

get pose at this index to the circulat queue

References getHistHeading(), getPose(), getPoseFitAtDistance(), getPoseNearDistance(), getTimeAtDistance(), MAX_HIST_POSES, poses, print(), and saveToLog().

UPoseTVQ UResPoseHist::getPoseAtTime ( UTime time )

Get the nearest pose at this time.

Parameters

time	is the time where pose is requested.

Returns: pos if it is available - interpolated from stored poses. if not available (time too new) and is the odometry pose, then the newest available pose is returned. if not available (too new) and not odometry, then the newest pose is extended using the odometry relation, to resulting pose when requesting the pose from the odometry pose store, and converted to the requested coordinate system. The returned time is the used pose-time. The returned 'q' and 'v' is the newest available quality from this coordinate system, even if extended by odometry. (the 'getNewest()' will get the available newest update to this coordinate system.

References UPoseTime::getPoseAtTime(), getPoseNearTime(), UPose::getPoseToMapPose(), odoPoseHist, odoPoseOrigin, UPoseTVQ::q, UPoseTime::t, and UPoseTVQ::vel.

Referenced by addIfNeeded(), UResObj3d::do3dCloudFromSvs(), UResObj3d::do3dVoxels(), UNamedBoxes::doDetect(), UFuncCropRow::findPlants(), UFuncTracking::FirstImage(), UFuncEfLine::getCurrentOdoPose(), getMapPoseID(), UFuncRoseBot::handleCommand(), UFuncCropRow::handleCommand(), UFuncLocalize::handleLocalize(), UFuncLocalize::handleLocalizeMHF(), UFuncLocalize::handleLocalizeUKF(), UFunctionPassable::handlePass(), UFunctionLaser::handleScanGetCommand(), UResLaserIfSf::handleSF(), UFuncLinefinder::houghTest(), UResLocater::locate(), UFuncBall::makeDisplayPolygon(), UFuncImSeg::makeDisplayPolygon(), UResLobst::methodCallV(), UNavPaint::paint(), UNavView::paint(), UNavPaint::paintHistScan(), UNavPaint::paintOdoDataText(), UNavView::paintOdoDataText(), UNavView::paintPointClouds(), UNavView::paintScansNewest(), UNavPaint::printRefSystems(), UResLobst::sendAsObstacles(), UNavPaint::setRefSystemsHere(), UNavView::setRefSystemsHere(), UFuncStraightLine::toRobot(), UFuncStraightLine::toWorld(), UFuncTracking::TrackObjects(), and UResV360::update().

double UResPoseHist::getPoseFitAtDistance	(	double	closeBy,
		double	farAway,
		const UPose *	ref,
		UPose *	closeFitPose
	)

Get Bet fit line from this distance interval. Return the fit variance, but negative (-1.0) if no result. Returns the close position, adjusted to the fittet line.

References U2Dlined::fit(), U2Dlined::getOnLine(), ULock::lock(), newest, poses, posesCnt, UPose::set(), ULock::unlock(), UPose::x, and UPose::y.

Referenced by getPoseAtIndex(), methodCall(), and updateVarPool().

bool UResPoseHist::getPoseNearDistance	(	const double	away,
		const UPose *	ref,
		UPoseTVQ *	closer,
		UPoseTVQ *	moreDistant
	)

Get the two nearest poses this distance away from the reference position 'ref'.

References ULock::lock(), newest, poses, posesCnt, ULock::unlock(), UPose::x, and UPose::y.

Referenced by getPoseAtIndex(), getTimeAtDistance(), UFunctionPoseHist::handlePoseHistCommand(), and methodCall().

bool UResPoseHist::getPoseNearTime	(	UTime	atTime,
		UPoseTVQ *	justBefore,
		UPoseTVQ *	justAfter
	)

Get the two nearst poses to time

References ULock::lock(), newest, poseNew, poses, posesCnt, UPoseTime::t, and ULock::unlock().

Referenced by getNewestSource(), getPoseAtTime(), UFunctionPoseHist::handlePoseHistCommand(), methodCall(), updateDisplacement(), and UFuncLoca2::updateDisplacement().

int UResPoseHist::getPosesCnt ( )

inline

Get number of poses in pose history

References posesCnt.

Referenced by UFunctionPoseHist::handlePoseHistCommand(), UFunctionPoseHist::listPoses(), UPaintPoseHist::paint(), and UNavPaint::paintPoseHistLine().

char * UResPoseHist::getReplayFileName	(	char *	fn,
		const int	fnCnt
	)

Get the full replay logfile name. Returns pointer to fn[] string.

References ULogFile::getLogName(), logPose, and replayPath.

Referenced by getReplayLogLine(), UFunctionPoseHist::handlePoseHistCommand(), setReplay(), and toKml().

int UResPoseHist::getReplayLogLine ( )

inline

Get current line number in replay logfile

References closeLogfile(), getReplayFileName(), methodCall(), openLogfile(), replayLogLine, and replayStep().

Referenced by UFunctionPoseHist::handlePoseHistCommand().

UTime UResPoseHist::getReplayTimeNext ( )

inline

Get replat time next

References UReplay::replayTimeNext.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

UTime UResPoseHist::getReplayTimeNow ( )

inline

Get replat time now

References UReplay::replayTimeNow.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

bool UResPoseHist::getReplayTimestamp2 ( )

inline

Get value of replay timestamp source (timestamp 1 or 2 - in logfile).

Returns: true if timestamp source is 2.

References processUtmFile(), replayTime2, and toKml().

Referenced by UFunctionPoseHist::handlePoseHistCommand().

UTime UResPoseHist::getTimeAtDistance ( double away )

Get time, when robot were (at least) this distance from newest position. If a pose at this distance is not in buffer, then the returned tme is zero.

References getPoseNearDistance(), ULock::lock(), newest, poses, posesCnt, UTime::setTime(), UPoseTime::t, and ULock::unlock().

Referenced by getPoseAtIndex().

static const char* UResPoseHist::getUtmPoseID ( )

inlinestatic

Constant name for defined alias for pose hist - UTM based on GPS source - may jump

Referenced by UResIfVar::handleNewData(), UResIfVar::handlePoseHist(), UCmdExe::loadStaticModule(), UFunctionLaser::setResource(), UNavPaint::setResource(), UFuncView::setResource(), UResIfVar::setResource(), UResSmrIf::setResource(), UResDisp::setResource(), UNavView::setResource(), UResGps::setResource(), and UFuncLaserBox::setResource().

bool UResPoseHist::gotAllResources	(	char *	missingThese,
		int	missingTheseCnt
	)

virtual

Test if pool has all needed resources.

Reimplemented from UResVarPool.

References UCmdExe::getResClassID(), UResBase::gotAllResources(), and UServerPush::gotCmdExe().

Referenced by setResID().

bool UResPoseHist::isLogfileOpen ( )

inline

Is logfile open

References ULogFile::isOpen(), and logPose.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

bool UResPoseHist::isReplayFileOpen ( )

inline

is replay file open

References replayFile.

Referenced by UFunctionPoseHist::handlePoseHistCommand(), and toKml().

bool UResPoseHist::methodCall	(	const char *	name,
		const char *	paramOrder,
		char **	strings,
		const double *	doubles,
		double *	value,
		UDataBase **	returnStruct = `NULL`,
		int *	returnStructCnt = `NULL`
	)

virtual

Replay until just before this time The varPool has methods, and a call to one of these are needed. Do the call now and return (a double sized) result in 'value' and return true if the method call is allowed. May return struct values in the 'returnStruct' pointer list (when the pointer list is not NULL), and in this case the 'returnStructCnt' is set to the maximum number of pointers available and should be modified to the number of valid pointers returned. The method should return true if the method exist (with this parameter list)

Reimplemented from UResVarPool.

References UPose::getDistance(), getHistHeading(), getNewest(), getPoseFitAtDistance(), getPoseNearDistance(), getPoseNearTime(), UPose::h, limitToPi(), poseNew, UPoseTVQ::q, setPoseResult(), UTime::setTime(), UPoseTime::t, UPoseTVQ::vel, UPose::x, and UPose::y.

Referenced by getReplayLogLine().

bool UResPoseHist::openLogfile ( )

Open pose history logfile, logs whenever a pose is stored in the pose history resource. the file is 'aliasName'.log and is stored in default dataPath. The format is time x y h, like:
123456789.111222 130.122 64.122 3.14076
Returns true if logfile is open. If logfile is already open, then it is closed and reopened.

References logPose, and ULogFile::openLog().

Referenced by getReplayLogLine(), and UFunctionPoseHist::handlePoseHistCommand().

const char * UResPoseHist::print	(	const char *	preString,
		char *	buff,
		int	buffCnt
	)

virtual

Print the (short) status of resource - to a string buffer

Reimplemented from UResVarPool.

References posesCnt.

Referenced by getPoseAtIndex().

bool UResPoseHist::processUtmFile	(	FILE *	fs,
		FILE *	fd,
		bool	doLine = `true`
	)

Convert source utmPose file to kml for google earth.

Parameters

fs	is a source file handle with UTM coordinates source format: time, east, north, heading (rad), speed, quality (gps-mode), (extra timestamp) 1241603463.054044 707875.529 6174294.230 -2.09900 0.071 4 1241603463.056364
fd	destination file
doLine	make google draw a line (else icons)

Returns: true if all lines had 6 floating point values (false if last line was less than full)

References UTMtoLL().

Referenced by getReplayTimestamp2(), and toKml().

bool UResPoseHist::replayStep ( int steps )

Replay N singles from the logfile. Returns true if N steps available.

References UReplay::replayAdvanceTime(), replayStep(), and UReplay::replayTimeNow.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

bool UResPoseHist::replayStep ( )

privatevirtual

Replay a single step from the logfile. Returns true if EOF not found.

Reimplemented from UReplay.

References addIfNeeded(), UPose::h, MAX_LOG_LINE_LENGTH, UPoseTVQ::q, UResBase::replay, replayFile, replayLine, replayLogLine, replayTime2, UReplay::replayTimeNext, UReplay::replayTimeNow, UTime::setTimeTod(), UPoseTime::t, UTime::valid, UPoseTVQ::vel, UPose::x, and UPose::y.

Referenced by getOdoPoseOrigin(), getReplayLogLine(), and replayStep().

void UResPoseHist::saveToLog ( FILE * logf )

Save pose history to logfile

References UTime::getMicrosec(), getPose(), UTime::getSec(), UPose::h, poseNew, posesCnt, UPoseTime::t, UPose::x, and UPose::y.

Referenced by getPoseAtIndex().

void UResPoseHist::setMinDist ( double value )

inline

Set minimum distance required to store a pose history pose

References UVariable::setValued(), and varMinDist.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

void UResPoseHist::setMinTheta ( double value )

inline

Set minimum distance required to store a pose history pose

References UVariable::setValued(), and varMinTurn.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

void UResPoseHist::setMinTime ( double value )

inline

Set minimum distance required to store a pose history pose

References setReplay(), UVariable::setValued(), and varMinTime.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

void UResPoseHist::setPoseResult	(	bool	found,
		UPoseTVQ	pose1,
		double *	value,
		UDataBase **	returnStruct,
		int *	returnStructCnt
	)

protected

Set this poseTime value as result into the varPool method call.

References UPoseTVQ::getPose(), UPoseTVQ::getPoseTime(), UPoseTVQ::getPoseV(), UResBase::isA(), UPosition::set(), UVariable::setPose(), varCalcPose, UPose::x, and UPose::y.

Referenced by getOdoPoseOrigin(), and methodCall().

bool UResPoseHist::setReplay ( bool value )

Set replay flag. If replay flag is changed to true, the logfile is attempted opened. If successfull, the current pose history is cleared and the first record read and implemented. If not successfull, the replay flag is set to true, and the pose history is cleared only. Returns true if replay file is opened

References clear(), getReplayFileName(), MAX_LOG_LINE_LENGTH, UResBase::replay, replayFile, replayLine, replayLogLine, replayTime2, UReplay::replayTimeNext, UTime::setTimeTod(), and UTime::valid.

Referenced by UFunctionPoseHist::handlePoseHistCommand(), and setMinTime().

void UResPoseHist::setReplayTimestamp2 ( bool value )

inline

Set replay timestamp source to time stamp 2 (in logfile).

References replayTime2.

Referenced by UFunctionPoseHist::handlePoseHistCommand().

void UResPoseHist::setResID ( const char * aliasName )

virtual

Set resource ID - may be an alias name

References UResBase::getResVersion(), logPose, UResVarPool::setDescription(), ULogFile::setLogName(), and UResBase::setResID().

Referenced by UFunctionPoseHist::createResources(), and UResPoseHist().

virtual void UResPoseHist::setResID	(	const char *	id,
		const int	version
	)

inlinevirtual

Set resource ID - may be an alias name

Reimplemented from UResBase.

References gotAllResources(), UResBase::setResID(), and setResource().

bool UResPoseHist::setResource	(	UResBase *	resource,
		bool	remove
	)

virtual

Set (or remove) ressource (core pointer needed by event handling)

Reimplemented from UResVarPool.

References getOdoPoseID(), UCmdExe::getResClassID(), UServerPush::gotCmdExe(), UResBase::isA(), odoPoseHist, and UServerPush::setCmdExe().

Referenced by setResID().

bool UResPoseHist::toKml	(	char *	aKmlName,
		const int	MRL
	)

Convert a utmPose source file to a kml-line for google earth

Parameters

name	is a string buffer with source name on entry and conversion result on return.

Returns: true if file found.

References ULogFile::closeLog(), ULogFile::doFlush(), ULogFile::getF(), ULogFile::getLogFileName(), ULogFile::getLogName(), getReplayFileName(), ULogFile::isOpen(), isReplayFileOpen(), logPose, ULogFile::openLog(), processUtmFile(), and ULogFile::setLogName().

Referenced by getReplayTimestamp2(), and UFunctionPoseHist::handlePoseHistCommand().

void UResPoseHist::updateVarPool ( double tripDist )

protected

Update variables in var pool with newest pose

References UVariable::add(), UVariable::getBool(), UTime::getDecSec(), getPoseFitAtDistance(), UVariable::getValued(), poseNew, UPoseTVQ::q, UVariable::setPose(), UVariable::setValued(), UPoseTime::t, varPose, varPoseh5m, varPoseh5mUse, varQ, varTime, varTrip, varTripA, varTripB, varTripTime, varTripTimeA, varTripTimeB, varVel, and UPoseTVQ::vel.

Referenced by addIfNeeded(), addPoseHist(), and getOdoPoseOrigin().

Member Data Documentation

ULogFile UResPoseHist::logPose

protected

Logfile

Referenced by addPoseHist(), closeLogfile(), getLogfileName(), getReplayFileName(), isLogfileOpen(), openLogfile(), setResID(), and toKml().

const int UResPoseHist::MAX_LOG_LINE_LENGTH = 200

staticprotected

Maximum length of a line in the logfile

Referenced by replayStep(), and setReplay().

int UResPoseHist::newest

protected

number of newest pose

Referenced by addPoseHist(), clear(), getHistHeading(), getNewestIndex(), getOldest(), getPose(), getPoseFitAtDistance(), getPoseNearDistance(), getPoseNearTime(), getTimeAtDistance(), and UResPoseHist().

int UResPoseHist::newestSource

protected

Client number of newest update

Referenced by addIfNeeded(), getNewestSource(), and UResPoseHist().

UResPoseHist* UResPoseHist::odoPoseHist

protected

Pointer to related odometry pose history

Referenced by addIfNeeded(), getPoseAtTime(), setResource(), and UResPoseHist().

UPoseTime UResPoseHist::odoPoseOrigin

protected

Position of the odometry pose origin in this coordinate system

Referenced by addIfNeeded(), getOdoPoseOrigin(), getPoseAtTime(), and UResPoseHist().

UPoseTVQ UResPoseHist::poseNew

protected

Newest pose

Referenced by addIfNeeded(), addPoseHist(), getNewest(), getPoseNearTime(), methodCall(), saveToLog(), updateVarPool(), and UResPoseHist().

UPoseTVQ UResPoseHist::poses[MAX_HIST_POSES]

protected

Cyclic buffer of history poses

Referenced by addPoseHist(), getHistHeading(), getOldest(), getPose(), getPoseAtIndex(), getPoseFitAtDistance(), getPoseNearDistance(), getPoseNearTime(), and getTimeAtDistance().

int UResPoseHist::posesCnt

protected

Used poses entries

Referenced by addIfNeeded(), addPoseHist(), clear(), getHistHeading(), getOldest(), getOldestTime(), getPoseFitAtDistance(), getPoseNearDistance(), getPoseNearTime(), getPosesCnt(), getTimeAtDistance(), print(), saveToLog(), and UResPoseHist().

FILE* UResPoseHist::replayFile

protected

Replay file handle

Referenced by isReplayFileOpen(), replayStep(), setReplay(), and UResPoseHist().

char UResPoseHist::replayLine[MAX_LOG_LINE_LENGTH]

protected

Buffer for latest line (but not used) line from logfile

Referenced by replayStep(), setReplay(), and UResPoseHist().

int UResPoseHist::replayLogLine

protected

Current line number in logfile

Referenced by getReplayLogLine(), replayStep(), setReplay(), and UResPoseHist().

bool UResPoseHist::replayTime2

protected

Is the second timestamp be used, replacing the first in each logline?

Referenced by getReplayTimestamp2(), replayStep(), setReplay(), setReplayTimestamp2(), and UResPoseHist().

const int UResPoseHist::SOURCE_OTHER_SERVER = -2

static

Source number for relay from other server (client interface)

const int UResPoseHist::SOURCE_REPLAY = -3

static

Source number for replay

Referenced by addIfNeeded().

UVariable* UResPoseHist::varCalcPose

protected

index to a calculated pose X

Referenced by createBaseVar(), and setPoseResult().

UVariable* UResPoseHist::varEstHead

protected

index to estimate heading variable */

Referenced by addIfNeeded(), and createBaseVar().

UVariable* UResPoseHist::varEstVel

protected

index to estimate velocity */

Referenced by addIfNeeded(), and createBaseVar().

UVariable* UResPoseHist::varMinDist

protected

Variable for static variables index to parameter used by addIfNeeded(..)

Referenced by createBaseVar(), getMinDist(), and setMinDist().

UVariable* UResPoseHist::varMinTime

protected

index to parameter used by addIfNeeded(..)

Referenced by createBaseVar(), getMinTime(), and setMinTime().

UVariable* UResPoseHist::varMinTurn

protected

index to parameter used by addIfNeeded(..)

Referenced by createBaseVar(), getMinTheta(), and setMinTheta().

UVariable* UResPoseHist::varOdoPoseOrigin

protected

origin of odometry pose in this coordinate system

Referenced by addIfNeeded(), and createBaseVar().

UVariable* UResPoseHist::varPose

protected

index to most current pose (all 3 values)

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varPoseh5m

protected

index to average heading the last 5 but 1 meter

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varPoseh5mUse

protected

Update average heading the last 5 but 1 meter

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varQ

protected

index to most current pose quality

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTime

protected

index to most current update time (sec since 1 jan 1970)

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTrip

protected

index to trip distance (main or mission)

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTripA

protected

index to trip distance (e.g some sub-mission trip)

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTripB

protected

index to trip distance (e.g. last drive command)

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTripTime

protected

index to time since start of mission

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTripTimeA

protected

index to time since start of mission

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varTripTimeB

protected

index to time since start of mission

Referenced by createBaseVar(), and updateVarPool().

UVariable* UResPoseHist::varVel

protected

index to most current velocity (m/s)

Referenced by createBaseVar(), and updateVarPool().

The documentation for this class was generated from the following files:

Public Member Functions

Static Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Static Protected Attributes

Private Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation