source: libs/libSamgar/src/Samgar.cpp @ 445

Revision 445, 16.8 KB checked in by lmalek, 10 years ago (diff)

Samgar edition by WRUT. Based on Qt. Initial import.

Line 
1#include <Samgar.h>
2#include <iostream>
3
4using namespace std;
5using namespace yarp::os;
6using namespace yarp::sig;
7
8namespace Samgar{
9
10////////////////////////////////////////////////////////////////////////////////
11// Data Port
12////////////////////////////////////////////////////////////////////////////////
13
14  void DataPort::onRead(Bottle& b)
15  {
16    *Status = StateRunning;
17    SavedBottle=b;
18  }
19 
20  Bottle DataPort::getBottle(){
21    Bottle result=SavedBottle;
22    SavedBottle=Bottle::getNullBottle();
23    return result;
24  }
25
26  DataPort::DataPort(ModuleState *state) {
27    Status = state;
28    SavedBottle=Bottle::getNullBottle();
29  }
30
31////////////////////////////////////////////////////////////////////////////////
32
33  /*!
34   * Constructor class, needs four varibles
35   * first is the name of module on the network
36   * secound is the main catagory of the module
37   * third is the sub catagory of the module
38   * forth is the type of module either -ModeRun -ModeInterupt
39   */
40  SamgarModule::SamgarModule(string NameOfModule, string Catagory,
41                             string SubCatagory, ModuleMode Type) {
42    Network yarp;
43    MyName          = NameOfModule;
44    MyCatagory      = Catagory;
45    MySubCatagory   = SubCatagory;
46    modulemode      = Type;
47    fullModuleName  = "/Main_" + MyName;
48    DoIWantModules  = false;
49    if( modulemode == ModeInterupt )
50        currentmode = Samgar::StateStoped;
51    else
52        currentmode = Samgar::StateRunning;
53  }
54
55  /*!
56   * A method which enables the module list to be updated when new data
57   * is available or called for on the network
58   * SendAllModulesCommand can be used to update the module list
59   */
60  void SamgarModule::TurnOnModuleListener(void){
61    DoIWantModules=true;
62  }
63
64  /*!
65   * Sends a query to SAMGAR Key to get available platforms
66   */
67  void SamgarModule::GetAvailPlatforms(void) {
68    Bottle& MyBottle = prepare();
69    MyBottle.clear();
70    MyBottle.addInt( AvailablePlatformsCode );
71    write();
72  }
73
74  /*!
75   * This function sends commands to all available modules within the group
76   * 0 = stop module , 1 = start module , 2 = get modules to respond with
77   * name and type
78   */
79  void SamgarModule::SendAllModulesCommand(int cm) {
80    Bottle& MyBottle = prepare();
81    MyBottle.clear();
82    MyBottle.addInt(cm);
83    write();
84  }
85
86  /*!
87   * This function sends commands to a singuler module
88   * 3 = stop module , 4 = start module , 5 = get modules to respond
89   * with name and type
90   */
91  void SamgarModule::SendModuleCommand(string name, int cm) {
92    Bottle& MyBottle = prepare();
93    MyBottle.clear();
94    MyBottle.addInt(cm);
95    MyBottle.addString(name.c_str()); // always adds to end of current list
96    write();
97  }
98
99  /*!
100   * This function is a interupt driven module for the main port of the module
101   * DO NOT MANUALLY CALL
102   */
103  void SamgarModule::onRead(Bottle& b) {
104    int myswitch = b.get(0).asInt();
105    //std::cout<<"got some data with int "<<myswitch<<std::endl;
106
107    /// have here a statment that catchs 10's is the user has specified it
108    if( DoIWantModules )
109    {
110        switch (myswitch)
111        {
112        case ResetModulesListCode:
113            {
114                int FF;
115                ModuleStruct TempStruct;
116                printf("got new modules \n");
117                ListOfKnownModules.clear();
118                FF = 1;
119                while(FF<=b.size())
120                {
121                    TempStruct.name        = b.get(FF).asString().c_str();FF++;
122                    TempStruct.catagory    = b.get(FF).asString().c_str();FF++;
123                    TempStruct.subcatagory = b.get(FF).asString().c_str();FF++;
124                    ListOfKnownModules.push_front(TempStruct);
125                }
126            }
127            break;
128        case ResetAvailablePlatformsCode:
129            ListOfKnownPlatforms.clear();
130            printf("got new platforms \n");
131            for(int hh =1; hh<b.size(); hh++)
132            {
133                ListOfKnownPlatforms.push_front(b.get(hh).asString().c_str());
134            }
135            break;
136        }
137    }
138
139    switch (myswitch)
140    {
141    case ModuleStateFullstopAll :
142        currentmode = StateFullstop;
143        //printf("mode changed to stop \n");
144        break; // get all modules to stop
145    case ModuleStateRunningAll :
146        currentmode = StateRunning;
147        //printf("mode changed to running \n");
148        break;  // get all modules to go again
149    case ModuleStateInfoCodeAll :
150                  printf("Sent some catagory data from a module \n");
151        Bottle& MyBottle = prepare();
152        MyBottle.clear();
153        MyBottle.addInt( ModuleInfoCode ); // code for gui
154        MyBottle.addString( MyName.c_str() );
155        MyBottle.addString( MyCatagory.c_str() );
156        MyBottle.addString( MySubCatagory.c_str() );
157        write();
158        break;
159    }
160
161    if (myswitch>ModuleStateInfoCodeAll)
162    {
163        string mestring = b.get( 1 ).asString().c_str();
164        //if(myswitch>2 && b.get(1).asString().c_str() == MyName.c_str()) // for personal commands
165        if( mestring.compare( MyName ) == 0)
166        {
167            //printf("got into secound statment to turn off modules \n");
168            switch ( myswitch )
169            {
170            case ModuleStateFullstop :
171                currentmode = StateFullstop;
172                break; // get all modules to stop
173            case ModuleStateRunning :
174                currentmode = StateRunning;
175                break;  // get all modules to go again
176            case ModuleStateInfoCode :
177                Bottle& MyBottle = prepare();
178                MyBottle.clear();
179                MyBottle.addInt( ModuleInfoCode ); // code for gui
180                MyBottle.addString( MyName.c_str() );
181                MyBottle.addString( MyCatagory.c_str() );
182                MyBottle.addString( MySubCatagory.c_str() );
183                write();
184                break;
185            }
186        }
187    }
188  }
189
190  /*! equality operator
191   */
192  bool operator==( ModuleStruct x, ModuleStruct y) {
193    if( x.name == y.name ) return true;
194    return false;
195  }
196
197  /*! grater than operator
198   */
199  bool operator>( ModuleStruct x, ModuleStruct y) {
200    if(x.name>y.name) return true;
201    return false;
202  }
203
204  /*!
205   * Setups a special port for images only
206   */
207  void SamgarModule::SetupImagePort( string outputname ) {
208    string Tempname = "/Port_" + MyName + "_" + outputname;
209    //Time::delay(1);
210    imagePortName = Tempname.c_str();
211    //YarpImagePort.open( Tempname.c_str() );
212  }
213
214  /*!
215   * sends a picture out in the yarp native format
216   */
217  void SamgarModule::SendPictureYarpNative( ImageOf<PixelBgr> Image ) {
218    YarpImagePort.prepare() = Image; // no longer likes this line
219    YarpImagePort.write();
220  }
221
222  /*!
223   * Recives a picture in the yarp native format
224   */
225  ImageOf<PixelBgr> SamgarModule::RecivePictureYarpNative( void ) {
226    ImageOf<PixelBgr> TempImage = *YarpImagePort.read();
227    return TempImage;
228  }
229
230  /*!
231   * Sends a picture in the OpenCV native format IplImage
232   */
233  void SamgarModule::SendPictureOCVNative( IplImage* Image ) {
234    ImageOf<PixelBgr> yarpReturnImage;
235    yarpReturnImage.wrapIplImage( Image );
236    SendPictureYarpNative( yarpReturnImage );
237  }
238
239  /*!
240   * Revices a picture in the OpenCV native formate IplImage
241   */
242  IplImage* SamgarModule::RecivePictureOCVNative( void ) {
243    if( YarpImagePort.getInputCount() > 0 ) {
244      ImageOf<PixelBgr> *img = YarpImagePort.read();
245      IplImage *frame2 = (IplImage*) img->getIplImage();
246      return frame2;
247    }
248    return false;
249  }
250
251  /*!
252   * Very important function, allows all modules to sleep/start from
253   * send commands. Also if the module has been designated as a interupt
254   * module then it will send back false for failure also can have
255   * additional data on how well the module has done
256   */
257  void SamgarModule::SucceedFail( bool Awns, double additionaldata ) {
258    if( modulemode == ModeInterupt ) {// only send it when you got
259                                      // worthwhile data
260      Bottle &MyBottle = prepare();
261      MyBottle.clear();
262      MyBottle.addInt( ActivationCode ); // so it knows its a log report
263      MyBottle.addString(MyName.c_str());
264      MyBottle.addDouble(additionaldata);
265      if(getOutputCount()>0)
266        write();
267      currentmode = StateStoped;
268    }
269    long i;
270    while( currentmode != StateRunning)
271    {
272      yarp::os::Time::delay(0.01);
273      i++;
274      if (i%100==0)
275      {
276        checkConnection();
277        //return;
278      }
279    }
280    checkConnection();
281  }
282
283  /*!
284   * Sends a message for the log in the gui
285   */
286  void SamgarModule::SendToLog( string LogData, int priority ) {
287    Bottle &MyBottle = prepare();
288    MyBottle.clear();
289    MyBottle.addInt( LogReportCode );
290    MyBottle.addString(MyName.c_str());
291    MyBottle.addString(LogData.c_str());
292    MyBottle.addInt(priority);
293    write();
294  }
295
296  /*!
297   * Just adds a port to the module
298   */
299  void SamgarModule::AddPortS( string outputname ) {
300    string Tempname = "/Port_" + MyName + "_" + outputname;
301    portNameList.push_front(Tempname);
302  }
303
304  /*!
305   *  simpler function calls to the bigger functions
306   */
307  void SamgarModule::SendBottleData(string port,Bottle data){
308    SendData(TypeBottle,port,(void*)&data);
309  }
310
311  /*!
312   *  simpler function calls to the bigger functions
313   */
314  void SamgarModule::SendIntData(string port,int data){
315    SendData(TypeInt,port,(void*)&data);
316  }
317
318  /*!
319   * simpler function calls to the bigger functions
320   */
321  void SamgarModule::SendDoubleData(string port,double data){
322    SendData(TypeDouble,port,(void*)&data);
323  }
324
325  /*!
326   * simpler function calls to the bigger functions
327   */
328  void SamgarModule::SendStringData(string port,string data){
329    SendData(TypeString,port,(void*)&data);
330  }
331
332  /*!
333   * private function, DO NOT CALL DIRECTLY
334   */
335  void SamgarModule::SendData(DataType type,string port,void* data) {
336    DataPort* MyTempPort;
337    list<DataPort*>::iterator ItPort;
338    string TempName;
339    bool FoundPort;
340
341    TempName="/Port_" + MyName + "_" + port.c_str();
342
343    FoundPort=false;
344    for ( ItPort=PortList.begin() ; ItPort != PortList.end(); ItPort++ ) {
345      MyTempPort=*ItPort;
346      if(MyTempPort->getName() == TempName.c_str()) {
347        FoundPort=true;
348        break; // we have the port so break the loop
349      }
350    }
351    if(FoundPort==true) {
352      Bottle& MyBottle = MyTempPort-> prepare();
353      MyBottle.clear();
354      switch (type){
355      case TypeInt:    MyBottle.addInt( *((int*) data) ); break;
356      case TypeDouble: MyBottle.addDouble( *((double*) data) ); break;
357      case TypeString: MyBottle.addString( ((string*) data)->c_str()); break;
358      case TypeBottle: MyBottle = *((Bottle*)data); break;
359      }
360      MyTempPort->write();
361    }
362  }
363
364
365
366  //These are just window dressing to make it easer for the user, instead of using GetDataFromPort
367
368  /*!
369   * Gets int data from port, you give it the int you want changed and
370   * it changes it, it also replys with weather the port has been
371   * updated True/False
372   */
373  bool SamgarModule::GetIntData(string NameOfPort,int *I){
374    return GetDataFromPort(NameOfPort,TypeInt,I);
375  }
376
377  /*!
378   * Gets double data from port, you give it the int you want changed
379   * and it changes it, it also replys with weather the port has been
380   * updated True/False
381   */
382  bool SamgarModule::GetDoubleData(string NameOfPort,double *I){
383    return GetDataFromPort(NameOfPort,TypeDouble,I);
384  }
385
386  /*!
387   * Gets string data from port, you give it the int you want changed
388   * and it changes it, it also replys with weather the port has been
389   * updated True/False
390   */
391  bool SamgarModule::GetStringData(string NameOfPort,string *I){
392    return GetDataFromPort(NameOfPort,TypeString,I);
393  }
394
395  /*!
396   * Gets Bottle data from port, you give it the int you want changed
397   * and it changes it, it also replys with weather the port has been
398   * updated True/False
399   */
400  bool SamgarModule::GetBottleData(string NameOfPort,Bottle *I){
401    return GetDataFromPort(NameOfPort,TypeBottle,I);
402  }
403
404  /*!
405   * DO NOT CALL DIRECTLY
406   */
407  bool SamgarModule::GetDataFromPort(string NameOfPort,int TypeOfData, void* data) {
408    list<DataPort*>::iterator ItPort;
409    DataPort *MyTempPort;
410    bool HaveIfoundPort;
411 
412    NameOfPort = "/Port_" + MyName + "_" + NameOfPort.c_str();
413    HaveIfoundPort = false;
414
415    for ( ItPort=PortList.begin(); ItPort != PortList.end(); ItPort++ ) {// find the right port loop
416      MyTempPort=*ItPort;
417      if(MyTempPort->getName() == NameOfPort.c_str()){
418        HaveIfoundPort = true;
419        break;
420      }// have the data we need so break the loop
421    }
422
423    if ( !HaveIfoundPort )
424        return false;
425
426    if (MyTempPort->isClosed())
427        return false;
428
429    if(MyTempPort->getInputCount() == 0)
430      return false; // if nothings connected to it     
431
432    // gets Bottle from  the DataPort storage
433    Bottle MyBottle=MyTempPort->getBottle();
434
435      if(MyBottle.isNull())
436        return false;
437   
438      if (TypeOfData == TypeInt && MyBottle.get(0).isInt() != false) {
439        *((int*)data) = MyBottle.get(0).asInt();
440        return true;
441      }
442      else if (TypeOfData == TypeString && MyBottle.get(0).isString() != false) {
443        *((string*)data) = MyBottle.get(0).asString();
444        return true;
445      }
446      else if(TypeOfData == TypeDouble && MyBottle.get(0).isDouble() != false) {
447        *((double*)data) = MyBottle.get(0).asDouble();
448        return true;
449      }
450      else if(TypeOfData == TypeBottle) {
451        *((Bottle*)data) = MyBottle;
452        return true;
453      }
454    return false;
455  }
456
457  bool YARPexists(std::string portName)
458  {
459      static FILE *inpipe;
460      char inbuf[200];
461
462      inpipe =  popen((std::string("yarp exists ")+portName+" ; echo $?").c_str(),"r");
463      if (!inpipe){ std::cout<<"PING FAILED"<<std::endl;}
464      else
465      {
466          if (fgets(inbuf, sizeof(inbuf), inpipe))
467            {
468              std::string mystring = inbuf;
469              if (inbuf[0]=='0')
470              {
471                  pclose(inpipe);
472                  return true;
473              }
474          }
475      }
476      pclose(inpipe);
477      return false;
478  }
479
480  bool SamgarModule::checkConnection()
481  {
482      ModuleState oldCurrentMode;
483      oldCurrentMode = currentmode;
484      if (YARPexists("/PortForModules"))
485      {
486          if (!yarp::os::Network::isConnected("/PortForModules",fullModuleName.c_str()) ||
487              !yarp::os::Network::isConnected(fullModuleName.c_str(),"/PortForModules"))
488          {
489              currentmode     = StateStoped;
490              // there is no connection with /PortForModules so we shut down all ports
491              disableCallback();
492              close();
493              //yarp::os::Network::unregisterName(fullModuleName.c_str());
494              list<DataPort*>::iterator ItPort;
495              for ( ItPort=PortList.begin(); ItPort != PortList.end(); ItPort++ )
496              {
497                  (*ItPort)->disableCallback();
498                  (*ItPort)->close();
499                  //yarp::os::Network::unregisterName((*ItPort)->getName());
500              }
501              if (!imagePortName.empty())
502              YarpImagePort.close();
503
504              //yarp::os::Time::delay(0.1);
505              while(Network::getNameServerName()=="/global"){;}
506              //if (this->where().getName() == fullModuleName.c_str())
507              //  open(this->where());
508              //else
509              while (!open(fullModuleName.c_str()))
510              {
511                  yarp::os::Network::unregisterName(fullModuleName.c_str());
512                  close();
513              }
514              this->useCallback();
515              //yarp::os::Time::delay(0.1);
516              Bottle& MyBottle = prepare();
517              MyBottle.addInt( ModuleInfoCode ); // code for gui
518              MyBottle.addString( MyName.c_str() );
519              MyBottle.addString( MyCatagory.c_str() );
520              MyBottle.addString( MySubCatagory.c_str() );
521              write();
522              //yarp::os::Time::delay(0.1);
523              PortList.clear();
524              for ( std::list<std::string>::iterator ItName=portNameList.begin(); ItName != portNameList.end(); ItName++ )
525              {
526                  PortList.push_front( new DataPort(&currentmode) );
527                  PortList.front()->open((*ItName).c_str());
528                  PortList.front()->useCallback();
529              }
530              if (!imagePortName.empty())
531              YarpImagePort.open( imagePortName.c_str() );
532              currentmode = oldCurrentMode;
533          }
534          //else
535          //    currentmode     = StateRunning;
536      }
537      return false;
538  }
539
540  ModuleState SamgarModule::getCurrentState()
541  {
542      return currentmode;
543  }
544
545} // namespace Samgar
Note: See TracBrowser for help on using the repository browser.