// ---------------------------------------------------------------------------- // // $Id$ // // Copyright 2008, 2009, 2010, 2011, 2012 Antonio Franchi and Paolo Stegagno // // This file is part of MIP. // // MIP is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // MIP is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with MIP. If not, see <http://www.gnu.org/licenses/>. // // Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it // // ---------------------------------------------------------------------------- #include <CommModule.h> #include <IPCommModule.h> #include <TaskDict.h> #include <CommonOptions.h> // #define EXAMPLE_CM_DEBUG // using namespace MipBaselib; using namespace MipResources; using namespace MipTasks; CommModule *commModule; int myRobId; int myTaskPlate; int receivedPackets = 0; void receiveCommPackets(){ // cout << "[RECEIVING PACKETS OF TASK " << myTaskPlate << "]" << endl; // fill the receiving box CommBox entPackets; commModule->receive(myTaskPlate, entPackets); // parse the received messages int size = entPackets.size(); for (int i=0; i<size; i++) { /*initial packet checks*/ CommPacket rP = *entPackets[i]; if (rP.sender().getIdsSize()<=0) { cout << "MIP-warning: Message without sender received" << endl; continue; } int otherId = rP.sender().getId(0); if (otherId<0) { cout << "MIP-warning: Message with invalid sender received: " << otherId << endl; continue; } // if (otherId!=myRobId){ receivedPackets ++; /*packet parsing*/ if (rP.messageType()=="MYPOSE") { /*the message is a pose*/ Serialization<Pose> srl(rP.message()); Pose otherPose = srl.getObj(); cout << receivedPackets << ". " << otherId << " MYPOSE " << otherPose.print() << ", " << rP.time().print() << endl; }else if(rP.messageType()=="MYANGVEL"){ /*the message is an angvel*/ Serialization<Decimal> srl(rP.message()); Decimal otherAngVel = srl.getObj(); cout << receivedPackets << ". " << otherId << " MYANGVEL " << otherAngVel << ", " << rP.time().print() << endl; }else{ cout << "MIP-warning: Message with unused type received: " << otherId << endl; } // } } // erase parsed messages commModule->receiveAck(ENT_TASK, entPackets); } void sendCommPackets(){ // sender, this is standard CommNode sender(myRobId,myTaskPlate); // recipient: robots=allRobots,tasks={ENT_TASK,myTaskPlate} CommNode recipient; if(myRobId==2) while(!recipient.addId(0)){}; if(myRobId==0) while(!recipient.addId(2)){}; while(!recipient.addTask(ENT_TASK)){}; while(!recipient.addTask(DRV_TASK)){}; // prepare a Pose to transmit Pose myPose(((Decimal)myRobId)+0.12,-78.45,Angle(0.33*M_PI)); Serialization<Pose> srlP(myPose); CommPacket pP(srlP.getStr(),"MYPOSE",sender,recipient); // cout << pP.print() << endl; // prepare a Decimal to transmit Decimal myAngVel(0.11); Serialization<Decimal> srlA(myAngVel); CommPacket pA(srlA.getStr(),"MYANGVEL",sender,recipient); // Decimal myAngVel(0.11); // Serialization<Decimal> srlA(myAngVel); CommPacket pC(srlA.getStr(),"MYANGVEL",sender,recipient); // send the packets // cout << "mando" << endl; commModule->send(pP); commModule->send(pA); commModule->send(pC); } int main(int argc, const char* argv[]){ //------- -------- --------- --------- --------- --------- --------- --------- // bootstrap //------- -------- --------- --------- --------- --------- --------- --------- // commonOptions are taken CommonOptions::init(argc,(char* const *)argv); commonOptions->updateValues(); // the commModule is loaded by the scheduler commModule = new IPCommModule(argc,argv); // Each communicating tasks must request its boxes commModule->requestBox(ENT_TASK); commModule->requestBox(DRV_TASK); myRobId = commonOptions->robotId->getValue(); cout << "[ROBOT ID " << myRobId << "]" << endl; //------- -------- --------- --------- --------- --------- --------- --------- // use //------- -------- --------- --------- --------- --------- --------- --------- Timer timer; while(1){ if(timer.get() > Time(1,0)){ // SEND from ENT_TASK // get myTaskPlate (inside a task you must use "getPlate()") myTaskPlate = ENT_TASK; sendCommPackets(); timer.reset(); } #ifdef EXAMPLE_CM_DEBUG cout << "[COMMUNICATION MODULE STATUS]" << endl; cout << commModule->print() << endl; #endif // RECEIVE in ENT_TASK // get myTaskPlate (inside a task you must use "getPlate()") myTaskPlate = ENT_TASK; receiveCommPackets(); #ifdef EXAMPLE_CM_DEBUG cout << "[COMMUNICATION MODULE STATUS]" << endl; cout << commModule->print() << endl; #endif // RECEIVE in DRV_TASK // get myTaskPlate (inside a task you must use "getPlate()") myTaskPlate = DRV_TASK; receiveCommPackets(); // // #ifdef EXAMPLE_CM_DEBUG // cout << "[COMMUNICATION MODULE STATUS]" << endl; // cout << commModule->print() << endl; // #endif usleep(1000000); } }