exampleCommModule.cpp

This is an example of how to use the CommModule class. More details about this example.

// ----------------------------------------------------------------------------
//
// $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);
  
 }
}