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 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 KalmanVisual::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string key = msg.GetKey();//Nome da mensagem
if(key == "PK[0]")
{
pk0 = msg.GetDouble();
t = MOOSTime();
}
if(key == "PK[1]")
{
pk1 = msg.GetDouble();
}
if(key == "PK[2]")
{
pk2 = msg.GetDouble();
}
if(key == "PK[3]")
{
pk3 = msg.GetDouble();
}
if(key == "PK[4]")
{
pk4 = msg.GetDouble();
}
if(key == "PK[5]")
{
pk5 = msg.GetDouble();
}
if(key == "PK[6]")
{
pk6 = msg.GetDouble();
}
if(key == "PK[7]")
{
pk7 = msg.GetDouble();
}
if(key == "X_ERRO[0]")
{
xerro0 = msg.GetDouble();
}
if(key == "X_ERRO[1]")
{
xerro1 = msg.GetDouble();
}
if(key == "X_ERRO[2]")
{
xerro2 = msg.GetDouble();
}
if(key == "X_ERRO[3]")
{
xerro3 = msg.GetDouble();
}
if(key == "X_ERRO[4]")
{
xerro4 = msg.GetDouble();
}
if(key == "X_ERRO[5]")
{
xerro5 = msg.GetDouble();
}
if(key == "X_ERRO[6]")
{
xerro6 = msg.GetDouble();
}
if(key == "X_ERRO[7]")
{
xerro7 = msg.GetDouble();
}
}
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 SensorViewer::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
//cout << msg.GetKey() << en
double pool_angle = MOOSDeg2Rad(0.0);
// **************** SENSORS DATA ************************
//
if( msg.GetKey() == "CAMERA_BOTTOM") {
memcpy(m_img_camera_bottom.data, msg.GetBinaryData(), m_img_camera_bottom.rows*m_img_camera_bottom.step);
}
else if( msg.GetKey() == "CAMERA_SIDE") {
memcpy(m_img_camera_side.data, msg.GetBinaryData(), m_img_camera_side.rows*m_img_camera_side.step);
}
else if(msg.GetKey() == "SONAR_RAW_DATA_MICRON"){
MOOSValFromString(m_new_bearing_micron, msg.GetString(), "bearing", true);
int nRows, nCols;
m_new_scanline_micron.clear();
MOOSVectorFromString(msg.GetString(), m_new_scanline_micron, nRows, nCols);
if(m_new_scanline_micron.size() != m_size_scanline_micron){
m_img_sonar_micron.create(2*m_new_scanline_micron.size(),2*m_new_scanline_micron.size(),CV_8UC(1));
m_img_micron.create(2*m_new_scanline_micron.size(),2*m_new_scanline_micron.size(),CV_8UC(3));
m_img_sonar_micron.setTo(Scalar(255));
m_img_micron.setTo(Scalar(255, 255, 255));
}
m_size_scanline_micron = m_new_scanline_micron.size();
m_new_data_micron = true;
}
else if(msg.GetKey() == "SONAR_RAW_DATA_MINIKING"){
MOOSValFromString(m_new_bearing_miniking, msg.GetString(), "bearing", true);
int nRows, nCols;
m_new_scanline_miniking.clear();
MOOSVectorFromString(msg.GetString(), m_new_scanline_miniking, nRows, nCols);
if(m_new_scanline_miniking.size() != m_size_scanline_miniking){
m_img_sonar_miniking.create(2*m_new_scanline_miniking.size(),2*m_new_scanline_miniking.size(),CV_8UC(1));
m_img_sonar_miniking.setTo(Scalar(255));
}
m_size_scanline_miniking = m_new_scanline_miniking.size();
m_new_data_miniking = true;
}
else if(msg.GetKey() == "SONAR_RAW_DATA"){
MOOSValFromString(m_new_bearing, msg.GetString(), "bearing", true);
int nRows, nCols;
m_new_scanline.clear();
MOOSVectorFromString(msg.GetString(), m_new_scanline, nRows, nCols);
if(m_new_scanline.size() != m_size_scanline){
m_img_sonar.create(2*m_new_scanline.size(),2*m_new_scanline.size(),CV_8UC(1));
m_img_sonar.setTo(Scalar(255));
}
m_size_scanline = m_new_scanline.size();
m_new_data = true;
}
// **************** ANALYSIS DATA ************************
//
// if( msg.GetKey() == "WALL_DETECTOR"){
// MOOSValFromString(m_bearing, msg.GetString(), "bearing");
// MOOSValFromString(m_distance, msg.GetString(), "distance");
// }
// **************** CONFIG DATA ************************
else if(msg.GetKey() == "SONAR_MINIKING_CONTRAST")
m_sonar_contrast_miniking = msg.GetDouble();
else if(msg.GetKey() == "SONAR_MICRON_CONTRAST")
m_sonar_contrast_micron = msg.GetDouble();
else if(msg.GetKey() == "SONAR_CONTRAST")
m_sonar_contrast = msg.GetDouble();
else if(msg.GetKey() == "WALL_DETECTOR_MICRON"){
}
else if(msg.GetKey() == "WALL_DETECTOR_MINIKING"){
double dist, bearing;
MOOSValFromString(bearing, msg.GetString(), "bearing", true);
MOOSValFromString(dist, msg.GetString(), "distance", true);
m_wall_detector_distance_miniking.push_back(dist);
m_wall_detector_bearing_miniking.push_back(bearing);
if(m_wall_detector_distance_miniking.size()>100){
m_wall_detector_distance_miniking.erase(m_wall_detector_distance_miniking.begin());
}
}
else if(msg.GetKey() == "WALL_DETECTOR_MICRON"){
double dist, bearing;
MOOSValFromString(bearing, msg.GetString(), "bearing", true);
MOOSValFromString(dist, msg.GetString(), "distance", true);
m_wall_detector_distance_micron.push_back(dist);
m_wall_detector_bearing_micron.push_back(bearing);
if(m_wall_detector_distance_micron.size()>100){
m_wall_detector_distance_micron.erase(m_wall_detector_distance_micron.begin());
}
}
}
return(true);
}
bool CAHGraphApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
std::string cad;
for(MOOSMSG_LIST::iterator i=NewMail.begin();i!=NewMail.end();++i)
{
/* Get path from node "Robot" to desired node
* GET_PATH "petitionID nX" (The petitionID is a unique identifier used by the Planer module)
*/
if( i->GetName() == "GET_PATH" )
{
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
printf( "\n\n[WorldModel]: Path requested to %s\n",lista.back().c_str());
size_t idtarget = graph.GetNodeId(lista.back());
size_t idrobot = graph.GetNodeId("Robot");
//! @moos_var PATH Sequence of nodes from "robot" to provided destiny in response to the "GET_PATH" var.
//! If node NOT found --> PATH "petitionID NOTFOUND"
//! If node found --> PATH "petitionID (nodeID1 node_label1 x1 y1) (nodeID2 node_label2 x2 y2) ..."
if (idtarget == (size_t)-1) //Node not found, cannot get path to it
{
printf( "[WorldModel]: ERROR - Node %s NOTFOUND in topology\n",lista.back().c_str());
//! @moos_publish PATH Sequence of nodes from "robot" to provided destiny in response to the "GET_PATH" var.
m_Comms.Notify("PATH", format("%s NOTFOUND",lista[0].c_str()));
}
else
{
//Since node Robot is only connected to the closest node in the graph (via a location arc),
//avoid node "robot" in the search by getting its node location
std::vector<size_t> neighbors;
graph.GetNodeNeighbors(idrobot,"Location",neighbors);
printf("[WorldModel]: Searching a path from id:%u to id:%u\n",(unsigned int)neighbors[0],(unsigned int)idtarget);
//Search path between two nodes of the graph
std::string path;
bool path_found = graph.FindPath(neighbors[0], idtarget, path);
if (path_found)
{
printf("[WorldModel]: Path FOUND!\n");
//! @moos_publish PATH Sequence of nodes from "robot" to provided destiny in response to the "GET_PATH" var.
m_Comms.Notify("PATH", format("%s %s",lista[0].c_str(),path.c_str()));
}
else
{
MOOSTrace("[WorldModel]: Path NOTFOUND\n");
//! @moos_publish PATH Sequence of nodes from "robot" to provided destiny in response to the "GET_PATH" var.
m_Comms.Notify("PATH", format("%s NOTFOUND", lista[0].c_str()));
}
}//end-if node found
}
// GET_PERSON_LOCATION "petitionID user_node_label" (requested by planner)
// Returns the node_label from the topology where the user is located
// A User(node) is defined in a location(another node) by a "location" arc between them.
if( i->GetName()=="GET_PERSON_LOCATION" )
{
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
//! @moos_var PERSON_LOCATION The result of finding a Person(node_label) in the Topological Graph
//! If NOT found = PERSON_LOCATION "petitionID NOTFOUND"
//! IF found = PERSON_LOCATION "petitionID location_node_label"
//check that (user)node_label exists in the Topological Graph
if ( graph.ExistsNodeLabel(lista[1]) )
{
size_t idperson = graph.GetNodeId(lista[1]);
printf("[WorldModel]: Searching for person %s (id: %d)\n",lista[1].c_str(),idperson);
std::vector<size_t> dest;
//Get list of neighbord nodes of type location
graph.GetNodeNeighbors(idperson,"Location",dest);
if (dest.size()!=0)
{
// Return the label of the node where the user is
std::string label="";
graph.GetNodeLabel(dest[0],label);
//! @moos_publish PERSON_LOCATION The result of finding a Person(node_label) in the Topological Graph
m_Comms.Notify("PERSON_LOCATION", format("%s %s",lista[0].c_str(),label.c_str()));
}
else
{
//! @moos_publish PERSON_LOCATION The result of finding a Person(node_label) in the Topological Graph
m_Comms.Notify("PERSON_LOCATION", format("%s NOTFOUND",lista[0].c_str()));
}
}
else
{
cout << "[WorldModel]: GET_PERSON_LOCATION ERROR - Node label not found." << endl;
//! @moos_publish PERSON_LOCATION The result of finding a Person(node_label) in the Topological Graph
m_Comms.Notify("PERSON_LOCATION", format("%s NOTFOUND",lista[0].c_str()));
}
}
// GET_NODE_POSITION "petitionID node_label"
// Returns the location (x,y) of a node given its label
if( i->GetName()=="GET_NODE_POSITION" )
{
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
//! @moos_variable NODE_POSITION geometric position of a node in response to the "GET_NODE_POSITION"
//! If NOT found = NODE_POSITION "petitionID NOTFOUND"
//! IF found = NODE_POSITION "petitoinID x y"
//check that node exists.
if ( graph.ExistsNodeLabel(lista[0]) )
{
double x,y;
bool res = graph.GetNodeLocation(lista[0],x,y);
//! @moos_publish NODE_POSITION geometric position of a node in response to the "GET_NODE_POSITION" var.
if (res)
m_Comms.Notify("NODE_POSITION", format("%s %f %f",lista[0].c_str(),x,y));
else
m_Comms.Notify("NODE_POSITION", format("%s %f %f notfound",lista[0].c_str(),x,y));
}
else
{
cout << "[WorldModel]: GET_NODE_POSITION EROR - Node label not found." << endl;
//m_Comms.Notify("NODE_POSITION", format("%s %f %f notfound",lista[0].c_str(),x,y));
//! @moos_publish NODE_POSITION geometric position of a node in response to the "GET_NODE_POSITION" var.
m_Comms.Notify( "NODE_POSITION", format("%s NOTFOUND",lista[0].c_str()) );
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Node label not found. Node position not published.");
}
}
//Add new node to topology --> ADD_NODE label type x y
if( (i->GetName()=="ADD_NODE") )
{
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
//Check that label doesn't exist in topology
if (!graph.ExistsNodeLabel(lista[0]))
{
cout << "[WorldModel]: Adding new node to Topology" << endl;
size_t id;
graph.AddNode(lista[0],lista[1],id,atof(lista[2].c_str()),atof(lista[3].c_str()));
RefreshGraph();
}
else
{
cout << "[WorldModel]: EROR - Node label already exists" << endl;
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Node label already exists. Node not Added.");
}
}
// Add new arc to topology --> ADD_ARC label_nodeA label_nodeB type
if( (i->GetName()=="ADD_ARC") )
{
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
//check that both nodes exists.
if (graph.ExistsNodeLabel(lista[0]) && graph.ExistsNodeLabel(lista[1]) )
{
cout << "[WorldModel]: Adding new arc to Topology" << endl;
size_t id;
// Add arc as Bidirectional: a->b + b->a
graph.AddArcbyLabel(lista[0].c_str(),lista[1].c_str(),"",lista[2].c_str(), true, id);
RefreshGraph();
}
else
{
cout << "[WorldModel]: EROR - Nodes not found. Arc not created. " << endl;
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Nodes not found. Arc not created.");
}
}
// Change location of existing node --> MOVE_NODE label x y
if( (i->GetName()=="MOVE_NODE") )
{
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
//check that node exists.
if ( graph.ExistsNodeLabel(lista[0]) )
{
printf("[WorldModel]: MOVE_NODE - Node %s moved successfully\n",lista[0].c_str());
graph.SetNodeLocation(lista[0],atof(lista[1].c_str()),atof(lista[2].c_str()));
RefreshGraph();
}
else
{
cout << "[WorldModel]: MOVE_NODE EROR - Node not found. Position not updated. " << endl;
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Node not found. Position not changed.");
}
}
// Change the label of existing node --> CHANGE_NODE_LABEL old_label new_label
if( (i->GetName()=="CHANGE_NODE_LABEL") )
{
bool error = false;
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
if (graph.ExistsNodeLabel(lista[0]))
{
//get ID of node
size_t node_id = graph.GetNodeId(lista[0]);
if (!graph.SetNodeLabel(node_id,lista[1]))
error = true;
else
RefreshGraph();
}
else
error = true;
if (error)
{
cout << "[WorldModel]: EROR: Node label not found or new label already exists. Cannot Rename it." << endl;
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Node label not changed because node not found or new_label already exists.");
}
}
// Remove Arcs between two given nodes: REMOVE_ARC nodeFrom nodeTo Arc_type
if( i->GetName()=="REMOVE_ARC" )
{
bool error = false;
std::deque<std::string> lista;
mrpt::system::tokenize(i->GetString()," ",lista);
//check that both nodes exists.
if (!graph.ExistsNodeLabel(lista[0]) || !graph.ExistsNodeLabel(lista[1]) )
error = true;
else
{
size_t idFrom = graph.GetNodeId(lista[0]);
size_t idTo = graph.GetNodeId(lista[1]);
if( graph.DeleteArcsBetweenNodes(idFrom, idTo, lista[2]) )
RefreshGraph();
else
error = true;
}
if (error)
{
cout << "[WorldModel]: EROR - Problem found when deleting arcs between two given nodes. Nodes not found." << endl;
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Problem found when deleting arcs between two given nodes. Nodes not found.");
}
}
// Remove Node from topological graph: REMOVE_NODE nodeLabel
if( i->GetName()=="REMOVE_NODE" )
{
if( graph.DeleteNode(i->GetString().c_str()) )
RefreshGraph();
else
{
cout << "[WorldModel]: EROR - Problem deleting Node. Node may not exists." << endl;
//! @moos_publish ERROR_MSG A string containing the description of an Error.
m_Comms.Notify("ERROR_MSG","pWorldModel: Problem deleting Node. Node may not exists.");
}
}
// Load Graph from file: LOAD_GRAPH file
if( i->GetName()=="LOAD_GRAPH" )
{
printf("[WorldModel]: Loading Topology graph from: [%s]\n",i->GetString().c_str());
graph.LoadGraph(i->GetString());
RefreshGraph();
}
// Save Graph to file: SAVE_GRAPH file
if( i->GetName()=="SAVE_GRAPH" )
{
printf("[WorldModel]: Saving Topology graph to: [%s]\n",i->GetString().c_str());
graph.SaveGraph(i->GetString());
RefreshGraph();
}
// SHUTDOWN
if( (i->GetName()=="SHUTDOWN") && (MOOSStrCmp(i->GetString(),"true")) )
{
// Disconnect comms:
MOOSTrace("Closing Module \n");
this->RequestQuit();
}
}
UpdateMOOSVariables(NewMail);
return true;
}
bool GenPath::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
string visit_point;
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
if (MOOSStrCmp(msg.GetKey(), "VISIT_POINT")) {
visit_point = msg.GetString();
//if its the firstpoint flag, init variables by clearing
if (MOOSStrCmp(visit_point, "firstpoint")) {
m_x_pts.clear();
m_y_pts.clear();
m_got_all_pts = false;
m_assigned_pts = false;
m_pt_counter = 0;
//if its the laspoint flag, flag that we have received all points so that we can calculate tour
} else if (MOOSStrCmp(visit_point, "lastpoint")) {
for (unsigned int i=0; i<m_x_pts.size(); i++) {
cout << m_x_pts[i] << "," << m_y_pts[i] << "," << m_id_pts[i] << endl;
}
m_got_all_pts = true;
m_num_pts = m_x_pts.size();
//otherwise, parse the point string and push it onto our position vectors
} else {
m_x_pts.push_back(atof(tokStringParse(visit_point, "x", ',', '=').c_str()));
m_y_pts.push_back(atof(tokStringParse(visit_point, "y", ',', '=').c_str()));
m_id_pts.push_back(atoi(tokStringParse(visit_point, "id", ',', '=').c_str()));
}
//get x pos
} else if (MOOSStrCmp(msg.GetKey(), "NAV_X")) {
m_x_pos = msg.GetDouble();
m_received_pos = true;
//get y pos
} else if (MOOSStrCmp(msg.GetKey(), "NAV_Y")) {
m_y_pos = msg.GetDouble();
m_received_pos = true;
//get the current waypoint of the path from the waypoint bhv
} else if (MOOSStrCmp(msg.GetKey(), "WPT_INDEX")) {
m_received_wpt_idx = true;
unsigned int new_wpt = (unsigned int)msg.GetDouble();
if (new_wpt > m_wpt_idx) {
m_wpt_idx = new_wpt;
}
cout << m_wpt_idx << endl;
if (m_wpt_idx > 0) {
cout << "Distance to prev point: " << m_wpt_dist << endl;
cout << m_x_pts_missed.size() << endl;
//perform a check to see if we actually visited the previous point
if (m_wpt_dist > m_visit_radius) {
m_x_pts_missed.push_back(m_x_pts_ordered[m_wpt_idx-1]);
m_y_pts_missed.push_back(m_y_pts_ordered[m_wpt_idx-1]);
m_id_pts_missed.push_back(m_id_pts_ordered[m_wpt_idx-1]);
}
}
//bhv may request a regeneration of the tour for any missed points
} else if (MOOSStrCmp(msg.GetKey(), "GENPATH_REGENERATE")) {
m_regen_path = 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 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 == "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);
}
else if ( key == "SONAR_POLL")
{
m_bPollSonar = (msg.GetDouble())?true:false;
if (m_bSonarReady && m_bSentCfg && !m_bNoParams)
{
SeaNetMsg_SendData msg_SendData;
msg_SendData.setTime(MOOSTime());
SendSonarMessage(msg_SendData);
retractRunWarning("Sonar not initialized!");
}
else
{
reportRunWarning("Sonar not initialized!");
}
}
else if ( key == "POWERED_SONAR")
{
if (msg.GetDouble() == 0)
{
m_bIsPowered = false;
m_bNoParams = true;
m_bSentCfg = false;
m_bSonarReady = false;
m_bPollSonar = false;
m_serial_thread.Stop();
}
else
{
m_bIsPowered = true;
Initialization();
}
}
else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
reportRunWarning("Unhandled Mail: " + key);
}
return(true);
}
bool Logger::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p = NewMail.begin() ; p != NewMail.end() ; p++)
{
CMOOSMsg &msg = *p;
bool handled = false;
if(msg.GetKey() == "NAV_X")
m_nav_x = msg.GetDouble();
if(msg.GetKey() == "NAV_Y")
m_nav_y = msg.GetDouble();
if(msg.GetKey() == "NAV_DEPTH")
m_nav_depth = msg.GetDouble();
if(msg.GetKey() == "NAV_SPEED")
m_active_speed = msg.GetDouble();
if(msg.GetKey() == "DRIFT_X")
m_drift_x = msg.GetDouble();
if(msg.GetKey() == "DRIFT_Y")
m_drift_y = msg.GetDouble();
if(msg.GetKey() == "CONSO_NOMINAL")
m_conso_nominal = msg.GetDouble();
if(msg.GetKey() == "CONSO_PROP")
m_conso_prop = msg.GetDouble();
if(msg.GetKey() == "CONSO_COMM")
m_conso_comm = msg.GetDouble();
if(msg.GetKey() == "CONSO_ALL")
m_conso_all = msg.GetDouble();
if(msg.GetKey() == "RATIO_RECORDING")
m_ratio_recording = msg.GetDouble();
if(msg.GetKey() == "ESTIMATION_DURATION_DAYS")
m_estimation_duration_days = msg.GetDouble();
if(!handled)
cout << "Unhandled Mail: " << msg.GetKey() << endl;
#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::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 CMOOSRemoteLite::FetchDB()
{
EnableMailLoop(false);
MOOSTrace("\n\n****** DB Fetch ******\n");
MOOSMSG_LIST MsgList;
if(m_Comms.ServerRequest("ALL",MsgList))
{
MOOSMSG_LIST::iterator p;
for(p=MsgList.begin();p!=MsgList.end();p++)
{
CMOOSMsg & rMsg = *p;
ostringstream os;
//ostrstream os;
os<<setw(22);
os<<rMsg.m_sKey.c_str();
os<<" ";
os<<setw(16);
os<<rMsg.m_sSrc.c_str();
os<<" ";
os<<setw(9);
os<<rMsg.m_dfTime;
os<<" ";
os<<setw(10);
switch(rMsg.m_cDataType)
{
case MOOS_STRING:
os<<rMsg.m_sVal.c_str();
break;
case MOOS_NOT_SET:
os<<"*";
break;
case MOOS_BINARY_STRING:
os<<rMsg.GetAsString().c_str();
break;
default:
os<<" ";
break;
}
os<<" ";
os<<setw(14);
switch(rMsg.m_cDataType)
{
case MOOS_DOUBLE:
os<<rMsg.m_dfVal;
break;
case MOOS_NOT_SET:
os<<"*";
break;
default:
os<<" ";
break;
}
os<<endl<<ends;
string sText = os.str();
MOOSTrace("%s",sText.c_str());
//os.rdbuf()->freeze(0);
}
}
else
{
MOOSTrace("DB failed to talk to me\n");
}
//renable mail loop...
EnableMailLoop(true);
return true;
}
