//a static helper function
bool CMOOSCommClient::PeekMail(MOOSMSG_LIST &Mail,
const string &sKey,
CMOOSMsg &Msg,
bool bRemove,
bool bFindYoungest )
{
MOOSMSG_LIST::iterator p;
MOOSMSG_LIST::iterator q =Mail.end();
double dfYoungest = -1;
for(p = Mail.begin();p!=Mail.end();p++)
{
if(p->m_sKey==sKey)
{
//might want to consider more than one msg....
if(bFindYoungest)
{
if(p->m_dfTime>dfYoungest)
{
dfYoungest=p->m_dfTime;
q = p;
}
}
else
{
//simply take first
q=p;
break;
}
}
}
if(q!=Mail.end())
{
Msg=*q;
if(bRemove)
{
//Mail.erase(p);
Mail.erase(q);
}
return true;
}
return false;
}
bool pControl::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
#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
if( msg.GetKey() == "YAW") // HEADING, MAG_Z
{
razor_heading = MOOSDeg2Rad(msg.GetDouble());
}
if( msg.GetKey() == "DESIRED_HEADING")
{
desired_heading = MOOSDeg2Rad(msg.GetDouble());
}
}
return(true);
}
bool CHelmApp::OnPostIterate()
{
TASK_LIST::iterator p;
MOOSMSG_LIST Notifications;
MOOSMSG_LIST::iterator q;
for(p = m_Tasks.begin();p!=m_Tasks.end();p++)
{
CMOOSBehaviour* pBehaviour = *p;
Notifications.clear();
pBehaviour->GetNotifications(Notifications);
for(q = Notifications.begin();q!=Notifications.end();q++)
{
if(m_Comms.IsConnected())
{
m_Comms.Post(*q);
}
}
}
return true;
}
bool EstimSpeed::OnNewMail(MOOSMSG_LIST &NewMail) {
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for (p = NewMail.begin(); p != NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
if (key == YAW_REGISTRATION_NAME) {
this->imu_yaw = msg.GetDouble();
}// We receive LEFT thruster value, in [-1,1]]
else if (key == U1_SUBSCRIPTION_NAME) {
this->u[0] = msg.GetDouble();
}// We receive RIGHT thruster value, in [-1,1]]
else if (key == U2_SUBSCRIPTION_NAME) {
this->u[1] = msg.GetDouble();
}// We receive VERTICAL thruster value, in [-1,1]]
else if (key == U3_SUBSCRIPTION_NAME) {
this->u[2] = msg.GetDouble();
} else if (key == GPS_E_SUBSCRIPTION_NAME) {
X_GPS_PRESSURE[0] = msg.GetDouble();
} else if (key == GPS_N_SUBSCRIPTION_NAME) {
X_GPS_PRESSURE[1] = msg.GetDouble();
} else if (key == KELLER_DEPTH_SUBSCRIPTION_NAME) {
X_GPS_PRESSURE[2] = msg.GetDouble();
} else if (key == RESET_SUBSCRIPTION_NAME) {
X = X_GPS_PRESSURE;
V = Vector3d::Zero();
v = Vector3d::Zero();
} else if (key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return (true);
}
bool CMOOSRemoteLite::MailLoop()
{
m_bRunMailLoop = true;
while(!m_bQuit)
{
MOOSPause(300);
MOOSMSG_LIST MailIn;
if(m_bRunMailLoop && m_Comms.Fetch(MailIn))
{
//process mail
//simply write out
MOOSMSG_LIST::iterator p;
//make it in time order
MailIn.sort();
MailIn.reverse();
for(p = MailIn.begin();p!=MailIn.end();p++)
{
if(p->IsSkewed(MOOSTime()))
continue;
if(MOOSStrCmp(p->m_sKey,"NAV_SUMMARY"))
{
DoNavSummary(*p);
}
else
if(p->GetKey().find("DEBUG")!=string::npos)
{
//we print MOOS_DEBUG messages to the screen
string sMsg = p->m_sVal;
MOOSRemoveChars(sMsg,"\r\n");
MOOSTrace(">%-10s @ %7.2f \"%s\"\n",
p->m_sSrc.c_str(),
p->m_dfTime-GetAppStartTime(),
sMsg.c_str());
}
else
{
CUSTOMJOURNAL_MAP::iterator w = m_CustomJournals.find(p->GetKey());
if(w!=m_CustomJournals.end())
{
w->second.Add(p->GetAsString());
}
}
}
UpdateMOOSVariables(MailIn);
}
}
return true;
}
bool PrimeFactor::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if (MOOSStrCmp(msg.GetKey(), "NUM_VALUE")) {
//initialize a new struct with the given number, and push it onto the list of inputted numbers
PrimeFactorStruct received_number;
received_number.number = (uint64_t)msg.GetDouble();
received_number.curr_number = received_number.number;
received_number.received = m_received_idx;
m_received_idx++;
received_number.start_time = MOOSTime();
received_number.curr_prime_factor = 2;
m_numbers.push_front(received_number);
}
#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 DeadManPost::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
#if 0 // Keep these around just for template
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
if(key == m_heart_var) {
m_last_heartbeat = MOOSTime();
m_total_heartbeats++;
if(m_post_policy == "reset") {
m_postings_done = false;
}
}
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool LoadWatch::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
double dval = msg.GetDouble();
#if 0 // Keep these around just for template
string comm = msg.GetCommunity();
string sval = msg.GetString();
string msrc = msg.GetSource();
double mtime = msg.GetTime();
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
#endif
if(strEnds(key, "_ITER_GAP"))
handleMailIterGap(key, dval);
else if(strEnds(key, "_ITER_LEN"))
handleMailIterLen(key, dval);
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool CMOOSLogger::OnNewMail(MOOSMSG_LIST &NewMail)
{
//these three calls look through the incoming mail
//and handle all appropriate logging
DoAsyncLog(NewMail);
UpdateMOOSVariables(NewMail);
LogSystemMessages(NewMail);
//here we look for more unusual things
MOOSMSG_LIST::iterator q;
for(q=NewMail.begin();q!=NewMail.end();q++)
{
if(q->IsSkewed(MOOSTime()))
continue;
//are we being asked to restart?
if(MOOSStrCmp(q->GetKey(),"LOGGER_RESTART"))
{
OnLoggerRestart();
}
}
return true;
}
bool Rad2Degree::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
string key = msg.GetKey();
#if 0 // Keep these around just for template
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
if(key == "DESIRED_HEADING_DEG")
Notify("DESIRED_HEADING", degToRadians(msg.GetDouble()));
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return true;
}
bool IrrotationalVortexCurrent::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if(msg.GetKey() == "NAV_X")
m_nav_x = msg.GetDouble();
if(msg.GetKey() == "NAV_Y")
m_nav_y = msg.GetDouble();
#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 CMOOSLogger::LogSystemMessages(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
m_SystemLogFile.setf(ios::left);
double dfTimeNow = MOOSTime();
for(p = NewMail.begin();p!=NewMail.end();p++)
{
CMOOSMsg & rMsg = *p;
if(IsSystemMessage(rMsg.m_sKey) && !rMsg.IsSkewed(dfTimeNow))
{
m_SystemLogFile<<setw(10)<<setprecision(7)<<rMsg.m_dfTime-GetAppStartTime()<<' ';
m_SystemLogFile<<setw(20)<<rMsg.m_sKey.c_str()<<' ';
m_SystemLogFile<<setw(20)<<rMsg.m_sSrc.c_str()<<' ';
if(rMsg.m_cDataType==MOOS_DOUBLE)
{
m_SystemLogFile<<setw(20)<<rMsg.m_dfVal<<' ';
}
else
{
MOOSRemoveChars(rMsg.m_sVal,"\n");
m_SystemLogFile<<setw(20)<<rMsg.m_sVal.c_str()<<' ';
}
m_SystemLogFile<<endl;
}
}
return true;
}
bool XSensINS::OnNewMail(MOOSMSG_LIST &NewMail) {
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for (p = NewMail.begin() ; p != NewMail.end() ; p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
#if 0 // Keep these around just for template
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
if (key == "FOO")
cout << "great!";
else if (key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return true;
}
bool Turtlebot::OnNewMail(MOOSMSG_LIST &NewMail)
{
cout << "in OnNewMail " <<endl;
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
//#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
if(key == "SEND_CMD")
{
m_current_cmd = toupper(sval); // for simplicity example format is like "move,1,2"
cout <<"in the key conditional; current cmd:" <<m_current_cmd<<endl;
}
}
return(true);
}
bool OpiDetection::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
string key = msg.GetKey();
#if 0 // Keep these around just for template
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
if(msg.GetKey() == image_name)
memcpy(img.data, msg.GetBinaryData(), img.rows*img.step);
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return true;
}
bool FollowOrders::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if ((msg.GetKey() == SLAVE_ORDERS_STRING && GetAppName() == "slave")
|| (msg.GetKey() == MASTER_ORDERS_STRING && GetAppName() == "master")) {
string order = msg.GetString();
vector<string> svector = parseString(order, '@');
for (int i = 0; i < svector.size(); i++) {
cout << "processing order << " << svector[i] << endl;
this->processOrderString(svector[i]);
}
}
else if (msg.GetKey() == "NAV_X") {
m_Comms.Notify("SLAVE_X", msg.GetDouble());
}
else if (msg.GetKey() == "NAV_Y") {
m_Comms.Notify("SLAVE_Y", msg.GetDouble());
} else {
cout << " unknown: " << msg.GetKey() << " --> " << msg.GetString() << endl;
}
}
return(true);
}
bool CPlannerApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
UpdateMOOSVariables(NewMail);
for(MOOSMSG_LIST::iterator i=NewMail.begin();i!=NewMail.end();++i)
{
// Get list of tasks in the Agenda and try scheduling them.
if (i->GetName()=="TASK_LIST")
{
// TASK_LIST : Task1#Task2#Task3...
// where each task is defined by: "UserID TaskID UserTaskID Command Task"
//printf("Received from task_list [%s]\n",i->GetAsString().c_str());
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString().c_str(),"#",lista);
//Run the list of pending tasks and if possible Schedule them
//-------------------------------------------------------------
std::string command,task,address;
size_t taskid,localtaskid;
//Run it N times, where N is the number of pending tasks
for (size_t j=0;j<lista.size();j++)
{
//Check Compatibility of ALL pending tasks. If any task is compatible then Schedule it.
bool tasktosolve = planner.SelectTasktobePlanned(i->GetAsString(), command, task, taskid, address, localtaskid);
//IF any task has been scheduled
if (taskid!=size_t(-1) && tasktosolve)
{
//Notify the taskID that has been planned
std::string goal = address+" "+format ("%d",(int)taskid)+" "+format ("%d",(int)localtaskid)+" "+command+" "+task;
printf("Planning %s\n",goal.c_str());
//Notify the Agenda that some task have been planned
//! @moos_publish TASK_PLANNED The Id of the task that has just been planned (to inform the Agenda)
m_Comms.Notify("TASK_PLANNED", format("%d",(int)taskid));
//Schedule the task in a new thread!
mrpt::system::createThreadFromObjectMethod(this,&CPlannerApp::Plan,goal);
}
else if (taskid==size_t(-1))
{
printf("Esa tarea no la puedo hacer ahora mismo...\n");
}
}
}
if( (i->GetName()=="SHUTDOWN") && (MOOSStrCmp(i->GetString(),"true")) )
{
// Disconnect comms:
MOOSTrace("Closing Module \n");
this->RequestQuit();
}
}
return true;
}
bool VideoCodec::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
#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
if (MOOSWildCmp("VVV_IMAGE_*", msg.GetName())
&& !m_decode)
{
//cout << "msg name " << msg.GetName() << endl;
memcpy(imgRaw.data, msg.GetBinaryData(), msg.GetBinaryDataSize());
vector<int> encode_params;
encode_params.push_back(CV_IMWRITE_JPEG_QUALITY);
encode_params.push_back(60);
cv::imencode(".jpg",imgRaw,bufComp,encode_params);
string pubName = "VCODEC_JPEG_" + msg.GetName();
Notify(pubName, bufComp, msg.GetTime());
if (m_affichage_image) {
Mat imdec = cv::imdecode(bufComp, CV_LOAD_IMAGE_COLOR);
imshow(m_display_name, imdec);
}
}
if (MOOSWildCmp("VCODEC_JPEG_*", msg.GetName())
&& m_decode)
{
//cout << "msg name " << msg.GetName() << endl;
msg.GetBinaryData(bufComp);
Mat imdec = cv::imdecode(bufComp, CV_LOAD_IMAGE_COLOR);
string pubName = msg.GetName().substr(12);
Notify(pubName, imdec, msg.GetTime());
if (m_affichage_image) {
imshow(m_display_name, imdec);
}
}
}
return(true);
}
bool OnNewMail(MOOSMSG_LIST & NewMail){
//process it
/**
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin();p!=NewMail.end();p++)
{
//lets get a reference to the Message - no need for pointless copy
CMOOSMsg & rMsg = *p;
// repetitive "ifs" is one way to build a switch yard for
// the messages
p->Trace();
}
**/
//Retrieve informaiton
MOOSMSG_LIST::iterator p1;
for(p1=NewMail.begin();p1!=NewMail.end();p1++)
{
//lets get a reference to the Message - no need for pointless copy
CMOOSMsg & rMsg = *p1;
// repetitive "ifs" is one way to build a switch yard for
// the messages
if(MOOSStrCmp(rMsg.GetKey(),"DESIRED_PORTTHRUSTER"))
{
//this message is about something called "DESIRED_PORTTHRUSTER"
CMOOSMsg &Msg = rMsg;
if(!Msg.IsDouble())
return MOOSFail("Ouch - was promised \"DESIRED_PORTTHRUSTER\" would be a double");
DESIRED_PORTTHRUSTER = Msg.GetDouble();
//MOOSTrace("DESIRED_PORTTHRUSTER is %f\n",DESIRED_PORTTHRUSTER);//the actual heading
//if you want to see all details in Msg, you can print a message by
//Msg.Trace();
}
else if(MOOSStrCmp(rMsg.GetKey(),"DESIRED_STARBOARDTHRUSTER"))
{
//this message is about something called "Input_FR"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"DESIRED_STARBOARDTHRUSTER\" would be a double");
DESIRED_STARBOARDTHRUSTER = Msg1.GetDouble();
//MOOSTrace("DESIRED_STARBOARDTHRUSTER is %f\n",DESIRED_STARBOARDTHRUSTER);//the actual heading
//if you want to see all details in Msg, you can print a message by
//Msg.Trace();
}
}
return true;
}
/**this will be called each time a new packet is recieved*/
bool CMOOSDB::OnRxPkt(const std::string & sClient,MOOSMSG_LIST & MsgListRx,MOOSMSG_LIST & MsgListTx)
{
MOOSMSG_LIST::iterator p;
for(p = MsgListRx.begin();p!=MsgListRx.end();p++)
{
ProcessMsg(*p,MsgListTx);
}
//good spot to update our internal time
UpdateDBTimeVars();
//and send clients an occasional membersip list
UpdateDBClientsVar();
if(!MsgListRx.empty())
{
//now we fill in the packet with our replies to THIS CLIENT
//MOOSMSG_LIST_STRING_MAP::iterator q = m_HeldMailMap.find(MsgListRx.front().m_sSrc);
MOOSMSG_LIST_STRING_MAP::iterator q = m_HeldMailMap.find(sClient);
if(q==m_HeldMailMap.end())
{
//CMOOSMsg & rMsg = MsgListRx.front();
//there is no mail waiting to be sent to this client
//should only happen at start up...
//string sClient = MsgListRx.front().m_sSrc;
MOOSMSG_LIST NewList;
m_HeldMailMap[sClient] = NewList;
q = m_HeldMailMap.find(sClient);
assert(q!=m_HeldMailMap.end());
}
if(q!=m_HeldMailMap.end())
{
if(!q->second.empty())
{
//copy all the held mail to MsgListTx
MsgListTx.splice(MsgListTx.begin(),q->second);
}
}
}
return true;
}
//this is a mail callback - it is called as soon as mail arrives
bool OnMail(void *pParam){
CMOOSCommClient* pC = reinterpret_cast<CMOOSCommClient*>(pParam);
//grab all the held mail
MOOSMSG_LIST M;
pC->Fetch(M); //get the mail
MOOSMSG_LIST::iterator q; //process it
for(q=M.begin();q!=M.end();q++){
q->Trace();//print it
}
return true;
}
bool HazardMgr::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
string sval = msg.GetString();
#if 0 // Keep these around just for template
string comm = msg.GetCommunity();
double dval = msg.GetDouble();
string msrc = msg.GetSource();
double mtime = msg.GetTime();
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
#endif
if(key == "UHZ_CONFIG_ACK")
handleMailSensorConfigAck(sval);
else if(key == "UHZ_OPTIONS_SUMMARY")
handleMailSensorOptionsSummary(sval);
else if(key == "UHZ_DETECTION_REPORT")
handleMailDetectionReport(sval);
else if(key == "HAZARDSET_REQUEST")
handleMailReportRequest();
else if(key == "UHZ_HAZARD_REPORT")
handleMailHazardReport(sval);
else if(key == "SLAVE_REPORT"){
handleMailSlaveReport(sval);
slave_report_time = MOOSTime();
}
else if(key == "DEPLOY"){
if(!m_deployed){
m_deploy_time = MOOSTime();
m_deployed = true;
}
}
else
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool OnMail (void * pParam)
{
CMOOSCommClient* pC = reinterpret_cast<CMOOSCommClient*> (pParam);
MOOSMSG_LIST M;
pC->Fetch(M);
MOOSMSG_LIST::iterator q;
for(q=M.begin();q!=M.end();q++)
{
q->Trace();
}
return true;
}
bool CNIDAQApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
try
{
// Process asynchronous outputs:
for (MOOSMSG_LIST::const_iterator it=NewMail.begin();it!=NewMail.end();++it)
{
const string sVar = it->GetName();
// Analog outputs:
for (map<string,size_t>::const_iterator itM=m_varnames_ao.begin();itM!=m_varnames_ao.end();++itM)
{
if (itM->first==sVar)
{
const size_t taskIdx = itM->second;
const double val = it->GetDouble();
m_daq.writeAnalogOutputTask(taskIdx,1, &val ,0.1, true);
break;
}
}
// Analog outputs:
for (map<string,size_t>::const_iterator itM=m_varnames_do.begin();itM!=m_varnames_do.end();++itM)
{
if (itM->first==sVar)
{
const size_t taskIdx = itM->second;
const bool val = it->GetDouble() != 0.0;
m_daq.writeDigitalOutputTask(taskIdx,val, 0.1);
break;
}
}
} // end for each message
}
catch(std::exception &e) {
std::cerr << "[NIDAQ] Exception: " << e.what() << std::endl;
}
catch(...) {
std::cerr << "[NIDAQ] Untyped exception!\n";
}
UpdateMOOSVariables(NewMail);
UpdateMOOSVariables_OpenMORA(NewMail);
return true;
}
bool CMOOSApp::LookForAndHandleAppCommand(MOOSMSG_LIST & NewMail)
{
MOOSMSG_LIST::iterator q;
bool bResult = true;
for(q=NewMail.begin();q!=NewMail.end();q++)
{
if(MOOSStrCmp(q->GetKey(),GetCommandKey()))
{
//give a derived class a chance to respond
bResult&= OnCommandMsg(*q);
}
}
return bResult;
}
/** OnNewMail */
bool CManPublisherApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
std::string cad;
for(MOOSMSG_LIST::iterator i=NewMail.begin();i!=NewMail.end();++i)
{
if( (i->GetName()=="SHUTDOWN") && (MOOSStrCmp(i->GetString(),"true")) )
{
// Disconnect comms:
MOOSTrace("Closing Module \n");
this->RequestQuit();
}
}
UpdateMOOSVariables(NewMail);
return true;
} // end-OnNewMail
bool LLTrackUpdate::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if (msg.GetKey() == "LLTRACK_SURVEYLINE") {
//process the lat/long to UTM
} else if (msg.GetKey() == "LLTRACK_TURNPOINT") {
} else if (msg.GetKey() == "LLTRACK_ALIGNMENTLINE") {
} else if (msg.GetKey() == "UTM_SURVEYLINE") {
//process the UTM to local lines
string msg_content = msg.GetString();
m_survey_pts = ProcessUTMString(msg_content);
if (m_survey_pts.size() != 0) {
m_survey_update = true;
}
} else if (msg.GetKey() == "UTM_TURNPOINT") {
string msg_content = msg.GetString();
m_turn_pts = ProcessUTMString(msg_content);
if (m_turn_pts.size() != 0) {
m_turn_update = true;
}
} else if (msg.GetKey() == "UTM_ALIGNMENTLINE") {
string msg_content = msg.GetString();
m_alignment_pts = ProcessUTMString(msg_content);
if (m_alignment_pts.size() != 0) {
m_alignment_update = true;
}
}
#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 TrajectorySim::OnNewMail(MOOSMSG_LIST &NewMail) {
MOOSMSG_LIST::iterator p1;
for (p1 = NewMail.begin(); p1!=NewMail.end(); p1++) {
CMOOSMsg & rMsg = *p1;
if (MOOSStrCmp(rMsg.GetKey(), "TIME")) {
CMOOSMsg &Msg = rMsg;
if (!Msg.IsDouble())
return MOOSFail("\"TIME\" needs to be a double");
t = Msg.GetDouble();
}
}
return true;
}
bool Sonar::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
#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
// Mise à jour des paramètres du sonar
if ( msg.GetKey() == "SONAR_PARAMS" && msg.IsString() )
{
string msg_val = msg.GetString();
// Le message est de la forme "Range=25,Gain=45,Continuous=true"
double dVal=0.0; int iVal; bool bVal;
if (MOOSValFromString(dVal, msg_val, "Range", true))
m_msgHeadCommand.setRange(dVal);
if (MOOSValFromString(iVal, msg_val, "nBins", true))
m_msgHeadCommand.setNbins(iVal);
if (MOOSValFromString(dVal, msg_val, "AngleStep", true))
m_msgHeadCommand.setAngleStep(dVal);
if (MOOSValFromString(bVal, msg_val, "Continuous", true))
m_msgHeadCommand.setContinuous(bVal);
if (MOOSValFromString(dVal, msg_val, "Gain", true))
m_msgHeadCommand.setGain(dVal);
if (MOOSValFromString(dVal, msg_val, "LeftLimit", true)){
m_msgHeadCommand.setLeftLimit(dVal); cout << "limite gauche " << dVal << endl;}
if (MOOSValFromString(dVal, msg_val, "RightLimit", true)){
m_msgHeadCommand.setRightLimit(dVal); cout << "limite droite " << dVal << endl;}
// Envoi de la commande au sondeur
// TODO: vérifier que le CMOOSSerialPort est bien thread safe. Sinon, rajouter un mutex
SendSonarMessage(m_msgHeadCommand);
}
}
return(true);
}
bool CPlannerApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
UpdateMOOSVariables(NewMail);
for(MOOSMSG_LIST::iterator i=NewMail.begin();i!=NewMail.end();++i)
{
if (i->GetName()=="TASK_LIST")
{
//printf("Received from task_list [%s]\n",i->GetAsString().c_str());
std::deque<std::string> lista;
mrpt::utils::tokenize(i->GetString().c_str(),"#",lista);
std::string command,task,address;
size_t taskid,localtaskid;
for (size_t j=0;j<lista.size();j++)
{
//CheckCompatibility
bool tasktosolve=planner.SelectTasktobePlanned(i->GetAsString(),command,task,taskid,address,localtaskid);
if (taskid!=size_t(-1) && tasktosolve)
{
std::string goal=address+" "+format ("%d",(int)taskid)+" "+format ("%d",(int)localtaskid)+" "+command+" "+task;
printf("Planning %s\n",goal.c_str());
m_Comms.Notify("TASK_PLANNED", format("%s#%d",address.c_str(),(int)taskid));
mrpt::utils::createThreadFromObjectMethod(this,&CPlannerApp::Plan,goal);
}
else if (taskid==size_t(-1))
{
printf("Esa tarea no la puedo hacer ahora mismo...\n");
}
}
}
if( (i->GetName()=="SHUTDOWN") && (MOOSStrCmp(i->GetString(),"true")) )
{
// Disconnect comms:
MOOSTrace("Closing Module \n");
this->RequestQuit();
}
}
return true;
}