//Support for binary-string from ROS to MOOS
bool MOOSNode::toMOOSBinaryString(std::string moosName, std::string myString){
m_Comms.Notify(moosName,(void *)myString.c_str(),myString.size(),MOOSTime());
return true;
}
bool CSingleAction::Run(CPathAction &DesiredAction)
{
m_Notifications.push_back(CMOOSMsg(MOOS_NOTIFY,m_sVarName,m_sValue,MOOSTime()));
return OnComplete();
}
bool MOOSNode::toMOOS(std::string moosName, double data){
m_Comms.Notify(moosName,data,MOOSTime());
return true;
}
bool MOOSNode::toMOOS(std::string moosName, std::string myString){
m_Comms.Notify(moosName,myString,MOOSTime());
return true;
}
bool CZPatternTask::SetNextSetPoint()
{
if(m_DepthDOF.GetError()>m_dfTolerance)
return true;
switch(m_ePatternType)
{
case MOOS_Z_PATTERN_SQUARE:
switch(m_eGoal)
{
case MOOS_Z_PATTERN_GOAL_WAIT_MIN:
case MOOS_Z_PATTERN_GOAL_WAIT_MAX:
if(MOOSTime()-m_dfLevelStartTime>=m_dfLevelDuration)
{
if(m_eGoal==MOOS_Z_PATTERN_GOAL_WAIT_MAX)
{
m_eGoal = MOOS_Z_PATTERN_GOAL_MIN_DEPTH;
m_DepthDOF.SetDesired(m_dfMinDepth);
}
else if(m_eGoal==MOOS_Z_PATTERN_GOAL_WAIT_MIN)
{
m_eGoal = MOOS_Z_PATTERN_GOAL_MAX_DEPTH;
m_DepthDOF.SetDesired(m_dfMaxDepth);
}
}
break;
case MOOS_Z_PATTERN_GOAL_MIN_DEPTH:
m_dfLevelStartTime = MOOSTime();
m_eGoal = MOOS_Z_PATTERN_GOAL_WAIT_MIN;
break;
case MOOS_Z_PATTERN_GOAL_MAX_DEPTH:
m_dfLevelStartTime = MOOSTime();
m_eGoal = MOOS_Z_PATTERN_GOAL_WAIT_MAX;
break;
}
break;
case MOOS_Z_PATTERN_YOYO:
switch(m_eGoal)
{
case MOOS_Z_PATTERN_GOAL_MIN_DEPTH:
m_DepthDOF.SetDesired(m_dfMaxDepth);
m_eGoal = MOOS_Z_PATTERN_GOAL_MAX_DEPTH;
break;
case MOOS_Z_PATTERN_GOAL_MAX_DEPTH:
m_DepthDOF.SetDesired(m_dfMinDepth);
m_eGoal = MOOS_Z_PATTERN_GOAL_MIN_DEPTH;
break;
}
break;
default:
return false;
break;
}
return true;
}
bool CMOOSSerialPort::CommsLoop()
{
char pTmp[DEFAULT_COMMS_SPACE];
char pAccumulator[2*DEFAULT_COMMS_SPACE];
int nInStore = 0;
double dfTimeOut = 0.1;
while(!m_bQuit)
{
int nRead = ReadNWithTimeOut(pTmp,DEFAULT_COMMS_SPACE,dfTimeOut);
if(nRead!=-1)
{
if((nInStore+nRead)>int(sizeof(pAccumulator)))
{
//oops...
MOOSTrace("Comms Loop Accumulator Overflow, reseting\n");
nInStore = 0;
continue;
}
//append to the accumulator
memcpy(pAccumulator+nInStore,pTmp,nRead);
//we have more in the cupboard!
nInStore+=nRead;
//lock
m_InBoxLock.Lock();
char * pTelegramEnd = NULL;
do
{
//A device will by default use the termination character <CR>
//But we now allow for setting of a termination character
//to allow for flexibility in a reply.
pTelegramEnd = (char*)memchr(pAccumulator,GetTermCharacter(),nInStore);
if(pTelegramEnd!=NULL)
{
//how long is the telegram?
int nTelegramLength = pTelegramEnd - pAccumulator + 1;
//make a buffer that is one larger to preserve the last character
//in case we are interested in it.
char bBiggerBuff[2*DEFAULT_COMMS_SPACE+1];
//copy the telegram to a buffer size that is 1 larger
memcpy(bBiggerBuff, pAccumulator, nTelegramLength);
//terminate it as a string
//*pTelegramEnd = '\0';
bBiggerBuff[nTelegramLength] = '\0';
//copy it
//std::string sTelegram((char*)pAccumulator);
std::string sTelegram((char*)bBiggerBuff);
MOOSRemoveChars(sTelegram,"\r\n");
if(IsVerbose())
{
MOOSTrace("Telegram = %s\n",sTelegram.c_str());
}
//shuffle accumulator
memmove(pAccumulator,
pAccumulator+nTelegramLength,
sizeof(pAccumulator)-nTelegramLength);
//we have less in our store now!
nInStore-=nTelegramLength;
//make a telegram
CMOOSSerialTelegram Tg(sTelegram,MOOSTime());
//stuff it
m_InBox.push_front(Tg);
}
}while(pTelegramEnd !=NULL);
//whatch we don't exhibit disgraceful behaviour!
while(m_InBox.size()>MOOS_SERIAL_INBOX_MAX_SIZE)
{
m_InBox.pop_back();
}
//and unlock
m_InBoxLock.UnLock();
}
else
{
}
}
return true;
}
bool CMOOSPlayBackV2::MessageFromLine(const std::string & sLine, CMOOSMsg &Msg)
{
int n = 0;
std::string sTime,sKey,sSrc;
m_ALog.GetNextToken(sLine,n,sTime);
m_ALog.GetNextToken(sLine,n,sKey);
m_ALog.GetNextToken(sLine,n,sSrc);
size_t nData = sLine.find_first_not_of(" \t",n);
if(nData==string::npos)
return false;
std::string sData = sLine.substr(nData,sLine.size()-nData);
Msg.m_dfTime = MOOSTime();
if(MOOSIsNumeric(sData))
{
Msg.m_dfVal = atof(sData.c_str());
Msg.m_cDataType = MOOS_DOUBLE;
Msg.m_sVal = "";
}
else
{
Msg.m_dfVal = 0.0;
if(sData.find("<MOOS_BINARY>") !=std::string::npos)
{
//Msg.MarkAsBinary();
long long nOffset;
if(!MOOSValFromString(nOffset,sData,"Offset"))
return MOOSFail("badly formed MOOS_BINARY indicator - missing \"Offset=xyz\"");
std::string sFile;
if(!MOOSValFromString(sFile,sData,"File"))
return MOOSFail("badly formed MOOS_BINARY indicator - missing \"File=XYZ\"");
int nBytes;
if(!MOOSValFromString(nBytes,sData,"Bytes"))
return MOOSFail("badly formed MOOS_BINARY indicator - missing \"Bytes=xyz\"");
//corner case of binary file changing half way through a log....(Why this might happen
//I don't know but catch it anyway....
if(sFile != m_sBinaryFileName && m_BinaryFile.is_open() )
{
m_BinaryFile.close();
}
m_sBinaryFileName = sFile;
if(!m_BinaryFile.is_open())
{
//open the file with the correct name
//here we need to point the file towards the full path - the blog file should be sitting next
//to teh alog being played
/** splits a fully qualified path into parts -path, filestem and extension */
std::string sPath,sFileName,sExtension;
if(!MOOSFileParts(m_ALog.GetFileName(),sPath,sFileName,sExtension))
return MOOSFail("failed to parse alog file into file parts %s",MOOSHERE);
std::string sFullBinaryLogPath = sPath+"/"+m_sBinaryFileName;
MOOSTrace("opening binary file %s\n",sFullBinaryLogPath.c_str());
m_BinaryFile.open(sFullBinaryLogPath.c_str(),std::ios::binary);
if(!m_BinaryFile)
{
return MOOSFail("unable to open binary file called %s", m_sBinaryFileName.c_str());
}
}
//move to the right place in the file
m_BinaryFile.seekg(nOffset);
//make space
char * pBD = new char[nBytes];
//read the right number of bytes
m_BinaryFile.read(pBD,nBytes);
//copy data
Msg.m_sVal.assign(pBD,nBytes);
//delete temporary space
delete [] pBD;
Msg.MarkAsBinary();
}
else
{
Msg.m_dfVal = 0.0;
Msg.m_cDataType = MOOS_STRING;
Msg.m_sVal = sData;
}
}
//fill in standard aspects
Msg.m_sSrc = sSrc;
Msg.m_sKey = sKey;
Msg.m_cMsgType = MOOS_NOTIFY;
return true;
}
bool CMOOSPlayBackV2::Iterate(MOOSMSG_LIST &Output)
{
if(IsEOF())
return false;
double dfStopTime = m_dfLastMessageTime+m_dfTickTime;
bool bDone = false;
while(!bDone && !IsEOF() )
{
CMOOSMsg NewMsg;
double dfTNext = m_ALog.GetEntryTime(m_nCurrentLine);
//are we in a mode in which we are slaved to a client
//via its publishing of MOOS_CHOKE?
m_dfClientLagTime = MOOSTime() - m_dfLastClientProcessedTime;
if (m_dfLastClientProcessedTime != -1 &&
m_dfClientLagTime > MAX_CHOKE_TIME)
{
m_bWaitingForClientCatchup = true;
bDone = true;
dfStopTime = dfTNext;
continue;
}
else
{
//normal sequential processing under our own steam
m_bWaitingForClientCatchup = false;
if(dfTNext<=dfStopTime)
{
if(MessageFromLine(m_ALog.GetLine(m_nCurrentLine),NewMsg))
{
if(!IsFiltered(NewMsg.GetSource()))
{
Output.push_front(NewMsg);
}
}
// arh moved this out of the loop above, because a failed
// call to MessageFromLine would make uPlayback hang in
// an infinite loop
m_nCurrentLine++;
}
else
{
bDone = true;
}
}
}
//post a playback time
if(!Output.empty())
{
CMOOSMsg timeMsg( MOOS_NOTIFY, "PLAYBACK_DB_TIME", dfStopTime );
timeMsg.m_sSrc = "uPlayback";
Output.push_front( timeMsg );
}
m_dfLastMessageTime = dfStopTime;
return true;
}
bool OnStartUp(){
m11=190;
m22=240;
m33=82;
d11=75;
d22=105;
d33=55;
D11=0;
D22=0;
D33=0;
L=1; //it should be the same as that in the controller
//here we extract the vehicle name..
m_sVehicleName = "UnNamed";
if(!m_MissionReader.GetConfigurationParam("VehicleName",m_sVehicleName))
MOOSTrace("Warning parameter \"VechicleName\" not specified. Using default of \"%s\"\n",m_sVehicleName.c_str());
x=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_x",x))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",x);
y=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_y",y))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",y);
psi=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_psi",psi))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",psi);
u=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_u",u))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",u);
v=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_v",v))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",v);
r=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_r",r))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",r);
DESIRED_PORTTHRUSTER=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("DESIRED_PORTTHRUSTER",DESIRED_PORTTHRUSTER))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",DESIRED_PORTTHRUSTER);
DESIRED_STARBOARDTHRUSTER=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("DESIRED_STARBOARDTHRUSTER", DESIRED_STARBOARDTHRUSTER))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",DESIRED_STARBOARDTHRUSTER);
//MOOSTrace("input_FL=%f, input_FR=%f, time=%f\n",input_FL,input_FR,MOOSTime());
t_r=MOOSTime();
return true;
}
