bool CPlannerApp::Iterate()
{
IterateBase();
// Are there
CMOOSVariable *path = GetMOOSVar("PATH");
CMOOSVariable *PLAN_FINISHED = GetMOOSVar("PLAN_FINISHED");
if (PLAN_FINISHED && PLAN_FINISHED->IsFresh())
{
PLAN_FINISHED->SetFresh(false);
planner.FinishedCommand(atoi(PLAN_FINISHED->GetStringVal().c_str()));
}
if (planner.PendingPetitions())
{
planner.pet_sem.enter();
std::string petition=planner.pet_map.begin()->second;
//printf("Pending %d,%s\n",planner.pet_map.begin()->first,petition.c_str());
std::deque<std::string> cad;
mrpt::utils::tokenize(petition," ",cad);
//printf("notifying %s %d %s\n",cad[0].c_str(),planner.pet_map.begin()->first,cad[1].c_str());
m_Comms.Notify(cad[0],format("%d %s",(int)planner.pet_map.begin()->first,cad[1].c_str()));
planner.asked_petitions.erase(planner.pet_map.begin()->first);
//printf("borre %d y quedan %d\n",planner.pet_map.begin()->first,planner.asked_petitions.size());
planner.pet_sem.leave();
}
if (path && path->IsFresh())
{
path->SetFresh(false);
// printf("%s\n",path->GetStringVal().c_str());
std::deque<std::string> lista;
mrpt::utils::tokenize(path->GetStringVal()," ",lista);
planner.pet_sem.enter();
std::deque<std::string> cad;
mrpt::utils::tokenize(path->GetStringVal()," ",cad);
planner.answer_map.insert(pet_pair(atoi(cad[0].c_str()),path->GetStringVal()));
planner.pet_sem.leave();
}
return true;
}
bool CMOOSRemoteLite::PrintCustomSummary()
{
ostringstream os;
os<<"\n****** User Custom Summary ******"<<endl;
STRING_LIST::iterator p;
for(p = m_CustomSummaryList.begin();p!=m_CustomSummaryList.end();p++)
{
string sWhat = *p;
CMOOSVariable * pVar = GetMOOSVar(sWhat);
if(pVar==NULL)
continue;
//make left justified
os.setf(ios::left);
os<<setw(20)<<sWhat.c_str()<<"t=";
os.setf(ios::fixed);
if(!pVar->IsFresh())
{
os<<setw(10)<<"****";
os<<setw(3)<<" : ";
os<<setw(10)<<"****";
}
else
{
os<<setw(10)<<setprecision(1)<<pVar->GetTime()-GetAppStartTime();
os<<setw(3)<<" : ";
if(pVar->IsDouble())
{
os<<setw(10)<<setprecision(1)<<pVar->GetDoubleVal();
}
else
{
os<<setw(10)<<pVar->GetStringVal().c_str();;
}
}
os<<endl;
}
os<<ends;
MOOSTrace("%s",string(os.str()).c_str());
return true;
}
bool CPlannerApp::Iterate()
{
IterateBase();
// Is plan finished?
CMOOSVariable *PLAN_FINISHED = GetMOOSVar("PLAN_FINISHED");
if (PLAN_FINISHED && PLAN_FINISHED->IsFresh())
{
PLAN_FINISHED->SetFresh(false);
planner.FinishedCommand(atoi(PLAN_FINISHED->GetStringVal().c_str()));
}
// PETITIONS
// Check for pending petitions: "asked_petitions" list.
// Petitions are requests of information to other modules via OpenMORA variables.
//-----------------------------------------------------------------------------------
if (planner.PendingPetitions())
{
planner.pet_sem.enter();
//Get first pending petition (petID, what)
std::string petition = planner.pet_map.begin()->second; //The string with the "what" to perform. ej "GET_PATH nX"
std::deque<std::string> cad;
mrpt::system::tokenize(petition," ",cad);
//printf("notifying %s %d %s\n",cad[0].c_str(),planner.pet_map.begin()->first,cad[1].c_str());
//! @moos_publish <OpenMORA_VAR> This variable could be any OpenMORA variable. It is the result of a scheduled task. The first parameter is always the petitionID.
// ej. Notify(GET_PATH,"petitionID nX")
// ej. Notify(GET_PERSON_LOCATION, "petitionID userID")
m_Comms.Notify(cad[0], format("%d %s", (int)planner.pet_map.begin()->first, cad[1].c_str()));
//Remove petition from pending list.
planner.asked_petitions.erase(planner.pet_map.begin()->first);
planner.pet_sem.leave();
}
// PATH
// Check if any PATH petition has been answered by the WorldModel
// Path = "petitionID NOTFOUND" (If node or path not found in the topology)
// Path = "petitionID (nodeID1 nodeLabel1 nodeX1 nodeY1) (nodeID2 nodeLanel2 nodeX2 nodeY2)..."
//-------------------------------------------------------------------------------------------
CMOOSVariable *path = GetMOOSVar("PATH");
if (path && path->IsFresh())
{
path->SetFresh(false);
printf("[Planner:Iterate] New Path received: '%s'",path->GetStringVal().c_str());
planner.pet_sem.enter();
std::deque<std::string> cad;
mrpt::system::tokenize(path->GetStringVal()," ",cad);
// remove the petitionID identifier from response
std::string response = path->GetStringVal().substr((size_t)1 + path->GetStringVal().find(" "));
// answer_map(petitionID, response)
planner.answer_map.insert( pet_pair(atoi(cad[0].c_str()),response) );
planner.pet_sem.leave();
}
// PERSON_LOCATION
// Check if any PERSON_LOCATION request has been answered by WorldModel
// PERSON_LOCATION "petitionID NOTFOUND"
// PERSON_LOCATION "petitoinID location_node_label"
//-----------------------------------------------------------------------
CMOOSVariable *person_location = GetMOOSVar("PERSON_LOCATION");
if (person_location && person_location->IsFresh())
{
person_location->SetFresh(false);
printf("%s\n",person_location->GetStringVal().c_str());
planner.pet_sem.enter();
std::deque<std::string> cad;
mrpt::system::tokenize(person_location->GetStringVal()," ",cad);
// remove the petitionID identifier from response
std::string response = person_location->GetStringVal().substr((size_t)1 + person_location->GetStringVal().find(" "));
// answer_map(petitionID, response)
planner.answer_map.insert( pet_pair(atoi(cad[0].c_str()),response) );
planner.pet_sem.leave();
}
return true;
}
/** UpdateTopologicalPlace
* Set the ID of the closest node to the current robot position.
* It updates the arcs between the graph and the "robot" node
* which indicates the node closest to the robot
*/
void CAHGraphApp::UpdateTopologicalPlace()
{
std::string closest_node = "empty";
mrpt::poses::CPose2D robotPose2D;
mrpt::math::CVectorDouble Robot_coord;
//Get current Robot pose. LOCALIZATION x y phi
CMOOSVariable * pVarLoc = GetMOOSVar( "LOCALIZATION" );
if(pVarLoc && pVarLoc->IsFresh())
{
pVarLoc->SetFresh(false);
robotPose2D.fromString( pVarLoc->GetStringVal() );
robotPose2D.getAsVector(Robot_coord);
if (verbose)
printf("\n Robot Pose is: [%.2f,%.2f]",Robot_coord[0],Robot_coord[1]);
}
else
{
MOOSTrace(mrpt::format("[%s]: Warning: 'LOCALIZATION' not available -> ROBOT_TOPOLOGICAL_PLACE not updated.\n", GetAppName().c_str() ));
return;
}
//Get closest node to current Robot location
std::string sNodes;
graph.GetAllNodes(sNodes);
std::string delimiter = "#";
if (verbose)
cout << "\n [WorldModel]: Estimating current node from list: " << sNodes << endl;
size_t pos = 0;
std::string snode_i;
float min_distance = 0.0;
bool first_node = true;
while ((pos = sNodes.find(delimiter)) != std::string::npos)
{
snode_i = sNodes.substr(0, pos); //get node [name x y]
size_t pos2 = 0;
//node name
pos2 = snode_i.find(" ");
std::string nodeName = snode_i.substr(0, pos2);
snode_i.erase(0, pos2 + 1);
//node x
pos2 = snode_i.find(" ");
std::string nodeX = snode_i.substr(0, pos2);
snode_i.erase(0, pos2 + 1);
//node y
std::string nodeY = snode_i;
if (nodeName.compare("Robot") != 0 )
{
//Distance from Robot_pose to node_i
float c1 = abs( atof(nodeX.c_str()) - (float)Robot_coord[0] );
float c2 = abs( atof(nodeY.c_str()) - (float)Robot_coord[1] );
float distance = sqrt( square(c1) + square(c2) );
if (verbose)
printf("[WorldModel]: Node: %s|%s|%s at d=%.2f\n",nodeName.c_str(),nodeX.c_str(),nodeY.c_str(),distance);
if ( (first_node) || (distance < min_distance) )
{
min_distance = distance;
closest_node = nodeName;
first_node = false;
}
}
if (pos != sNodes.length()-1)
{
//Delete node from list of Nodes to be parsed
sNodes.erase(0, pos + delimiter.length());
}
else
{
break;
}
}//end-while
//Update Graph
size_t id = 0;
size_t idrobot = graph.GetNodeId("Robot");
if( graph.ExistsNodeLabel(closest_node) )
{
size_t idtarget = graph.GetNodeId(closest_node);
//Did Robot_Topological_Place changed?
std::vector<size_t> neighbors;
graph.GetNodeNeighbors(idrobot,"Location",neighbors);
if (neighbors[0] != idtarget )
{
graph.DeleteOutcommingArcs(idrobot,"Location");
printf("[WorldModel]: Updating robot pose to %s\n",closest_node.c_str());
graph.AddArc(idrobot,idtarget,"robotpose","Location",id);
RefreshGraph();
}
}
//! @moos_publish ROBOT_TOPOLOGICAL_PLACE The id of the closet node in the world model graph to the current robot position
m_Comms.Notify("ROBOT_TOPOLOGICAL_PLACE",closest_node);
}