void moossafir::SimDriver::do_work()
{
MOOSMSG_LIST msgs;
if(moos_client_.Fetch(msgs))
{
for(MOOSMSG_LIST::iterator it = msgs.begin(),
end = msgs.end(); it != end; ++it)
{
const std::string& in_moos_var = driver_cfg_.GetExtension(moossafir::Config::incoming_moos_var);
if(it->GetKey() == in_moos_var)
{
const std::string& value = it->GetString();
glog.is(DEBUG1) &&
glog << group(glog_in_group()) << in_moos_var << ": " << value << std::endl;
goby::acomms::protobuf::ModemRaw in_raw;
in_raw.set_raw(value);
ModemDriverBase::signal_raw_incoming(in_raw);
try
{
goby::acomms::protobuf::ModemTransmission m;
std::string data;
int id;
if(goby::moos::val_from_string(data, value, "data"))
m.add_frame(dccl::hex_decode(data));
if(!goby::moos::val_from_string(id, value, "modem_id"))
throw(std::runtime_error("No `modem_id` field"));
m.set_src(id);
receive_message(m);
}
catch(std::exception& e)
{
glog.is(DEBUG1) &&
glog << group(glog_in_group()) << warn << "Failed to parse incoming message: " << e.what() << std::endl;
}
}
}
}
}
bool SimGPS::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
double dval = msg.GetDouble();
string sval = msg.GetString();
if(key==in_prefix + "_X")
sim_gps(0) = dval;
if(key==in_prefix + "_Y")
sim_gps(1) = dval;
if(key==in_prefix + "_DEPTH")
sim_depth = dval;
}
return(true);
}
bool WayPointsViewer::OnNewMail(MOOSMSG_LIST &NewMail)
{
bool gps_update = false;
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
if(msg.GetKey() == "VVV_HEADING_DESIRED")
m_heading = msg.GetDouble();
else if(msg.GetKey() == "GPS_LAT")
{
m_gps_lat = msg.GetDouble();
gps_update = true;
}
else if(msg.GetKey() == "GPS_LON")
{
m_gps_long = msg.GetDouble();
gps_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
}
if(gps_update)
m_moosgeodesy.LatLong2LocalUTM(m_gps_lat, m_gps_long, m_position.y, m_position.x);
return(true);
}
bool MSEASOceanModelDirect::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
bool handled = false;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if(msg.GetKey() == "NODE_REPORT") {
handled = HandleNewNodeReport(msg);
if(!handled)
cout << "Unhandled Node Report From " << msg.GetCommunity() << endl;
} else if (msg.GetKey() == m_varname_moosvar) {
m_model_varnames_request = msg.GetString();
m_model_varname_vec_request = parseString(m_model_varnames_request, ',');
m_num_model_vars = m_model_varname_vec_request.size();
Cell a(m_num_model_vars, 1);
for (unsigned int i=0; i<m_num_model_vars; i++)
a(i) = m_model_varname_vec_request[i];
m_model_varname_cell_request = a;
} else if (msg.GetKey() == m_time_request_offset_moosvar) {
m_model_time_request_offset = msg.GetDouble();
} else if (msg.GetKey() == m_time_offset_reset_moosvar) {
m_reset_time_flag = 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 MacJoystick::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
}
return(true);
}
bool OnNewMail(MOOSMSG_LIST & Mail){
//process it
MOOSMSG_LIST::iterator q;
for(q=Mail.begin();q!=Mail.end();q++)
{
//are we a ponger
if(q->GetKey()=="ex1010-ping")
Notify("ex1010-pong",q->GetDouble());
if(q->GetKey()=="ex1010-pong"){
double latency =(MOOSLocalTime()-q->GetDouble())/2;
mean_latency_+= latency;
if(count_>2)//this simply removes case of stale data in DB
max_latency_ = std::max(latency,max_latency_);
if(count_<burstsize_-1)
Notify("ex1010-ping",MOOSLocalTime());
}
count_++;
}
return true;
}
bool AcommsScheduler::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 comm = msg.GetCommunity();
double dval = msg.GetDouble();
string sval = msg.GetString();
if(key == "DEPLOY"){
deployed = (tolower(sval)=="true");
last_transmit_time = time_in_ms();
}
}
return(true);
}
bool NodeManager::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if (msg.GetKey() == "NAV_LAT") {
m_node_lat = msg.GetDouble();
m_lat_set = true;
} else if (msg.GetKey() == "NAV_LONG") {
m_node_lon = msg.GetDouble();
m_lon_set = true;
} else if (msg.GetKey() == "MSEASOCEANMODEL_RETURN") {
int num_values = p->GetBinaryDataSize()/sizeof(double);
int num_rows = num_values;
Matrix a(num_rows, 1);
double *a_data = a.fortran_vec();
memcpy(a_data, p->GetBinaryData(), p->GetBinaryDataSize());
m_model_values_return = a;
double drift_x = m_model_values_return(0)/100; //MSEAS model velocity data is in cm/s
double drift_y = m_model_values_return(1)/100;
m_Comms.Notify("DRIFT_X", drift_x);
m_Comms.Notify("DRIFT_Y", drift_y);
}
#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 Manual::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
string temp;
CMOOSMsg &msg = *p;
string key = msg.GetKey();//Nome da mensagem
//Pega os valores das variaveis
if(key == "MANUAL_CONTROL")
{
manualControl = msg.GetString();
toLower(manualControl);
if(manualControl=="true") // realiza o controle
{
//Configuracoes para parar o pHelm
m_Comms.Notify("MOOS_MANUAL_OVERIDE","TRUE");
m_Comms.Notify("MOOS_MANUAL_OVERRIDE","TRUE");
//Desativar o controle do barco baseado no pHelm
m_Comms.Notify("DISABLE_CONTROLEBARCO","true");
//Turn off engine
double M1=0;
double M2=0;
m_Comms.Notify("M1",M1);
m_Comms.Notify("M2",M2);
}
if(manualControl=="false")
{
//Configuracoes para voltar o pHelm
m_Comms.Notify("MOOS_MANUAL_OVERIDE","FALSE");
m_Comms.Notify("MOOS_MANUAL_OVERRIDE","FALSE");
//Desativar o controle do barco baseado no pHelm
m_Comms.Notify("DISABLE_CONTROLEBARCO","false");
}
}
}
return(true);
}
bool CISCREA::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();
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_THRUST")
m_desired_thrust = dval;
else if (key == "DESIRED_SLIDE")
m_desired_slide = dval;
else if (key == "DESIRED_RUDDER")
m_desired_rudder = dval;
else if (key == "DESIRED_ELEVATOR")
m_desired_elevator = dval;
else if (key == "FRONT_LIGHTS")
m_front_lights = dval;
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
//-------------------------------------
// OnNewMail()
//-------------------------------------
bool CMCEnoseApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
std::string cad;
for(MOOSMSG_LIST::iterator i=NewMail.begin();i!=NewMail.end();++i)
{
// MOOSTrace(format("New msg received: %s \n",i->GetKey()));
if (i->GetName() == "ENOSE_MCE_SWITCH_CHAMBER")
{
MOOSTrace("\n ----------------------------------------------------\n An External Module Ordered a Switching.... \n ----------------------------------------------------\n");
SwitchChamber(true,num_chambers); //force switching
}
if( (i->GetName()=="SHUTDOWN") && (MOOSStrCmp(i->GetString(),"true")) )
{
// Disconnect comms:
MOOSTrace("Closing Module \n");
this->RequestQuit();
}
}
UpdateMOOSVariables(NewMail);
return true;
}
bool GPS::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 != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return (true);
}
bool NodeBroker::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();
string comm = msg.GetCommunity();
bool msg_is_local = true;
if(msg.GetCommunity() != m_node_host_record.getCommunity())
msg_is_local = false;
//double dval = msg.GetDouble();
//double mtime = msg.GetTime();
//bool mdbl = msg.IsDouble();
//bool mstr = msg.IsString();
//string msrc = msg.GetSource();
// Only accept HOST_INFO coming from our own community
//local check disabled for ROS, may cause errors
if((key == "PHI_HOST_INFO") /*&& msg_is_local*/)
handleMailHostInfo(sval);
// Only accept an ACK coming from a different community
else if((key == "NODE_BROKER_ACK") && !msg_is_local)
handleMailAck(sval);
else
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool imuSpatial_MOOSApp::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");
time_mission_prev = time_mission;
time_mission = Msg.GetDouble();
dt = time_mission - time_mission_prev;
}
}
return true;
}
bool Logs::OnNewMail(MOOSMSG_LIST &NewMail)
{
char chemin_fichier[500];
this->m_numero_mail ++;
// Récupération de la date
struct tm Debut = *localtime(&m_debut);
time_t maintenant;
time(&maintenant);
struct tm Today = *localtime(&maintenant);
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
if(msg.GetKey() == "VVV_ON_MISSION")
this->m_generer_logs = (msg.GetDouble() == 1.0);
if(msg.GetKey() == "VVV_NAV_X")
m_position_auv_x = msg.GetDouble();
if(msg.GetKey() == "VVV_NAV_Y")
m_position_auv_y = msg.GetDouble();
if(msg.GetKey() == "VVV_NAV_Z")
m_position_auv_z = msg.GetDouble();
if(msg.GetKey() == "VVV_AUV_NAME")
{
char date[20];
sprintf(date, "%2.2d-%2.2d-%4.4d", Today.tm_mday, Today.tm_mon + 1, Today.tm_year + 1900);
// Calcul du nom du fichier de logs
sprintf(chemin_fichier, "%s/%s_%s_%s.log", REPERTOIRE_LOGS, PREFIXE_FICHIER_LOGS, (char*)msg.GetString().c_str(), date);
this->m_nom_fichier = string(chemin_fichier);
this->m_auv_connecte = true;
}
if(msg.GetKey() == "VVV_IMAGE_BOTTOM" || msg.GetKey() == "VVV_IMAGE_SIDE")
{
IplImage *img = cvCreateImage(cvSize(LARGEUR_IMAGE_CAMERA, HAUTEUR_IMAGE_CAMERA), 8, 3);
if((int)msg.GetString().size() == img->imageSize)
memcpy(img->imageData, msg.GetString().data(), img->imageSize);
else
cout << "Erreur : mauvaises dimensions dans la variable image \"" << msg.GetKey() << "\" depuis la MOOSDB" << endl;
// Calcul du nom du fichier de logs
sprintf(chemin_fichier, "%s/", REPERTOIRE_LOGS);
MOOSCreateDirectory(string(chemin_fichier));
sprintf(chemin_fichier, "%s/%s/", chemin_fichier, (char*)msg.GetKey().c_str());
MOOSCreateDirectory(string(chemin_fichier));
sprintf(chemin_fichier, "%s/%04d%02d%02d_%02d%02d%02d/",
chemin_fichier,
(Debut.tm_year + 1900),
Debut.tm_mon,
Debut.tm_mday,
Debut.tm_hour,
Debut.tm_min,
Debut.tm_sec);
MOOSCreateDirectory(string(chemin_fichier));
sprintf(chemin_fichier, "%s%08d.jpeg", chemin_fichier, this->m_numero_mail);
Mat imgMat(img);
imwrite(chemin_fichier, imgMat);
}
#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 GUIMOOSApp::Iterate()
{
// Send vars GUI -> MOOS
{
CCriticalSectionLocker lock(& m_theWxApp->m_lstToSendToMOOS_cs );
pair<string,string> d;
while (! m_theWxApp->m_lstToSendToMOOS.empty() )
{
// Pop:
d = m_theWxApp->m_lstToSendToMOOS.front();
m_theWxApp->m_lstToSendToMOOS.pop();
// Send to MOOS:
m_Comms.Notify(d.first,d.second);
// cout << "Sending to MOOS: " << d.first << " -> " << d.second << endl;
}
}
// Only at a small frequency, query meta-information on existing modules, etc.
static CTicTac tim;
string LIST_MODULES_DATA;
if (tim.Tac()>5.0)
{
tim.Tic();
if(m_Comms.IsConnected())
{
MOOSMSG_LIST InMail;
if(m_Comms.ServerRequest("PROC_SUMMARY",InMail, 3.0 /* timeout */, false /* dont clear main */ ))
{
for(MOOSMSG_LIST::const_iterator p =InMail.begin();p!=InMail.end();++p)
{
const CMOOSMsg & rMsg = *p;
if ("PROC_SUMMARY"==rMsg.GetKey())
{
const string &val = rMsg.m_sVal;
// Typical string: "pLocalizationPF:SUBSCRIBED=...,PLOCALIZATIONPF_STATUS"
size_t i;
if ( (i=val.find(":"))!=string::npos)
{
const string mod_name = val.substr(0,i);
LIST_MODULES_DATA+=mod_name;
LIST_MODULES_DATA+=",";
//cout << "module: " << mod_name << endl;
// Subscribe, if we are not already, to the status of that module:
const string sVarName = mrpt::system::upperCase(mod_name)+string("_STATUS");
if (AddMOOSVariable(sVarName,sVarName,sVarName, 2.0 /* min. seconds between updates */) )
RegisterMOOSVariables(); // Only update if the variable is new.
}
}
} // end for each msg
}
}
} // end meta-information
// Process changes in MOSS -> GUI
{
CCriticalSectionLocker lock(& m_theWxApp->m_lstFromMOOS_cs);
// MOOS vars:
for (MOOSVARMAP::iterator it=m_MOOSVars.begin();it!=m_MOOSVars.end();it++)
if ( it->second.IsFresh() )
{
it->second.SetFresh(false);
if (it->second.IsDouble())
m_theWxApp->m_lstFromMOOS[it->first] = format("%f",it->second.GetDoubleVal());
else m_theWxApp->m_lstFromMOOS[it->first] = it->second.GetStringVal();
}
// MAPIR vars:
for (MOOSVARMAP::iterator it=m_OpenMORA_subscribedVars.begin();it!=m_OpenMORA_subscribedVars.end();it++)
if ( it->second.IsFresh() )
{
it->second.SetFresh(false);
if (it->second.IsDouble())
m_theWxApp->m_lstFromMOOS[it->first] = format("%f",it->second.GetDoubleVal());
else m_theWxApp->m_lstFromMOOS[it->first] = it->second.GetStringVal();
}
if (!LIST_MODULES_DATA.empty())
m_theWxApp->m_lstFromMOOS["LIST_MODULES"] = LIST_MODULES_DATA;
}
return true;
}
bool CMOOSLogger::DoAsyncLog(MOOSMSG_LIST &NewMail)
{
//log asynchronously...
if(m_bAsynchronousLog)
{
MOOSMSG_LIST::iterator q;
std::stringstream sStream[2];
for(q = NewMail.begin();q!=NewMail.end();q++)
{
CMOOSMsg & rMsg = *q;
//now see if we are logging this kind of message..
//if so we will have a variable named after it...
//which is used for the synchronous case..
if(m_MOOSVars.find(rMsg.m_sKey)!=m_MOOSVars.end())
{
std::stringstream sEntry;
sEntry.setf(ios::left);
sEntry.setf(ios::fixed);
sEntry<<setw(15)<<setprecision(3)<<rMsg.GetTime()-GetAppStartTime()<<' ';
sEntry<<setw(20)<<rMsg.GetKey().c_str()<<' ';
if(!m_bLogAuxSrc)
{
sEntry<<setw(15)<<rMsg.GetSource().c_str()<<' ';
}
else
{
std::string sT = rMsg.GetSource()+":"+(rMsg.GetSourceAux().empty() ? "NO_AUX_SRC" : rMsg.GetSourceAux());
sEntry<<setw(15)<<sT<<' ';
}
if(rMsg.IsDataType(MOOS_STRING) || rMsg.IsDataType(MOOS_DOUBLE))
{
sEntry<<rMsg.GetAsString(12,m_nDoublePrecision).c_str()<<' ';
}
else if(rMsg.IsDataType(MOOS_BINARY_STRING))
{
//here we append to the binary log and begin each line with a summary....
m_BinaryLogFile<<sEntry.str();
//write in coordinates in the alog
sEntry<<"<MOOS_BINARY>File="<<(m_sLogRootName+".blog")<<",Offset="<<m_BinaryLogFile.tellp()<<",Bytes="<<rMsg.m_sVal.size()<<"</MOOS_BINARY>";
//write the binary data to file
m_BinaryLogFile.write(rMsg.m_sVal.data(), rMsg.m_sVal.size());
//add a new line so even the binary log file is broadly human readable
m_BinaryLogFile<<std::endl;
}
int i=0;
if(m_bUseExcludedLog)
{
switch(GetDestinationLog(rMsg.m_sKey))
{
case XLOG: i = 1; break;
case ALOG: i = 0; break;
default:
i = 0;
}
}
sStream[i]<<sEntry.str()<<endl;
}
}
if(m_bCompressAlog)
{
//send to the worker thread...
m_AlogZipper.Push(sStream[0].str());
m_XlogZipper.Push(sStream[1].str());
}
else
{
//a regular write
if(m_AsyncLogFile.is_open())
m_AsyncLogFile<<sStream[0].str();
if(m_ExcludeLogFile.is_open())
m_ExcludeLogFile<<sStream[1].str();
}
}
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);
}
double dfNow = MOOS::Time();
if(dfNow-m_dfSummaryTime>2.0)
{
m_dfSummaryTime = dfNow;
//good spot to update our internal time
UpdateDBTimeVars();
//and send clients an occasional membersip list
UpdateDBClientsVar();
//update a db summary var once in a while
UpdateSummaryVar();
//update quality of service summary
UpdateQoSVar();
//update variable which publishes who is reading and writing what
UpdateReadWriteSummaryVar();
}
if(!MsgListRx.empty())
{
//now we fill in the packet with our replies to THIS CLIENT
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())
{
//MOOSTrace("%f OnRxPkt %d messages held for client %s\n",MOOSTime(),q->second.size(),sClient.c_str());
if(!q->second.empty())
{
//copy all the held mail to MsgListTx
MsgListTx.splice(MsgListTx.begin(),
q->second,
q->second.begin(),
q->second.end());
}
}
}
return true;
}
bool Sonar::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 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 ( key == "MICRON_PARAMS" && msg.IsString() && m_snrType == SeaNetMsg::MicronDST)
{
string msg_val = msg.GetString();
// Le message est de la forme "Range=25,Gain=45,Continuous=true"
// uPokeDB SONAR_PARAMS="Range=25,Gain=45,Continuous=true, VoS=1500, invert=1, nBins=200, AngleStep=, Gain=, LeftLimit=, RightLimit="
double dVal=0.0; int iVal; bool bVal;
if (MOOSValFromString(dVal, msg_val, "VoS", true))
m_msgHeadCommand.setVOS(dVal);
if (MOOSValFromString(dVal, msg_val, "Range", true))
m_msgHeadCommand.setRange(dVal);
if (MOOSValFromString(dVal, msg_val, "invert", true))
m_msgHeadCommand.setInverted(dVal);
if (MOOSValFromString(iVal, msg_val, "nBins", true))
{
m_iParamBins = iVal;
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);}
if (MOOSValFromString(dVal, msg_val, "RightLimit", true)){
m_msgHeadCommand.setRightLimit(dVal);}
// Envoi de la commande au sondeur
// TODO: vérifier que le CMOOSSerialPort est bien thread safe. Sinon, rajouter un mutex
//SendSonarMessage(m_msgHeadCommand);
//if we change params, sonar do not have params and had to send it again via listenMEssage Thread
m_bHasParams = false;
}
else if ( key == "MICRON_PARAMS" && msg.IsString() && m_snrType == SeaNetMsg::MiniKingNotDST){/* TO AVOID UNHANDELED MAIL*/}
else if ( key == "MINIKING_PARAMS" && msg.IsString() && m_snrType == SeaNetMsg::MiniKingNotDST)
{
string msg_val = msg.GetString();
// Le message est de la forme "Range=25,Gain=45,Continuous=true"
// uPokeDB SONAR_PARAMS="Range=25,Gain=45,Continuous=true, VoS=1500, invert=1, nBins=200, AngleStep=, Gain=, LeftLimit=, RightLimit="
double dVal=0.0; int iVal; bool bVal;
if (MOOSValFromString(dVal, msg_val, "VoS", true))
m_msgHeadCommand.setVOS(dVal);
if (MOOSValFromString(dVal, msg_val, "Range", true))
m_msgHeadCommand.setRange(dVal);
if (MOOSValFromString(dVal, msg_val, "invert", true))
m_msgHeadCommand.setInverted(dVal);
if (MOOSValFromString(iVal, msg_val, "nBins", true))
{
m_iParamBins = iVal;
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);}
if (MOOSValFromString(dVal, msg_val, "RightLimit", true)){
m_msgHeadCommand.setRightLimit(dVal);}
// Envoi de la commande au sondeur
// TODO: vérifier que le CMOOSSerialPort est bien thread safe. Sinon, rajouter un mutex
//SendSonarMessage(m_msgHeadCommand);
//if we change params, sonar do not have params and had to send it again via listenMEssage Thread
m_bHasParams = false;
}
else if ( key == "MINIKING_PARAMS" && msg.IsString() && m_snrType == SeaNetMsg::MicronDST){/* TO AVOID UNHANDELED MAIL*/}
else if ( key == "MICRON_POLL" && m_snrType == SeaNetMsg::MicronDST)
{
m_bPollSonar = (msg.GetDouble())?true:false;
if (m_bSonarReady && m_bSentCfg && m_bHasParams)
{
SeaNetMsg_SendData msg_SendData;
msg_SendData.setTime(MOOSTime());
SendSonarMessage(msg_SendData);
retractRunWarning("Sonar not initialized!");
}
else
{
reportRunWarning("Sonar not initialized!");
}
}
else if ( key == "MICRON_POLL" && msg.IsString() && m_snrType == SeaNetMsg::MiniKingNotDST){/* TO AVOID UNHANDELED MAIL*/}
else if ( key == "MINIKING_POLL" && m_snrType == SeaNetMsg::MiniKingNotDST)
{
m_bPollSonar = (msg.GetDouble())?true:false;
if (m_bSonarReady && m_bSentCfg && m_bHasParams)
{
SeaNetMsg_SendData msg_SendData;
msg_SendData.setTime(MOOSTime());
SendSonarMessage(msg_SendData);
retractRunWarning("Sonar not initialized!");
}
else
{
reportRunWarning("Sonar not initialized!");
}
}
else if ( key == "MINIKING_POLL" && msg.IsString() && m_snrType == SeaNetMsg::MicronDST){/* TO AVOID UNHANDELED MAIL*/}
else if ( key == "POWERED_SONAR" && m_snrType == SeaNetMsg::MicronDST)
{
m_bSonarReady = false;
if (m_serial_thread.IsThreadRunning())
m_serial_thread.Stop();
if (msg.GetDouble() == 0)
{
m_bIsAlive = false;
m_bReplyVersionData = false;
m_bReplyBBUserData = false;
m_bHasParams = false;
m_bSentCfg = false;
m_bIsPowered = false;
}
else
{
m_bIsPowered = true;
if (!m_serial_thread.IsThreadRunning())
m_serial_thread.Start();
}
}
else if ( key == "POWERED_SONAR" && msg.IsString() && m_snrType == SeaNetMsg::MiniKingNotDST){/* TO AVOID UNHANDELED MAIL*/}
else if ( key == "POWERED_MINIKING" && m_snrType == SeaNetMsg::MiniKingNotDST)
{
m_bIsAlive = false;
m_bReplyVersionData = false;
m_bReplyBBUserData = false;
m_bHasParams = false;
m_bSentCfg = false;
m_bSonarReady = false;
if (m_serial_thread.IsThreadRunning())
m_serial_thread.Stop();
if (msg.GetDouble() == 0)
m_bIsPowered = false;
else
{
m_bIsPowered = true;
if (!m_serial_thread.IsThreadRunning())
m_serial_thread.Start();
}
}
else if ( key == "POWERED_MINIKING" && msg.IsString() && m_snrType == SeaNetMsg::MicronDST){/* TO AVOID UNHANDELED MAIL*/}
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool CMOOSLogger::DoAsyncLog(MOOSMSG_LIST &NewMail)
{
//log asynchronously...
if(m_bAsynchronousLog)
{
MOOSMSG_LIST::iterator q;
std::stringstream sStream[2];
for(q = NewMail.begin();q!=NewMail.end();q++)
{
CMOOSMsg & rMsg = *q;
//now see if we are logging this kind of message..
//if so we will have a variable named after it...
//which is used for the synchronous case..
if(m_MOOSVars.find(rMsg.m_sKey)!=m_MOOSVars.end())
{
int i=0;
if(m_bUseExcludedLog)
{
switch(GetDestinationLog(rMsg.m_sKey))
{
case XLOG: i = 1; break;
case ALOG: i = 0; break;
default:
i = 0;
}
}
sStream[i].setf(ios::left);
sStream[i].setf(ios::fixed);
sStream[i]<<setw(15)<<setprecision(3)<<rMsg.GetTime()-GetAppStartTime()<<' ';
sStream[i]<<setw(20)<<rMsg.GetKey().c_str()<<' ';
sStream[i]<<setw(15)<<rMsg.GetSource().c_str()<<' ';
sStream[i]<<rMsg.GetAsString(12,m_nDoublePrecision).c_str()<<' ';
sStream[i]<<endl;
}
}
if(m_bCompressAlog)
{
//send to the worker thread...
m_AlogZipper.Push(sStream[0].str());
m_XlogZipper.Push(sStream[1].str());
}
else
{
//a regular write
if(m_AsyncLogFile.is_open())
m_AsyncLogFile<<sStream[0].str();
if(m_ExcludeLogFile.is_open())
m_ExcludeLogFile<<sStream[1].str();
}
}
return true;
}
bool CommunicationAngle::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if (MOOSStrCmp(msg.GetKey(), "VEHICLE_NAME")) {
m_veh_name = msg.GetString();
} else if (MOOSStrCmp(msg.GetKey(), "COLLABORATOR_NAME")) {
m_collab_name = msg.GetString();
m_collab_ready = true;
std::stringstream collab_var;
collab_var << m_collab_name << "_NAV_X";
m_Comms.Register(collab_var.str(), 0);
m_collab_nav_x = collab_var.str();
collab_var.str("");
collab_var.clear();
collab_var << m_collab_name << "_NAV_Y";
m_Comms.Register(collab_var.str(), 0);
m_collab_nav_y = collab_var.str();
collab_var.str("");
collab_var.clear();
collab_var << m_collab_name << "_NAV_DEPTH";
m_Comms.Register(collab_var.str(), 0);
m_collab_nav_depth = collab_var.str();
collab_var.str("");
collab_var.clear();
collab_var << m_collab_name << "_NAV_HEADING";
m_Comms.Register(collab_var.str(), 0);
m_collab_nav_heading = collab_var.str();
collab_var.str("");
collab_var.clear();
collab_var << m_collab_name << "_NAV_SPEED";
m_Comms.Register(collab_var.str(), 0);
m_collab_nav_speed = collab_var.str();
} else if (MOOSStrCmp(msg.GetKey(), "NAV_X")) {
m_x_pos = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), "NAV_Y")) {
m_y_pos = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), "NAV_DEPTH")) {
m_z_pos = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), "NAV_HEADING")) {
m_heading = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), "NAV_SPEED")) {
m_speed = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), "ELEV_ANGLE_REF")) {
reference_angle = msg.GetDouble()*PI/180;
}
if (m_collab_ready) {
if (MOOSStrCmp(msg.GetKey(), m_collab_nav_x)) {
m_collab_x_pos = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), m_collab_nav_y)) {
m_collab_y_pos = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), m_collab_nav_depth)) {
m_collab_z_pos = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), m_collab_nav_heading)) {
m_collab_heading = msg.GetDouble();
} else if (MOOSStrCmp(msg.GetKey(), m_collab_nav_speed)) {
m_collab_speed = 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 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 CMOOSLogger::HandleWildCardLogging()
{
static double dfLastWildCardTime = -1.0;
if(MOOSTime()-dfLastWildCardTime>DEFAULT_WILDCARD_TIME)
{
MOOSMSG_LIST InMail;
if(m_Comms.ServerRequest("VAR_SUMMARY", InMail, 2.0, false))
{
MOOSAssert(InMail.size()==1);
std::string ss(InMail.begin()->GetString());
bool bHit = false;
while(!ss.empty())
{
std::string sVar = MOOSChomp(ss);
if(GetMOOSVar(sVar)==NULL)
{
bool bWouldNormallyReject = IsWildCardRejected(sVar);
bool bWouldNormallyAccept = IsWildCardAccepted(sVar);
bool bWanted = false;
if(m_bUseExcludedLog)
{
bWanted = true;
if( bWouldNormallyAccept && !bWouldNormallyReject)
{
MOOSTrace(" Added wildcard logging of %-20s\n",sVar.c_str());
m_LogDestinations[sVar] = ALOG;
}
else
{
MOOSTrace(" Added wildcard logging of %-20s (xlog) \n",sVar.c_str());
m_LogDestinations[sVar] = XLOG;
}
}
else
{
if( bWouldNormallyAccept && !bWouldNormallyReject )
{
MOOSTrace(" Added wildcard logging of %-20s\n",sVar.c_str());
m_LogDestinations[sVar] = ALOG;
bWanted = true;
}
else if(bWouldNormallyAccept && bWouldNormallyReject)
{
//MOOSTrace(" denied added wildcard logging of %-20s (fits Omit pattern as well as Accept pattern)\n",sVar.c_str());
bWanted = false;
}
}
if(bWanted)
{
//yep we want to know....
if(AddMOOSVariable(sVar,sVar,"",0.0))
{
bHit = true;
}
}
}
}
if(bHit)
RegisterMOOSVariables();
}
dfLastWildCardTime = MOOSTime();
}
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 == "CLEAR_FOR_SHARE") {
handleSendHazardReport();
}
else if(key == "SHARED_DATA") {
handleReceiveHazardReport(sval);
}
else if(key == "DEPLOY") {
if (sval=="true") {
m_deployed = true;
m_deploy_time = MOOSTime();
m_rep_time = MOOSTime();
m_options_set = true; // <----------------------------- put below?
}
}
else if(key == "UHZ_MISSION_PARAMS") {
if (!m_options_set) {
handleMailMissionParams(sval);
}
}
// else if(key == "HAZARDREPORT_OTHER") {
// //m_hazard_set.findMinXPath(20);
// string summary_report = m_hazard_set.getSpec("final_report");
//
// Notify("HAZARDSET_REPORT", summary_report+sval);
// }
else if(key == "SEARCH_REPS") {
if (m_traverse_time == 0) {
m_traverse_time = m_search_time;
}
m_rep_time = MOOSTime();
m_repeats++;
}
else
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool MarinePID::OnNewMail(MOOSMSG_LIST &NewMail)
{
double curr_time = MOOSTime();
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
double dfT;
msg.IsSkewed(curr_time, &dfT);
string key = toupper(stripBlankEnds(msg.m_sKey));
#define ACCEPTABLE_SKEW_FIX_ME 360.0
if(fabs(dfT)<ACCEPTABLE_SKEW_FIX_ME) {
if((key == "MOOS_MANUAL_OVERIDE") ||
(key == "MOOS_MANUAL_OVERRIDE")) {
if(MOOSStrCmp(msg.m_sVal, "FALSE")) {
m_has_control = true;
MOOSTrace("\n");
MOOSDebugWrite("pMarinePID Control Is On");
}
else if(MOOSStrCmp(msg.m_sVal, "TRUE")) {
if(m_allow_overide) {
m_has_control = false;
MOOSTrace("\n");
MOOSDebugWrite("pMarinePID Control Is Off");
}
}
}
else if(key == "PID_VERBOSE") {
if(msg.m_sVal == "verbose")
m_verbose = "verbose";
else if(msg.m_sVal == "quiet")
m_verbose = "quiet";
else
m_verbose = "terse";
}
else if(key == "NAV_HEADING")
m_current_heading = msg.m_dfVal;
else if(key == "NAV_SPEED")
m_current_speed = msg.m_dfVal;
else if(key == "NAV_DEPTH")
m_current_depth = msg.m_dfVal;
else if(key == "NAV_PITCH")
m_current_pitch = msg.m_dfVal;
if(!strncmp(key.c_str(), "NAV_", 4))
m_time_of_last_nav_msg = curr_time;
else if(key == "DESIRED_SURGE_THRUST")
m_current_surge_thrust = msg.m_dfVal;
else if(key == "DESIRED_HEADING") {
m_desired_heading = msg.m_dfVal;
m_time_of_last_helm_msg = curr_time;
}
else if(key == "DESIRED_SPEED") {
m_desired_speed = msg.m_dfVal;
m_time_of_last_helm_msg = curr_time;
}
else if(key == "DESIRED_DEPTH") {
m_desired_depth = msg.m_dfVal;
m_time_of_last_helm_msg = curr_time;
}
}
}
return(true);
}
bool NodeReporter::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.m_sKey;
string sdata = msg.m_sVal;
double ddata = msg.m_dfVal;
if(key == "NAV_X") {
m_record.setX(ddata);
m_nav_xy_updated = m_curr_time;
}
else if(key == "NAV_Y") {
m_record.setY(ddata);
m_nav_xy_updated = m_curr_time;
}
else if(key == "NAV_LAT") {
m_record.setLat(ddata);
m_nav_latlon_updated = m_curr_time;
}
else if(key == "NAV_LONG") {
m_record.setLon(ddata);
m_nav_latlon_updated = m_curr_time;
}
else if(key == "NAV_SPEED")
m_record.setSpeed(ddata);
else if(key == "NAV_HEADING")
m_record.setHeading(angle360(ddata));
else if(key == "NAV_DEPTH")
m_record.setDepth(ddata);
else if(key == "NAV_YAW")
m_record.setYaw(ddata);
else if(key == "THRUST_MODE_REVERSE") {
bool reverse = (tolower(sdata)=="true");
m_record.setThrustModeReverse(reverse);
}
// BEGIN logic for checking for alternative nav reporting
if(m_alt_nav_prefix != "") {
bool record_gt_updated = true;
if(key == (m_alt_nav_prefix + "X")) {
m_record_gt.setX(ddata);
m_nav_xy_updated_gt = m_curr_time;
}
else if(key == (m_alt_nav_prefix + "Y")) {
m_record_gt.setY(ddata);
m_nav_xy_updated_gt = m_curr_time;
}
else if(key == (m_alt_nav_prefix + "LAT")) {
m_record_gt.setLat(ddata);
m_nav_latlon_updated_gt = m_curr_time;
}
else if(key == (m_alt_nav_prefix + "LONG")) {
m_record_gt.setLon(ddata);
m_nav_latlon_updated_gt = m_curr_time;
}
else if(key == (m_alt_nav_prefix + "SPEED"))
m_record_gt.setSpeed(ddata);
else if(key == (m_alt_nav_prefix + "HEADING"))
m_record_gt.setHeading(angle360(ddata));
else if(key == (m_alt_nav_prefix + "DEPTH"))
m_record_gt.setDepth(ddata);
else if(key == (m_alt_nav_prefix + "YAW"))
m_record_gt.setYaw(ddata);
else
record_gt_updated = false;
if(record_gt_updated)
m_record_gt_updated = m_curr_time;
}
// END logic for checking for alternative nav reporting
if(key == "AUX_MODE")
m_record.setModeAux(sdata);
else if(key == "PNR_PAUSE")
setBooleanOnString(m_paused, sdata);
else if(key == "LOAD_WARNING") {
string app = tokStringParse(sdata, "app", ',', '=');
string gap = tokStringParse(sdata, "maxgap", ',', '=');
if((app != "") && (gap != ""))
m_record.setLoadWarning(app + ":" + gap);
}
else if(key == "IVPHELM_SUMMARY") {
m_helm_lastmsg = m_curr_time;
handleLocalHelmSummary(sdata);
}
else if(key == "IVPHELM_ALLSTOP") {
m_helm_lastmsg = m_curr_time;
m_helm_allstop_mode = sdata;
}
else if(key == "IVPHELM_STATE") {
m_helm_lastmsg = m_curr_time;
string helm_status = stripBlankEnds(sdata);
if((helm_status == "DRIVE") ||
(helm_status == "PARK") ||
(helm_status == "MALCONFIG") ||
(helm_status == "DISABLED"))
m_helm_status_primary = helm_status;
else if((helm_status == "DRIVE+") ||
(helm_status == "PARK+") ||
(helm_status == "MALCONFIG+") ||
(helm_status == "STANDBY"))
m_helm_status_standby = helm_status;
if(strEnds(helm_status, "+"))
handleHelmSwitch();
if(strBegins(helm_status, "MALCONFIG"))
m_helm_allstop_mode = "n/a";
}
if(vectorContains(m_plat_vars, key)) {
string value = sdata;
if(msg.IsDouble())
value = dstringCompact(doubleToString(ddata, 8));
updatePlatformVar(key, value);
}
}
return(true);
}
bool USM_MOOSApp::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();
string sval = msg.GetString();
if(key == "DESIRED_THRUST") {
if(m_thrust_mode_differential == false)
m_model.setThrust(dval);
}
else if(key == "DESIRED_RUDDER") {
if(m_thrust_mode_differential == false)
m_model.setRudder(dval, MOOSTime());
}
else if(key == "DESIRED_THRUST_L") {
if(m_thrust_mode_differential == true)
m_model.setThrustLeft(dval);
}
else if(key == "DESIRED_THRUST_R") {
if(m_thrust_mode_differential == true)
m_model.setThrustRight(dval);
}
else if(key == "DESIRED_ELEVATOR")
m_model.setElevator(dval);
else if(key == "USM_SIM_PAUSED")
m_model.setPaused(toupper(sval) == "TRUE");
else if(key == "THRUST_MODE_REVERSE") {
m_thrust_mode_reverse = false;
if(tolower(sval) == "true")
m_thrust_mode_reverse = true;
m_model.setThrustModeReverse(m_thrust_mode_reverse);
}
else if(key == "THRUST_MODE_DIFFERENTIAL") {
setBooleanOnString(m_thrust_mode_differential, sval);
if(m_thrust_mode_differential)
m_model.setThrustModeDiff("differential");
else
m_model.setThrustModeDiff("normal");
}
else if((key == "USM_BUOYANCY_RATE") || // Deprecated
(key == "BUOYANCY_RATE"))
m_model.setParam("buoyancy_rate", dval);
else if((key == "USM_FORCE_THETA") || // Deprecated
(key == "ROTATE_SPEED"))
m_model.setParam("rotate_speed", dval);
else if((key == "USM_FORCE_X") || // Deprecated
(key == "CURRENT_X") ||
(key == "DRIFT_X")) {
m_srcs_drift.insert(msg.GetSource());
m_model.setParam("drift_x", dval);
}
else if((key == "USM_FORCE_Y") || // Deprecated
(key == "CURRENT_Y") ||
(key == "DRIFT_Y")) {
m_srcs_drift.insert(msg.GetSource());
m_model.setParam("drift_y", dval);
}
else if((key == "USM_FORCE_VECTOR") || // Deprecated
(key == "DRIFT_VECTOR")) {
m_srcs_drift.insert(msg.GetSource());
m_model.setDriftVector(sval, false);
}
else if((key == "USM_FORCE_VECTOR_ADD") || // Deprecated
(key == "DRIFT_VECTOR_ADD")) {
m_srcs_drift.insert(msg.GetSource());
m_model.setDriftVector(sval, true);
}
else if((key == "USM_FORCE_VECTOR_MULT") || // Deprecated
(key == "DRIFT_VECTOR_MULT")) {
m_srcs_drift.insert(msg.GetSource());
m_model.magDriftVector(dval);
}
else if((key == "USM_WATER_DEPTH") || // Deprecated
(key == "WATER_DEPTH"))
m_model.setParam("water_depth", dval);
else if(key == "USM_RESET") {
m_reset_count++;
Notify("USM_RESET_COUNT", m_reset_count);
m_model.initPosition(sval);
}
else if(key == "OBSTACLE_HIT") {
if(dval != 0)
m_obstacle_hit = true;
}
// Added buoyancy and trim control and sonar handshake. HS 2012-07-22
else if (key == "BUOYANCY_CONTROL") {
if(MOOSStrCmp(sval,"true")) {
// Set buoyancy to zero to simulate trim
m_model.setParam("buoyancy_rate", 0.0);
buoyancy_request_time = MOOSTime();
std::string buoyancy_status="status=1,error=0,progressing,buoyancy=0.0";
Notify("BUOYANCY_REPORT",buoyancy_status);
buoyancy_requested = true;
last_report = buoyancy_request_time;
}
}
else if(key == "TRIM_CONTROL") {
if(MOOSStrCmp(sval,"true")) {
trim_request_time = MOOSTime();
std::string trim_status="status=1,error=0,progressing,trim_pitch=0.0,trim_roll=0.0";
Notify("TRIM_REPORT",trim_status);
trim_requested = true;
last_report = trim_request_time;
}
}
#if 0 // OLD
// Added buoyancy and trim control and sonar handshake. HS 2012-07-22
else if(key == "BUOYANCY_CONTROL") {
if(MOOSStrCmp(sval,"true")) {
// Set buoyancy to zero to simulate trim
m_model.setParam("buoyancy_rate", 0.0);
buoyancy_request_time = MOOSTime();
std::string buoyancy_status="status=1,error=0,progressing,buoyancy=0.0";
Notify("BUOYANCY_REPORT",buoyancy_status);
buoyancy_requested = true;
}
}
else if(key == "TRIM_CONTROL") {
if(MOOSStrCmp(sval,"true")) {
trim_request_time = MOOSTime();
string trim_status="status=1,error=0,progressing,trim_pitch=0.0,trim_roll=0.0";
Notify("TRIM_REPORT",trim_status);
trim_requested = true;
}
}
else
reportRunWarning("Unhandled mail: " + key);
# endif // OLD
}
return(true);
}
bool FldWrapDetect::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(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
string key = msg.GetKey();
if((key == "NODE_REPORT") && (m_deployed)){
//if a node report is received, process the node report information.
string sval = msg.GetString();
NodeRecord record = string2NodeRecord(sval);
string node_name = record.getName();
double x2 = record.getX();
double y2 = record.getY();
// find last (x,y) position
NodeRecord prev_record = m_node_record[node_name];
if(! (prev_record.valid()) ){
m_node_record[node_name] = record;
m_odometer[node_name] = 0;
}
else{
double x1 = prev_record.getX();
double y1 = prev_record.getY();
double node_dist_traveled = distPointToPoint(x1,y1,x2,y2);
if(node_dist_traveled == 0){
return false;
}
m_odometer[node_name] += node_dist_traveled;
// now have (x,y) data for making new XYSegment
m_node_record[node_name] = record;
XYSegment new_segment;
new_segment.set(x1,y1,x2,y2);
// add new_segment to front of list;
m_xysegments[node_name].push_front(new_segment);
// trim segments down to allowed size
while(m_xysegments[node_name].size() > (unsigned int) m_max_num_segments){
m_xysegments[node_name].pop_back();
} // end while
if(CheckForWrap(node_name)){
reportEvent("Wrap Around Detected: " + node_name);
string out_string = "vname=" + node_name;
Notify("WRAP_AROUND_DETECTED",out_string);
m_num_wraps++;
m_xysegments[node_name].clear();
}
} // end else for valide node record
} // end key=node report
else if(key == "DEPLOY_ALL"){
m_deployed = true;
}
}// end for loop
return(true);
}
bool BacksteppingController::OnNewMail(MOOSMSG_LIST &NewMail) {
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(),"NAV_Y"))
{
//this message is about something called "NAV_Y"
CMOOSMsg &Msg = rMsg;
if(!Msg.IsDouble())
return MOOSFail("Ouch - was promised \"NAV_Y\" would be a double");
y = Msg.GetDouble();
//MOOSTrace("Input_FL is %f\n",x);//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(),"NAV_X"))
{
//this message is about something called "NAV_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"NAV_X\" would be a double");
x = Msg1.GetDouble();
//MOOSTrace("Y is %f\n",y);//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(),"NAV_YAW"))
{
//this message is about something called "Psi"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"NAV_YAW\" would be a double");
psi = Msg1.GetDouble();
//MOOSTrace("Psi is %f\n",ipsi);//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(),"NAV_SPEED"))
{
//this message is about something called "U"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"U\" would be a double");
u = Msg1.GetDouble();
//MOOSTrace("U is %f\n",u);//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(),"V"))
{
//this message is about something called "V"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"V\" would be a double");
v = Msg1.GetDouble();
//MOOSTrace("V is %f\n",v);//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(),"R"))
{
//this message is about something called "R"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"R\" would be a double");
r = Msg1.GetDouble();
//MOOSTrace("R is %f\n",R);//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(),"TIME"))
{
//this message is about something called "Time"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"TIME\" would be a double");
t = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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_SPEED"))
{
//this message is about something called "DESIRED_SPEED"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"DESIRED_SPEED\" would be a double");
ud = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e0_X"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e0_X\" would be a double");
e0_X = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e0_Y"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e0_Y\" would be a double");
e0_Y = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e2_X"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e2_X\" would be a double");
e2_X = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e2_Y"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e2_Y\" would be a double");
e2_Y = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e3_X"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e3_X\" would be a double");
e3_X = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e3_Y"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e3_Y\" would be a double");
e3_Y = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e1_X"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e1_X\" would be a double");
e1_X = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e1_Y"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e1_Y\" would be a double");
e1_Y = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e1_X"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e1_X\" would be a double");
e1_X = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"e1_Y"))
{
//this message is about something called "e0_X"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"e1_Y\" would be a double");
e1_Y = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//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(),"IMU_GPS_Stable"))
{
//this message is about something called "IMU_GPS_Stable"
CMOOSMsg &Msg1 = rMsg;
if(!Msg1.IsDouble())
return MOOSFail("Ouch - was promised \"IMU_GPS_Stable\" would be a double");
IMU_ready = Msg1.GetDouble();
//MOOSTrace("Ddyd is %f\n",input_FR);//the actual heading
//if you want to see all details in Msg, you can print a message by
//Msg.Trace();
}
}
return true;
}
/** This function stuffs messages in/from a packet */
bool CMOOSCommPkt::Serialize(MOOSMSG_LIST &List, bool bToStream, bool bNoNULL, double * pdfPktTime)
{
if(bToStream)
{
m_nMsgLen=0;
m_pNextData = m_pStream;
m_nByteCount = 0;
//we need to leave space at the start of the packet for total
//length and number of include messages
m_pNextData+=2*sizeof(int);
m_nByteCount+= 2* sizeof(int);
#define PKT_TMP_BUFFER_SIZE 40000
unsigned char TmpBuffer[PKT_TMP_BUFFER_SIZE];
unsigned char * pTmpBuffer = TmpBuffer;
MOOSMSG_LIST::iterator p;
int nCount = 0;
for(p = List.begin();p!=List.end();p++,nCount++)
{
//MOOSTrace("Sending %s \n",p->m_sKey.c_str());
int nTmpSize = PKT_TMP_BUFFER_SIZE;
int nCopied = (*p).Serialize(pTmpBuffer,nTmpSize);
if(nCopied!=-1)
{
//now copy to our stream...
CopyToStream(pTmpBuffer,nCopied);
}
else
{
return false;
}
}
//finally write how many bytes we have written at the start
//look for need to swap byte order if required
m_pNextData = m_pStream;
int nBC = IsLittleEndian() ? m_nByteCount : SwapByteOrder<int>(m_nByteCount);
memcpy((void*)m_pNextData,(void*)(&nBC),sizeof(m_nByteCount));
m_pNextData+=sizeof(m_nByteCount);
//and then how many messages are included
//look for need to swap byte order if required
int nMessages = List.size();
nMessages = IsLittleEndian() ? nMessages : SwapByteOrder<int>(nMessages);
memcpy((void*)m_pNextData,(void*)(&nMessages),sizeof(nMessages));
m_pNextData+=sizeof(nMessages);
}
else
{
m_pNextData = m_pStream;
m_nMsgLen = 0;
m_nByteCount = 0;
//first figure out the length of the message
//look to swap byte order as required
memcpy((void*)(&m_nMsgLen),(void*)m_pNextData,sizeof(m_nMsgLen));
m_nMsgLen = IsLittleEndian() ? m_nMsgLen : SwapByteOrder<int>(m_nMsgLen);
m_pNextData+=sizeof(m_nMsgLen);
m_nByteCount+=sizeof(m_nMsgLen);
int nSpaceFree=m_nMsgLen - sizeof(m_nMsgLen);
//no figure out how many messages are packed in this packet
//look to swap byte order as required
int nMessages = 0;
memcpy((void*)(&nMessages),(void*)m_pNextData,sizeof(nMessages));
nMessages = IsLittleEndian() ? nMessages : SwapByteOrder<int>(nMessages);
m_pNextData+=sizeof(nMessages);
nSpaceFree-=sizeof(nMessages);
m_nByteCount+=sizeof(nMessages);
for(int i = 0; i<nMessages;i++)
{
CMOOSMsg Msg;
int nUsed = Msg.Serialize(m_pNextData,nSpaceFree,false);
if(nUsed!=-1)
{
//allows us to not store NULL messages
bool bOmit = bNoNULL && (Msg.m_cMsgType==MOOS_NULL_MSG);
if(Msg.m_cMsgType==MOOS_NULL_MSG && pdfPktTime!=NULL && i == 0 )
{
*pdfPktTime = Msg.GetDouble();
}
if(!bOmit)
{
List.push_front(Msg);
}
m_pNextData+=nUsed;
m_nByteCount+=nUsed;
nSpaceFree-=nUsed;
}
else
{
//bad news...
break;
}
}
}
//here at the last moment we can fill in our totalm length for safe keeping
m_nMsgLen = m_nByteCount;
return true;
}