#include <usmrcl.h>

Inheritance diagram for USmrCl:

Public Member Functions
bool	doDisconnect ()

virtual void	eventGpsUpdate (UPoseTime odoState, double easting, double northing, double heading, double quality, double satellites, double dop)

virtual void	eventHakoVarUpdate (int hakoManual, int liftPos, int ptoSpeed)

virtual void	eventInsUpdate (UPoseTime odoState, double accx, double accy, double accz, double roll, double tilt, double pan, double insTime)

virtual void	eventPoseUpdated (bool streamSource)

char *	getHost ()

int	getLogMode ()

int	getPort ()

virtual bool	getSimulatedPose ()

int	getStopCondition ()

bool	handleLineData (const int timeoutMs)

bool	isConnected ()

bool	isLogging ()

void	print (const char *prestring)

void	run ()

void	saveMrcLog (bool restart)

bool	sendSMReval (const char varName, double timeoutSec, double value, char *valStr=NULL, const int valStrCnt=0)

bool	sendString (const char s1, const char s2=NULL, const char *s3=NULL)

int	setHost (const char *iHost)

bool	setIOLog (const char path, const char name, const int mode)

void	setIOLogClosed ()

void	setLogMode (int value)

bool	setOdoLog (const char , const char )

bool	setOdoLogClosed ()

int	setPort (int iPort)

void	setVerbose (bool value)

bool	start ()

bool	startOdoStream (const int samplesInterval)

void	stop (bool andWait)

bool	tryConnect ()

	USmrCl ()

virtual	~USmrCl ()

Public Member Functions inherited from ULock
bool	lock ()

void	lockInit ()

void	post ()

bool	tryLock ()

bool	tryWait ()

	ULock ()

void	unlock ()

bool	wait ()

	~ULock ()

Public Attributes
int	cmdLineFinished

int	cmdLineQueued

int	cmdLineStarted

int	cmdLineStopCnd

int	cmdLineSyntaxError

int	cmdLineUserEvent

bool	cmdStop

double	gpsVals [maxGpsVals]

double	ins [INSV]

ULogFile	logIO

USmrOdoState	odoState

double	streamInsTime

int	streamShowGpsCnt

int	streamShowGpsEvery

int	streamShowInsCnt

int	streamShowInsEvery

int	streamShowOdoCnt

int	streamShowOdoEvery

bool	tryHoldConnection

Static Public Attributes
static const int	INSV = 7

static const int	maxGpsVals = 6
	number of doubles in gps streaming array (E,N,H, mode, sats, dof) More...

Protected Member Functions
virtual void	closeConnection ()

virtual void	connectionChange (bool connected)

virtual void	eventWatchFired (const char *, double)
	A watch event occured, update as needed. More...

bool	getDataFromLine (int timeout, bool waitFullTime)

bool	getLineFromSocket (int timeout_ms)

virtual void	gotUserEvent (const char *)

virtual void	info (const char *msg, int type=0)

bool	isMessageInBuffer ()

virtual void	lineStateUpdated ()

bool	setDriveState (bool idEevent, bool eventTimeout, bool *streamData)

void	toLog (const char *logString, const int logLevel)

Protected Attributes
unsigned int	connCnt

bool	connected

unsigned int	errCnt

int	evalReq

int	evalRes

char	evalResult [MaxEvalLen]

char	host [MAX_HOST_LENGTH]

int	logIOMode

int	port

unsigned int	readCnt

char	reply [MAX_REPLY_LENGTH]

int	replyCnt

char	replyLine [MAX_REPLY_LENGTH]

bool	running

bool	stopRead

bool	streamImu

unsigned int	txCnt

bool	verbose

Static Protected Attributes
static const int	MaxEvalLen = 500

Private Member Functions
bool	handleGetevents (const int timeoutMs)

bool	listen (int timeout, const char *gotReply, bool handled, bool handledID, bool handledEventTimeout, bool *handledStream)

bool	sendOnly (const char *cmd)

Private Attributes
bool	gotGeteventReply

int	sock

pthread_t	thRead

Detailed Description

Connection to SMR-CL

Author: Christian Andersen

Constructor & Destructor Documentation

USmrCl::USmrCl ( )

Constructor

USmrCl::~USmrCl ( )

virtual

Destructor

Member Function Documentation

void USmrCl::closeConnection ( )

protectedvirtual

Shut down the socket connection in a proper way

Reimplemented in UResSmrIf.

References Wait().

Referenced by UResSmrIf::closeConnection().

void USmrCl::connectionChange ( bool connected )

protectedvirtual

A connection change has happened. The function is calle if connected or disconnected

Reimplemented in UResSmrIf.

References bool2str().

bool USmrCl::doDisconnect ( )

Disconnect and stop command loop.

Referenced by UFunctionSmrIf::handleSmr(), testsmrcl(), and UResSmrIf::~UResSmrIf().

void USmrCl::eventGpsUpdate	(	UPoseTime	odoState,
		double	easting,
		double	northing,
		double	heading,
		double	quality,
		double	satellites,
		double	dop
	)

virtual

A new GPS position is received from MRC with these values. The function should be overwritten at a higher level

Parameters

heading may be from odo-gps kalman filter to prevent printout of the received position.

Reimplemented in UResSmrIf.

References UTime::getMicrosec(), UTime::getSec(), and UPoseTime::t.

virtual void USmrCl::eventHakoVarUpdate	(	int	hakoManual,
		int	liftPos,
		int	ptoSpeed
	)

inlinevirtual

the hako variables may be updated - tell possible users.

Parameters

hakoManual	manual or automatic enabled - probably useless
liftPos	is the detected lift position - 100 is about center 80 is high and 120 is low
ptoSpeed	is the RPM? of the power take off. all from: "stream %d \"$hakomanual" "$hakoliftinggearpos" "$hakopowertakeoffspeed" ",

Reimplemented in UResSmrIf.

void USmrCl::eventInsUpdate	(	UPoseTime	odoState,
		double	accx,
		double	accy,
		double	accz,
		double	roll,
		double	tilt,
		double	pan,
		double	insTime
	)

virtual

A new INS data is received from MRC with these values. The function should be overwritten at a higher level to prevent printout of the received position. virtual void eventInsUpdate(UPoseTime odoState, double accx, double accy, double accz, double roll, double tilt, double pan, double insTime);

Reimplemented in UResSmrIf.

References UTime::getMicrosec(), UTime::getSec(), and UTime::setTime().

void USmrCl::eventPoseUpdated ( bool streamSource )

virtual

A new pose state is available in the 'odoState' structure

Reimplemented in UResSmrIf.

virtual void USmrCl::eventWatchFired	(	const char *	,
		double
	)

inlineprotectedvirtual

A watch event occured, update as needed.

Parameters

name	is the name of the watch fired
atTime	is the MRC time reported.

Reimplemented in UResSmrIf.

bool USmrCl::getDataFromLine	(	int	timeout,
		bool	waitFullTime
	)

protected

Listen to the line for the timeout period of time and collect the reply to the reply buffer. The timeout should be about 10 ms, to ensure that the reply to the request is available. If 'waitFullTime' is false, then function returns after first data is received (or timeout). Reply and replyCnt holds the reply. Returns true if new data is available, and false if timeout.

References UTime::getTimePassed(), MAX_REPLY_LENGTH, maxi(), UTime::Now(), roundi(), scWarning, and Wait().

char* USmrCl::getHost ( )

inline

Set socket port number

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

bool USmrCl::getLineFromSocket ( int timeout_ms )

protected

Get a reply from MRC, wait no longer than timout for the reply. Reply may be split over more than one message block, or more than one line may be received Function returns when a '
' is found in the reply, or a timeout has occured. Returns true if data is available. Setc connected flag if a connection error occured. Received data is stored in 'reply[]', with length in replyCnt

int USmrCl::getLogMode ( )

inline

Get logging mode

Referenced by UFunctionSmrIf::handleSmr().

int USmrCl::getPort ( )

inline

Set socket port number

Referenced by UFunctionSmrIf::handleSmr(), UResSmrIf::print(), and testsmrcl().

bool USmrCl::getSimulatedPose ( )

virtual

Get new simulated or replay data, as determined by this virtual function. It should be filled by an appropriate virtual function. Returns true if new data is filled into the odoState.

int USmrCl::getStopCondition ( )

inline

Get stop condition on last finished command

virtual void USmrCl::gotUserEvent ( const char * )

inlineprotectedvirtual

An interface specification for handling of user events

Parameters

eventstring is the string returned by MRC after the keyword 'userevent' and stripped for whitespace.

Reimplemented in UResSmrIf.

bool USmrCl::handleGetevents ( const int timeoutMs )

private

Handle all inut and empty 'event' queue, until an eventtimeout has occured. the 'Eventtimeout' could be short, as each receive should be handled in one MRC sampletime (10 ms) + transmission time. Returns true if an event timeout were received.

bool USmrCl::handleLineData ( const int timeoutMs )

Listen to the line from the smr and handle any stream data (and not handled getevents. Returns true if data is received.

void USmrCl::info	(	const char *	msg,
		int	type = `0`
	)

protectedvirtual

There is info to report. this function may be overwritten by parent class.

References scError, scInfo, and scWarning.

bool USmrCl::isConnected ( )

inline

Is connection established

Referenced by UFunctionSmrIf::handleSmr(), UResSmrIf::print(), and testsmrcl().

bool USmrCl::isLogging ( )

inline

Is log open

Referenced by UFunctionSmrIf::handleSmr().

bool USmrCl::isMessageInBuffer ( )

protected

Test for avaiable message in reply buffer, that is not yet handled

void USmrCl::lineStateUpdated ( )

protectedvirtual

Called when a getevent returnes a change in line status, i.e. a queued, finished or started

Reimplemented in UResSmrIf.

bool USmrCl::listen	(	int	timeout,
		const char **	gotReply,
		bool *	handled,
		bool *	handledID,
		bool *	handledEventTimeout,
		bool *	handledStream
	)

private

Listen for a line, and handle any known reply types these are IDXX queued, started, stopcond, flushed, syntaxErr, stream (assumed odometry), if handled then 'handled' is returned true. Returnes after first line, but more may be available. 'gotReply' holds the received line. Returns true if a line were received. Function sets 'connected' flag as apprppriate. If simulated flag is set, then odometry data is updated as appropriate.

void USmrCl::print ( const char * prestring )

Print smrcl connection status

References bool2str().

Referenced by testsmrcl().

void USmrCl::run ( )

Run the listen thread - is started by a start() call and stopped by stop(true). Do NOT call this function directly.

References UTime::getTimePassed(), UTime::now(), and Wait().

Referenced by startSmrCl().

void USmrCl::saveMrcLog ( bool restart )

Close and reset the mrc log, but maintain log variables

Parameters

restart if true, the logging is restarted

References UTime::getForFilename(), and UTime::now().

Referenced by UFunctionSmrIf::handleSmr(), and UResSmrIf::methodCall().

bool USmrCl::sendOnly ( const char * cmd )

private

Send a message to the MRC and return true if send. connected is set to false, if it is not possible to send.

References scWarning.

bool USmrCl::sendSMReval	(	const char *	varName,
		double	timeoutSec,
		double *	value,
		char *	valStr = `NULL`,
		const int	valStrCnt = `0`
	)

Send this buffer of data and wait for reply. Returns true if send and reply received. Reply 'eventtimeout' and 'streem' do not count as reply. Send a drive command and wait for the queue number. If the queued reply is available within the timeout period, its number is returned at 'lineID', else last queued line is used, and the function returns false. If 'mustBeQueued', then a IDXX queued must be handled within timeout period to return true. Get the value of a variable from the MRC (MRC), if the value is received within timeout period, 'value' will hold the returned value.

Parameters

varName	is the variable name in the MRC (MRC), possibly including the initial '$' as needed.
timeoutSec	Minimum timeout period is about 0.75 sec if message flow is low, so a low 'timeoutSec' is replaced by this period.
value	is a pointer to the place where the value is returned.
valStr	is a buffer string, where the reply string from the MRC is placed.
valStrCnt	is the length of the buffer

Returns: true if a value is received.

References UTime::getTimePassed(), and UTime::Now().

Referenced by UFunctionSmrIf::handleSmr(), UResSmrIf::methodCall(), and testsmrcl().

bool USmrCl::sendString	(	const char *	s1,
		const char *	s2 = `NULL`,
		const char *	s3 = `NULL`
	)

Send string direct to smr, and do not wait for a replay The connection is locked while sending to avoid conflict with service thread and menoeuvre delivery.

Parameters

s1	string to send
s2	any additional string to send, e.g. a terminaing line feed or extra stop condition
s3	any additional string to send, e.g. a terminaing line feed

Returns: true if send within the timeout period

References ULock::lock(), and ULock::unlock().

Referenced by UResSmrIf::connectionChange(), UResSmrIf::doSmrDrive(), UFunctionSmrIf::handleSmr(), UResSmrIf::methodCall(), UResSmrIf::sendAddWatch(), UResSmrCtl::sendNewManoeuvreToSMR(), UResSmrIf::sendUserEvent(), UResSmrIf::stopRobot(), and testsmrcl().

bool USmrCl::setDriveState	(	bool *	idEevent,
		bool *	eventTimeout,
		bool *	streamData
	)

protected

trach event to se if last 'drive' command is started or executed. Returns true if a hata in line were handled. The parameter flags is set according to line type.

References UTime::getDecSec(), UPose::h, limitToPi(), mini(), roundi(), UPose::set(), and UPoseTime::t.

int USmrCl::setHost ( const char * iHost )

inline

Set socket port number

References MAX_HOST_LENGTH.

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

bool USmrCl::setIOLog	(	const char *	path,
		const char *	name,
		const int	mode
	)

Set input - output log for the connection to MRC. if mode is 0, nothing is logged - except errors. if mode is 1, all output is logged, including ID events numbers if mode is 2, all input and output is logged.

void USmrCl::setIOLogClosed ( )

Close logfile

void USmrCl::setLogMode ( int value )

inline

Set logging mode ( 0 = just errors. 1=errors + output to smr, 3=errors + output + info, 4=errors + output + info + input from smr) (everything)

Referenced by UFunctionSmrIf::handleSmr().

bool USmrCl::setOdoLog	(	const char *	,
		const char *
	)

inline

Set odometry logfile. The filename is appended with timestamp and a '.log' extension. sample: path/name _yyyymmdd_hhmmss.ddd.log

bool USmrCl::setOdoLogClosed ( )

inline

Close odometry log

int USmrCl::setPort ( int iPort )

inline

Set socket port number

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

void USmrCl::setVerbose ( bool value )

inline

Set verbose message level

References USmrOdoState::print().

bool USmrCl::start ( )

Start read thread

References startSmrCl().

Referenced by testsmrcl().

bool USmrCl::startOdoStream ( const int samplesInterval )

Set odometry update time Start odometry in stream mode. Return true if true

References Wait().

Referenced by UResSmrCtl::startPoseStreaming(), and testsmrcl().

void USmrCl::stop ( bool andWait )

Stop read thread

References Wait().

void USmrCl::toLog	(	const char *	logString,
		const int	logLevel
	)

protected

Get odometer measurements command from MRC (as eval) Returns true if valid reply is available within timeout (in seconds) Save this string in 'log' (communication log), if 'log' is open and logmode is higher or equal to this 'logLevel'

References UTime::getMicrosec(), UTime::getSec(), and UTime::Now().

bool USmrCl::tryConnect ( )

Try connect to socket, NB! there is no loop that listens for events.

References MAX_HOST_LENGTH, MAX_SC_INFO_SIZE, scDebug, scInfo, and scWarning.

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

Member Data Documentation

int USmrCl::cmdLineFinished

command is finished (robot has stopped)

Referenced by UFunctionSmrIf::handleSmr(), UResSmrIf::lineStateUpdated(), and testsmrcl().

int USmrCl::cmdLineQueued

Last line reply

Referenced by UFunctionSmrIf::handleSmr(), UResSmrIf::lineStateUpdated(), and testsmrcl().

int USmrCl::cmdLineStarted

command is started - not completed

Referenced by UFunctionSmrIf::handleSmr(), UResSmrIf::lineStateUpdated(), and testsmrcl().

int USmrCl::cmdLineStopCnd

stop condition for last command

int USmrCl::cmdLineSyntaxError

Last line with syntaxerror

Referenced by UFunctionSmrIf::handleSmr(), and UResSmrIf::lineStateUpdated().

int USmrCl::cmdLineUserEvent

got a user event with this number

Referenced by UResSmrIf::doUserEvent(), and UResSmrIf::lineStateUpdated().

bool USmrCl::cmdStop

stop robot now!

unsigned int USmrCl::connCnt

protected

Number of connections tried

bool USmrCl::connected

protected

Is connection to smrCl established

Referenced by UResSmrIf::closeConnection(), and UResSmrIf::stopRobot().

unsigned int USmrCl::errCnt

protected

Number of receive errors

int USmrCl::evalReq

protected

semaphore when waiting for eval result, implemented as request number and request and result number. evalReq is incremented before eval is send, and evalRes is set to evalReq on reply received. This is to allow recovery when MRC goes down or do not accept eval (e.g. syntax error)

int USmrCl::evalRes

protected

char USmrCl::evalResult[MaxEvalLen]

protected

Eval string as received

bool USmrCl::gotGeteventReply

private

set true, whenever a valid event reply is received

double USmrCl::gpsVals[maxGpsVals]

Gps streamed values. The values are Easting, Northing, Satellites, dop, not used. DOP is Dillution Of Prediction 1 is perfect, 6 is OK, 50 is bad.

Referenced by UFunctionSmrIf::handleSmr().

char USmrCl::host[MAX_HOST_LENGTH]

protected

Name of host to connect to

double USmrCl::ins[INSV]

INS values last received - beeing accx, accy, accz, roll, tilt, pan, time

const int USmrCl::INSV = 7

static

number of stored INS values

ULogFile USmrCl::logIO

Logfile handle for general communication (debug) logging

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

int USmrCl::logIOMode

protected

Log outgoing and incomming values, including Log getevent, eval, and all incomming. 0 = no logging 1 = log outgoing only 2 = log all

const int USmrCl::MaxEvalLen = 500

staticprotected

Eval result string length

const int USmrCl::maxGpsVals = 6

static

number of doubles in gps streaming array (E,N,H, mode, sats, dof)

USmrOdoState USmrCl::odoState

robot position in own coordinates set when requested

Referenced by UResSmrIf::eventPoseUpdated().

int USmrCl::port

protected

Port used when connecting to host

unsigned int USmrCl::readCnt

protected

Number of reads (with >0 bytes returned).

char USmrCl::reply[MAX_REPLY_LENGTH]

protected

reply from MRC, may be a line and a bit

int USmrCl::replyCnt

protected

Bytes valid in reply

char USmrCl::replyLine[MAX_REPLY_LENGTH]

protected

last fully received line from MRC

bool USmrCl::running

protected

Is thread running

int USmrCl::sock

private

Socket

bool USmrCl::stopRead

protected

Stop connection

bool USmrCl::streamImu

protected

Stream IMU data or hako tool details

double USmrCl::streamInsTime

Count of gps stream updates

int USmrCl::streamShowGpsCnt

Count of gps stream updates

Referenced by UFunctionSmrIf::handleSmr().

int USmrCl::streamShowGpsEvery

Show evert N stream gps update

Referenced by UFunctionSmrIf::handleSmr().

int USmrCl::streamShowInsCnt

Count of gps stream updates

Referenced by UFunctionSmrIf::handleSmr().

int USmrCl::streamShowInsEvery

Show evert N stream ins update

Referenced by UFunctionSmrIf::handleSmr().

int USmrCl::streamShowOdoCnt

Count of odometry stream updates

Referenced by UFunctionSmrIf::handleSmr().

int USmrCl::streamShowOdoEvery

Show evert N stream odometry update

Referenced by UFunctionSmrIf::handleSmr().

pthread_t USmrCl::thRead

private

Thread handle for read loop.

bool USmrCl::tryHoldConnection

When true the control loop will try and reestables connection if the connection fails.

unsigned int USmrCl::txCnt

protected

Number of transmitted messages

bool USmrCl::verbose

protected

Make verbose messages (debug and info)

warnings and errors atr always shown

Referenced by UResSmrIf::methodCall().

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

Public Member Functions

Public Attributes

Static Public Attributes

Protected Member Functions

Protected Attributes

Static Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation