#include <uressmrctl.h>

Inheritance diagram for UResSmrCtl:

Public Member Functions
bool	methodCall (const char name, const char paramOrder, char *strings, const double pars, double value, UDataBase returnStruct=NULL, int returnStructCnt=NULL)

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

void	run ()

bool	setResource (UResBase *resource, bool remove)

bool	start ()

bool	startPoseStreaming (int interval)

virtual void	stop (bool andWait)

	UResSmrCtl ()

void	UResSmrCtlInit ()

virtual	~UResSmrCtl ()

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 ()

virtual bool	gotAllResources (char *missingThese, int missingTheseCnt)

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	setResID (const char *id, const int version)

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)

virtual bool	replayStep ()

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 ()

Static Public Member Functions
static const char *	getResClassID ()

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

Public Attributes
ULogFile	logctl

ULogFile	logprim

Public Attributes inherited from UReplay
bool	replayTimeAdvancePending

UTime	replayTimeNow

Protected Member Functions
void	createBaseVar ()

void	getSMRCLDrive2cmd (char cmd, const int cmdCnt, UPoseV startPose, UPoseV endPose, double breakAt, double orderDist, bool final, UTime t)
	Get the command string to pass to MRC for a drive type command to this end pose (starting at the start pose). at the break distance a user event should be plased in form of a (tab) character on an otherwise empty line. More...

bool	implementDrivePath (const char driverName, UManSeq man)

bool	sendNewManoeuvreToSMR (UManSeq *manseq)

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
UManSeq *	currentPlan

bool	currentPlanNew

int	driveCmdFinishedID

UTime	driveCmdFinishedIDTime

UPose	driveCmdStartPose

const char *	driver

UManSeq *	implementedPlan

UResPoseHist *	poseHist

bool	running

UResSmrIf *	smrif

bool	stopFlag

UVariable *	varDirectOrWait

UVariable *	varDirectWait

UVariable *	varManEndID

UVariable *	varManEndTime

UVariable *	varManIsDirect

UVariable *	varManPlanDist

UVariable *	varManQueueingTime

UVariable *	varRenewDestDist

UVariable *	varRenewDestHeading

UVariable *	varRenewDestVel

UVariable *	varRenewDriveDist

UVariable *	varRunning

UVariable *	varUseDriveonDist

UVariable *	varUseDriveonHeading

UVariable *	varUseDriveonIfDirect

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

Private Attributes
pthread_t	thDrive

Additional Inherited Members
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...

Detailed Description

This is the shared resource class. It must enherit from the resource base class (or one of its decendent) as shown.

Author: Christian Andersen

Constructor & Destructor Documentation

UResSmrCtl::UResSmrCtl ( )

inline

Constructor

References getResClassID(), UResBase::setResID(), UResSmrCtlInit(), and ~UResSmrCtl().

UResSmrCtl::~UResSmrCtl ( )

virtual

Destructor

References ULogFile::closeLog(), logctl, logprim, and stop().

Referenced by UResSmrCtl().

Member Function Documentation

void UResSmrCtl::createBaseVar ( )

protected

Create the smrif related variables

References UResVarPool::addMethod(), UResVarPool::addVar(), UResBase::getResVersion(), varDirectOrWait, varDirectWait, varManEndID, varManEndTime, varManIsDirect, varManPlanDist, varManQueueingTime, varRenewDestDist, varRenewDestHeading, varRenewDestVel, varRenewDriveDist, varRunning, varUseDriveonDist, varUseDriveonHeading, and varUseDriveonIfDirect.

Referenced by getResClassID(), and UResSmrCtlInit().

static const char* UResSmrCtl::getResClassID ( )

inlinestatic

Fixed name of this resource type

References createBaseVar(), getSMRCLDrive2cmd(), implementDrivePath(), methodCall(), print(), run(), sendNewManoeuvreToSMR(), setResource(), start(), startPoseStreaming(), and stop().

Referenced by UResSmrCtl().

void UResSmrCtl::getSMRCLDrive2cmd	(	char *	cmd,
		const int	cmdCnt,
		UPoseV	startPose,
		UPoseV	endPose,
		double	breakAt,
		double	orderDist,
		bool	final,
		UTime *	t
	)

protected

Get the command string to pass to MRC for a drive type command to this end pose (starting at the start pose). at the break distance a user event should be plased in form of a (tab) character on an otherwise empty line.

Parameters

cmd	is the string buffer, where the command should be placed.
cmdCnt	is the size of the buffer
startPose	is the assumed current pose
endPose	is the desired end pose
breakAt	is the distance from start pose to placement of the tab character
order	distance is the end distance
final	indicates that the order distance is to the end of the manoeuver, and thus cam be used directly as the order distance.
t	is current odometry time - for debug only, may be NULL

References ULogFile::getF(), UTime::getMicrosec(), U2Dlined::getOnLine(), UTime::getSec(), UPoseV::getVel(), UPose::h, ULogFile::isOpen(), UResBase::line, logprim, UPoseV::set(), UPose::set(), U2Dlined::setPH(), UPoseV::vel, UPose::x, and UPose::y.

Referenced by getResClassID(), and sendNewManoeuvreToSMR().

bool UResSmrCtl::implementDrivePath	(	const char *	driverName,
		UManSeq *	man
	)

protected

Implement this new short term drive plan from this driver. The plan may be discarded, if it is very close to to the last received plan from the same driver.

References UManSeq::copy(), currentPlan, currentPlanNew, driver, ULock::lock(), ULock::tryLock(), and ULock::unlock().

Referenced by getResClassID(), and methodCall().

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

virtual

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.

Reimplemented from UResVarPool.

References UResSmrIf::doUserEvent(), driver, implementDrivePath(), UResBase::isA(), UDataString::makeString(), roundi(), UDataString::setStr(), and smrif.

Referenced by getResClassID().

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

virtual

print status to a string buffer

Reimplemented from UResVarPool.

References driver.

Referenced by getResClassID().

void UResSmrCtl::run ( )

Main loop - supposed to run in a thread. Checkes the waypoint list and issued commands to the smr if a new list is available.

References absf(), bool2str(), currentPlan, currentPlanNew, driveCmdStartPose, driver, UPose::getDistance(), UPose::getDistToPoseLine(), UManPPSeq::getEndPoseV(), UResPoseHist::getNewest(), UManSeq::getP2P(), UManSeq::getP2PCnt(), UVariable::getValued(), UPoseV::getVel(), UPose::h, implementedPlan, ULock::lock(), poseHist, running, sendNewManoeuvreToSMR(), UVariable::setValued(), stopFlag, ULock::tryLock(), ULock::unlock(), varManIsDirect, varRenewDestDist, varRenewDestVel, varRenewDriveDist, varRunning, Wait(), UPose::x, and UPose::y.

Referenced by getResClassID(), and startDrive().

bool UResSmrCtl::sendNewManoeuvreToSMR ( UManSeq * manseq )

protected

Send this manoeuvre sequence to the robot real time controller

Todo:: fix this the right way - ensure it never happens

Todo:: set time - send or not - helps during replay, but should realy set replay time (from smr-IO log?- once this is available)

References bool2str(), UManSeq::copy(), dist, driveCmdFinishedID, driveCmdFinishedIDTime, driveCmdStartPose, UManSeq::expandMan(), UResSmrIf::getAcc(), UVariable::getBool(), UTime::getDecSec(), UPose::getDistance(), UManSeq::getDistance(), UManSeq::getDistanceFromEndPoseLine(), UManPPSeq::getEndPoseV(), UManSeq::getEndPoseV(), ULogFile::getF(), UManPPSeq::getMan(), UTime::getMicrosec(), UResSmrIf::getMinTurnRad(), UResPoseHist::getNewest(), UManSeq::getP2P(), UManSeq::getP2PCnt(), UTime::getSec(), UManPPSeq::getSeqCnt(), UManoeuvre::getSMRCLcmd2(), getSMRCLDrive2cmd(), UResSmrIf::getSpeed(), UManPPSeq::getStartPoseV(), UManSeq::getStartPoseV(), UTime::getTimePassed(), UResSmrIf::getTurnAcc(), UVariable::getValued(), UPose::h, implementedPlan, ULogFile::isOpen(), logctl, logprim, UTime::Now(), UTime::now(), poseHist, UManSeq::print(), USmrCl::sendString(), UResSmrIf::sendUserEvent(), UPoseV::set(), UManPPSeq::setMaxVel(), UVariable::setValued(), smrif, UPoseTime::t, ULogFile::toLog(), varDirectOrWait, varDirectWait, varManEndID, varManEndTime, varManIsDirect, varManPlanDist, varManQueueingTime, varUseDriveonDist, varUseDriveonHeading, varUseDriveonIfDirect, UPoseV::vel, UPose::x, and UPose::y.

Referenced by getResClassID(), and run().

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

virtual

Called by server core when new resources are available. return true is resouurce is used Save a pointer to the resource as needed.

Reimplemented from UResVarPool.

References UResPoseHist::getOdoPoseID(), UResSmrIf::getResClassID(), UResBase::isA(), poseHist, UResVarPool::setResource(), and smrif.

Referenced by getResClassID().

bool UResSmrCtl::start ( )

Start running in thread

References UVariable::getBool(), startDrive(), stopFlag, thDrive, and varRunning.

Referenced by getResClassID(), and UResSmrCtlInit().

bool UResSmrCtl::startPoseStreaming ( int interval )

Start pose streaming - this will not be started if the control loop is not running. Returns true if stream start is send.

References UVariable::getBool(), UResSmrIf::isStreaming(), smrif, USmrCl::startOdoStream(), and varRunning.

Referenced by getResClassID(), and UFunctionSmrIf::handleSmr().

void UResSmrCtl::stop ( bool andWait )

virtual

Stop drive thread and close logfile

Reimplemented from UResBase.

References running, stopFlag, thDrive, and Wait().

Referenced by getResClassID(), and ~UResSmrCtl().

void UResSmrCtl::UResSmrCtlInit ( )

Initialize class

References createBaseVar(), UResVarPool::createVarSpace(), currentPlan, currentPlanNew, driveCmdFinishedID, driver, implementedPlan, logctl, logprim, poseHist, running, ULogFile::setLogName(), smrif, start(), and stopFlag.

Referenced by UResSmrCtl().

Member Data Documentation

UManSeq* UResSmrCtl::currentPlan

protected

Newest plan of the type UManSeq to be implemented.

Referenced by implementDrivePath(), run(), and UResSmrCtlInit().

bool UResSmrCtl::currentPlanNew

protected

Set this flag to true, when the current plan is changed

Referenced by implementDrivePath(), run(), and UResSmrCtlInit().

int UResSmrCtl::driveCmdFinishedID

protected

Last ID of the main part of a manoeuvre sequence

Referenced by sendNewManoeuvreToSMR(), and UResSmrCtlInit().

UTime UResSmrCtl::driveCmdFinishedIDTime

protected

Time the last manoeuvre were implemented

Referenced by sendNewManoeuvreToSMR().

UPose UResSmrCtl::driveCmdStartPose

protected

Latest implemented start pose

Referenced by run(), and sendNewManoeuvreToSMR().

const char* UResSmrCtl::driver

protected

Name of driver (is a string owned by the driver, and the pointer adress is used to determine a change in driver responcibility.

Referenced by implementDrivePath(), methodCall(), print(), run(), and UResSmrCtlInit().

UManSeq* UResSmrCtl::implementedPlan

protected

Newest plan of the type UManSeq to be implemented.

Referenced by run(), sendNewManoeuvreToSMR(), and UResSmrCtlInit().

ULogFile UResSmrCtl::logctl

Logfile for drive controller

Referenced by UFunctionSmrIf::handleSmr(), sendNewManoeuvreToSMR(), UResSmrCtlInit(), and ~UResSmrCtl().

ULogFile UResSmrCtl::logprim

Logfile for drive primitives, lines and arcs, actually commanded

Referenced by getSMRCLDrive2cmd(), UFunctionSmrIf::handleSmr(), sendNewManoeuvreToSMR(), UResSmrCtlInit(), and ~UResSmrCtl().

UResPoseHist* UResSmrCtl::poseHist

protected

Pointer to resource with current pose information

Referenced by run(), sendNewManoeuvreToSMR(), setResource(), and UResSmrCtlInit().

bool UResSmrCtl::running

protected

local variable is true if thread is running

Referenced by run(), stop(), and UResSmrCtlInit().

UResSmrIf* UResSmrCtl::smrif

protected

local variables provided by this resource. Pointer to resource with interface to MRC

Referenced by methodCall(), sendNewManoeuvreToSMR(), setResource(), startPoseStreaming(), and UResSmrCtlInit().

bool UResSmrCtl::stopFlag

protected

Stop non-realtime robot controller thread

Referenced by run(), start(), stop(), and UResSmrCtlInit().

pthread_t UResSmrCtl::thDrive

private

Thread handle for main loop.

Referenced by start(), and stop().

UVariable* UResSmrCtl::varDirectOrWait

protected

should direct manoeuvers be accepted only?

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varDirectWait

protected

are we waiting for a direct manoeuvre?

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varManEndID

protected

Index to variable with latest user event ID posted for the latest manoeuvre

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varManEndTime

protected

Index to time when the user event ID were posted for the latest manoeuvre

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varManIsDirect

protected

flag that indicates, that the received manoeuvre is direct to destination

Referenced by createBaseVar(), run(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varManPlanDist

protected

Index to planning distance when implementing a manoeuvre

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varManQueueingTime

protected

Index to time it takes to queue a manoeuvre command (the latest)

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varRenewDestDist

protected

minimum distance change in destination to trigger command renewal

Referenced by createBaseVar(), and run().

UVariable* UResSmrCtl::varRenewDestHeading

protected

minimum heading change in destination to trigger command renewal.

Referenced by createBaseVar().

UVariable* UResSmrCtl::varRenewDestVel

protected

minimum velocity change in destination to trigger command renewal.

Referenced by createBaseVar(), and run().

UVariable* UResSmrCtl::varRenewDriveDist

protected

distance traveled where new man should be implemented even if it looks like the old plan, (to avoid stand still).

Referenced by createBaseVar(), and run().

UVariable* UResSmrCtl::varRunning

protected

Is non-realtime robot controller thread running

Referenced by createBaseVar(), run(), start(), and startPoseStreaming().

UVariable* UResSmrCtl::varUseDriveonDist

protected

variable with max deviation from end-pose line for using drive command only

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varUseDriveonHeading

protected

variable with max deviation from end-pose heading for using drive command only

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

UVariable* UResSmrCtl::varUseDriveonIfDirect

protected

Use driveon command if manoeuvre indicates direct is possible

Referenced by createBaseVar(), and sendNewManoeuvreToSMR().

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

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation