bool WallFollowing::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
/* if (msg.GetSource() == this->GetAppName())
continue;
*/
/*
if(msg.GetKey() == "WF_START" && msg.GetAsString()!="")
{
m_regulate = true;
Notify("WF_START", "");
}
if(msg.GetKey() == "WF_STOP" && msg.GetAsString()!="")
{
m_regulate = false;
Notify("WF_STOP", "");
}*/
if(msg.GetKey() == "VVV_HEADING")
{
m_last_heading = m_current_heading;
m_current_heading = msg.GetDouble();
for(list<pair<float, float> >::iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; it ++)
{
it->first += (m_current_heading-m_last_heading); // clef
}
}
if(msg.GetKey() == "SONAR_RAW_DATA")
{
float angle = 0;
int nRows, nCols;
float ad_interval = 0.25056;
vector<unsigned int> scanline;
MOOSValFromString(angle, msg.GetString(), "bearing");
MOOSValFromString(ad_interval, msg.GetString(), "ad_interval");
MOOSValFromString(scanline, nRows, nCols, msg.GetString(), "scanline");
// Récupération du max de la scanline
float distance = 0, distance_mini = 1.0;
int obstacle_max = 0;
for(int i = 0 ; i < (int)scanline.size() ; i ++)
{
if((int)scanline[i] > obstacle_max)
{
obstacle_max = scanline[i];
distance = ad_interval / 2.0 * i;
}
}
m_obstacles.push_back(make_pair(angle, distance));
while(m_obstacles.size() > 80)
{
m_obstacles.pop_front();
}
}
#if 0 // Keep these around just for template
string key = msg.GetKey();
string comm = msg.GetCommunity();
double dval = msg.GetDouble();
string sval = msg.GetString();
string msrc = msg.GetSource();
double mtime = msg.GetTime();
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
#endif
}
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;
}
