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;

}
示例#3
0
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;
}
示例#4
0
/** 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);
}