iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
258 #include <cstdio>
259 #include <cstdarg>
260 #include <mutex>
261 #include <sstream>
262 #include <string>
263 #include <map>
264 #include <deque>
265 
266 #include <yarp/os/all.h>
267 
268 using namespace std;
269 using namespace yarp::os;
270 
271 #define CMD_ADD createVocab32('a','d','d')
272 #define CMD_DEL createVocab32('d','e','l')
273 #define CMD_GET createVocab32('g','e','t')
274 #define CMD_SET createVocab32('s','e','t')
275 #define CMD_LOCK createVocab32('l','o','c','k')
276 #define CMD_UNLOCK createVocab32('u','n','l','o')
277 #define CMD_OWNER createVocab32('o','w','n','e')
278 #define CMD_TIME createVocab32('t','i','m','e')
279 #define CMD_DUMP createVocab32('d','u','m','p')
280 #define CMD_ASK createVocab32('a','s','k')
281 #define CMD_SYNC createVocab32('s','y','n','c')
282 #define CMD_ASYNC createVocab32('a','s','y','n')
283 #define CMD_QUIT createVocab32('q','u','i','t')
284 #define CMD_BYE createVocab32('b','y','e')
285 
286 #define REP_ACK createVocab32('a','c','k')
287 #define REP_NACK createVocab32('n','a','c','k')
288 #define REP_UNKNOWN createVocab32('u','n','k','n')
289 
290 #define OPT_ALL createVocab32('a','l','l')
291 #define OPT_DISABLED (-1.0)
292 #define OPT_OWNERSHIP_ALL ("all")
293 
294 #define PROP_ID ("id")
295 #define PROP_LIFETIMER ("lifeTimer")
296 #define PROP_SET ("propSet")
297 #define BCTAG_EMPTY ("empty")
298 #define BCTAG_SYNC ("sync")
299 #define BCTAG_ASYNC ("async")
300 
301 
303 {
304 
305 /************************************************************************/
306 bool alwaysTrue(Value &a, Value &b)
307 {
308  return true;
309 }
310 
311 
312 /************************************************************************/
313 bool greater(Value &a, Value &b)
314 {
315  if (a.isFloat64() && b.isFloat64())
316  return (a.asFloat64()>b.asFloat64());
317  else if (a.isInt32() && b.isInt32())
318  return (a.asInt32()>b.asInt32());
319  else
320  return false;
321 }
322 
323 
324 /************************************************************************/
325 bool greaterEqual(Value &a, Value &b)
326 {
327  if (a.isFloat64() && b.isFloat64())
328  return (a.asFloat64()>=b.asFloat64());
329  else if (a.isInt32() && b.isInt32())
330  return (a.asInt32()>=b.asInt32());
331  else
332  return false;
333 }
334 
335 
336 /************************************************************************/
337 bool lower(Value &a, Value &b)
338 {
339  if (a.isFloat64() && b.isFloat64())
340  return (a.asFloat64()<b.asFloat64());
341  else if (a.isInt32() && b.isInt32())
342  return (a.asInt32()<b.asInt32());
343  else
344  return false;
345 }
346 
347 
348 /************************************************************************/
349 bool lowerEqual(Value &a, Value &b)
350 {
351  if (a.isFloat64() && b.isFloat64())
352  return (a.asFloat64()<=b.asFloat64());
353  else if (a.isInt32() && b.isInt32())
354  return (a.asInt32()<=b.asInt32());
355  else
356  return false;
357 }
358 
359 
360 /************************************************************************/
361 bool equal(Value &a, Value &b)
362 {
363  if (a.isFloat64() && b.isFloat64())
364  return (a.asFloat64()==b.asFloat64());
365  else if (a.isInt32() && b.isInt32())
366  return (a.asInt32()==b.asInt32());
367  else if (a.isString() && b.isString())
368  {
369  string aStr=a.asString();
370  string bStr=b.asString();
371  return (aStr==bStr);
372  }
373  else
374  return false;
375 }
376 
377 
378 /************************************************************************/
379 bool notEqual(Value &a, Value &b)
380 {
381  if (a.isFloat64() && b.isFloat64())
382  return (a.asFloat64()!=b.asFloat64());
383  else if (a.isInt32() && b.isInt32())
384  return (a.asInt32()!=b.asInt32());
385  else if (a.isString() && b.isString())
386  {
387  string aStr=a.asString();
388  string bStr=b.asString();
389  return (aStr!=bStr);
390  }
391  else
392  return false;
393 }
394 
395 }
396 
397 
398 /************************************************************************/
399 class DataBase : public PeriodicThread
400 {
401 protected:
402  /************************************************************************/
403  struct Item
404  {
405  Property *prop;
406  double lastUpdate;
407  string owner;
408 
409  Item() : prop(NULL),
410  lastUpdate(OPT_DISABLED),
411  owner(OPT_OWNERSHIP_ALL) { }
412  };
413 
414  /************************************************************************/
415  struct Condition
416  {
417  string prop;
418  bool (*compare)(Value&,Value&);
419  Value val;
420  };
421 
422  ResourceFinder *rf;
423  map<int,Item> itemsMap;
424  mutex mtx;
425  int idCnt;
427  bool nosavedb;
428  bool quitting;
429 
430  BufferedPort<Bottle> *pBroadcastPort;
432 
433  /************************************************************************/
434  void clear()
435  {
436  for (map<int,Item>::iterator it=itemsMap.begin(); it!=itemsMap.end(); it++)
437  delete it->second.prop;
438 
439  itemsMap.clear();
440  }
441 
442  /************************************************************************/
443  void eraseItem(map<int,Item>::iterator &it)
444  {
445  delete it->second.prop;
446  itemsMap.erase(it);
447  }
448 
449  /************************************************************************/
450  void write(FILE *stream)
451  {
452  int i=0;
453  for (map<int,Item>::iterator it=itemsMap.begin(); it!=itemsMap.end(); it++)
454  fprintf(stream,"item_%d (%s %d) (%s)\n",
455  i++,PROP_ID,it->first,it->second.prop->toString().c_str());
456  }
457 
458  /************************************************************************/
459  bool recursiveCheck(Property *item, deque<Condition> &condList,
460  deque<string> &opList, const unsigned int i=0)
461  {
462  bool result;
463  if (item->check(condList[i].prop))
464  {
465  // take the current value of the item's property under test
466  Value &val=item->find(condList[i].prop);
467 
468  // compute the condition over the current value
469  result=(*condList[i].compare)(val,condList[i].val);
470  }
471  else
472  result=false;
473 
474  if ((i+1)>=condList.size())
475  return result;
476  else if (opList[i]=="||")
477  return (result||recursiveCheck(item,condList,opList,i+1));
478  else // at this point we know we deal with "&&" operations
479  {
480  unsigned int j;
481  for (j=i+1; j<condList.size(); j++)
482  {
483  if (result)
484  {
485  if (item->check(condList[j].prop))
486  {
487  Value &val=item->find(condList[j].prop);
488  result=result&&(*condList[j].compare)(val,condList[j].val);
489  }
490  else
491  result=false;
492  }
493 
494  if (j<opList.size())
495  if (opList[j]=="||")
496  break;
497  }
498 
499  if (j>=condList.size())
500  return result;
501  else
502  return (result||recursiveCheck(item,condList,opList,j+1));
503  }
504  }
505 
506  /************************************************************************/
507  void run()
508  {
509  broadcast(BCTAG_SYNC);
510  }
511 
512 public:
513  /************************************************************************/
514  DataBase() : PeriodicThread(1.0)
515  {
516  pBroadcastPort=NULL;
517  asyncBroadcast=false;
518  initialized=false;
519  nosavedb=false;
520  quitting=false;
521  idCnt=0;
522  }
523 
524  /************************************************************************/
526  {
527  if (isRunning())
528  stop();
529 
530  save();
531  clear();
532  }
533 
534  /************************************************************************/
535  void configure(ResourceFinder &rf)
536  {
537  this->rf=&rf;
538  if (initialized)
539  {
540  yWarning("database already initialized ...");
541  return;
542  }
543 
544  nosavedb=rf.check("no-save-db");
545  if (!rf.check("no-load-db"))
546  load();
547 
548  dump();
549  initialized=true;
550  yInfo("database ready ...");
551 
552  if (rf.check("sync-bc"))
553  {
554  setPeriod(rf.check("sync-bc",Value(1.0)).asFloat64());
555  start();
556  }
557 
558  asyncBroadcast=rf.check("async-bc");
559  }
560 
561  /************************************************************************/
562  void setBroadcastPort(BufferedPort<Bottle> &broadcastPort)
563  {
564  pBroadcastPort=&broadcastPort;
565  }
566 
567  /************************************************************************/
568  void load()
569  {
570  string dbFileName=rf->findFile("db");
571  if (dbFileName.empty())
572  {
573  yWarning("requested database to be loaded not found!");
574  return;
575  }
576 
577  yInfo("loading database from %s ...",dbFileName.c_str());
578 
579  lock_guard<mutex> lck(mtx);
580  clear();
581  idCnt=0;
582 
583  Property finProperty;
584  finProperty.fromConfigFile(dbFileName);
585 
586  Bottle finBottle; finBottle.read(finProperty);
587  for (int i=0; i<finBottle.size(); i++)
588  {
589  ostringstream tag;
590  tag<<"item_"<<i;
591  Bottle &b1=finBottle.findGroup(tag.str());
592 
593  if (b1.isNull())
594  continue;
595 
596  if (b1.size()<3)
597  {
598  yWarning("error while loading %s!",tag.str().c_str());
599  continue;
600  }
601 
602  Bottle *b2=b1.get(1).asList();
603  Bottle *b3=b1.get(2).asList();
604  if ((b2==NULL) || (b3==NULL))
605  {
606  yWarning("error while loading %s!",tag.str().c_str());
607  continue;
608  }
609 
610  if (b2->size()<2)
611  {
612  yWarning("error while loading %s!",tag.str().c_str());
613  continue;
614  }
615 
616  int id=b2->get(1).asInt32();
617  itemsMap[id].prop=new Property(b3->toString().c_str());
618 
619  if (idCnt<=id)
620  idCnt=id+1;
621  }
622 
623  yInfo("database loaded");
624  }
625 
626  /************************************************************************/
627  void save()
628  {
629  if (nosavedb)
630  return;
631 
632  lock_guard<mutex> lck(mtx);
633  string dbFileName=rf->getHomeContextPath();
634  dbFileName+="/";
635  dbFileName+=rf->find("db").asString();
636  yInfo("saving database in %s ...",dbFileName.c_str());
637 
638  FILE *fout=fopen(dbFileName.c_str(),"w");
639  write(fout);
640  fclose(fout);
641 
642  yInfo("database stored");
643  }
644 
645  /************************************************************************/
646  void dump()
647  {
648  lock_guard<mutex> lck(mtx);
649  yInfo("dumping database content ...");
650 
651  if (itemsMap.size()==0)
652  yInfo("empty");
653  else
654  write(stdout);
655  }
656 
657  /************************************************************************/
658  void broadcast(const string &type)
659  {
660  if (pBroadcastPort!=NULL)
661  {
662  if (pBroadcastPort->getOutputCount()>0)
663  {
664  lock_guard<mutex> lck(mtx);
665  Bottle &bottle=pBroadcastPort->prepare();
666  bottle.clear();
667 
668  bottle.addString(type);
669  if (itemsMap.empty())
670  bottle.addString(BCTAG_EMPTY);
671  else for (map<int,Item>::iterator it=itemsMap.begin(); it!=itemsMap.end(); it++)
672  {
673  Bottle &item=bottle.addList();
674  item.read(*it->second.prop);
675 
676  Bottle &idList=item.addList();
677  idList.addString(PROP_ID);
678  idList.addInt32(it->first);
679  }
680 
681  pBroadcastPort->writeStrict();
682  }
683  }
684  }
685 
686  /************************************************************************/
687  bool add(Bottle *content)
688  {
689  if (content==NULL)
690  return false;
691 
692  if (content->check(PROP_ID))
693  {
694  yWarning("%s field cannot be specified as a property!",PROP_ID);
695  return false;
696  }
697 
698  lock_guard<mutex> lck(mtx);
699  itemsMap[idCnt].prop=new Property(content->toString().c_str());
700  itemsMap[idCnt].lastUpdate=Time::now();
701 
702  return true;
703  }
704 
705  /************************************************************************/
706  bool remove(Bottle *content)
707  {
708  if (content==NULL)
709  return false;
710 
711  if (content->size()==1)
712  {
713  if (content->get(0).isVocab32() || content->get(0).isString())
714  {
715  if (content->get(0).asVocab32()==OPT_ALL)
716  {
717  lock_guard<mutex> lck(mtx);
718  clear();
719  yInfo("database cleared");
720  return true;
721  }
722  }
723  }
724 
725  if (!content->check(PROP_ID))
726  {
727  yWarning("%s field not present within the request!",PROP_ID);
728  return false;
729  }
730 
731  int id=content->find(PROP_ID).asInt32();
732 
733  lock_guard<mutex> lck(mtx);
734  map<int,Item>::iterator it=itemsMap.find(id);
735  if (it!=itemsMap.end())
736  {
737  Bottle *propSet=content->find(PROP_SET).asList();
738  if (propSet!=NULL)
739  {
740  for (int i=0; i<propSet->size(); i++)
741  it->second.prop->unput(propSet->get(i).asString());
742 
743  it->second.lastUpdate=Time::now();
744  }
745  else
746  eraseItem(it);
747 
748  return true;
749  }
750 
751  return false;
752  }
753 
754  /************************************************************************/
755  bool get(Bottle *content, Bottle &response)
756  {
757  if (content==NULL)
758  return false;
759 
760  if (!content->check(PROP_ID))
761  {
762  yWarning("%s field not present within the request!",PROP_ID);
763  return false;
764  }
765 
766  int id=content->find(PROP_ID).asInt32();
767 
768  lock_guard<mutex> lck(mtx);
769  map<int,Item>::iterator it=itemsMap.find(id);
770  if (it!=itemsMap.end())
771  {
772  Property *pProp=it->second.prop;
773  response.clear();
774 
775  Bottle *propSet=content->find(PROP_SET).asList();
776  if (propSet!=NULL)
777  {
778  Property prop;
779  for (int i=0; i<propSet->size(); i++)
780  {
781  string propName=propSet->get(i).asString();
782  if (pProp->check(propName))
783  prop.put(propName,pProp->find(propName));
784  }
785 
786  response.read(prop);
787  }
788  else
789  response.read(*pProp);
790 
791  return true;
792  }
793 
794  return false;
795  }
796 
797  /************************************************************************/
798  bool set(Bottle *content, const string &agent)
799  {
800  if (content==NULL)
801  return false;
802 
803  if (!content->check(PROP_ID))
804  {
805  yWarning("%s field not present within the request!",PROP_ID);
806  return false;
807  }
808 
809  int id=content->find(PROP_ID).asInt32();
810 
811  lock_guard<mutex> lck(mtx);
812  map<int,Item>::iterator it=itemsMap.find(id);
813  if (it!=itemsMap.end())
814  {
815  string owner=it->second.owner;
816  if ((owner==OPT_OWNERSHIP_ALL) || (owner==agent))
817  {
818  Property *pProp=it->second.prop;
819  for (int i=0; i<content->size(); i++)
820  {
821  if (Bottle *option=content->get(i).asList())
822  {
823  if (option->size()<2)
824  continue;
825 
826  string prop=option->get(0).asString();
827  Value val=option->get(1);
828 
829  if (prop==PROP_ID)
830  continue;
831 
832  pProp->unput(prop);
833  pProp->put(prop,val);
834  }
835  else
836  continue;
837  }
838 
839  it->second.lastUpdate=Time::now();
840  return true;
841  }
842  }
843 
844  return false;
845  }
846 
847  /************************************************************************/
848  bool lock(Bottle *content, const string &agent)
849  {
850  if (content==NULL)
851  return false;
852 
853  if (!content->check(PROP_ID))
854  {
855  yWarning("%s field not present within the request!",PROP_ID);
856  return false;
857  }
858 
859  int id=content->find(PROP_ID).asInt32();
860 
861  lock_guard<mutex> lck(mtx);
862  map<int,Item>::iterator it=itemsMap.find(id);
863  if (it!=itemsMap.end())
864  {
865  string owner=it->second.owner;
866  if ((owner==OPT_OWNERSHIP_ALL) || (owner==agent))
867  {
868  it->second.owner=agent;
869  return true;
870  }
871  }
872 
873  return false;
874  }
875 
876  /************************************************************************/
877  bool unlock(Bottle *content, const string &agent)
878  {
879  if (content==NULL)
880  return false;
881 
882  if (!content->check(PROP_ID))
883  {
884  yWarning("%s field not present within the request!",PROP_ID);
885  return false;
886  }
887 
888  int id=content->find(PROP_ID).asInt32();
889 
890  lock_guard<mutex> lck(mtx);
891  map<int,Item>::iterator it=itemsMap.find(id);
892  if (it!=itemsMap.end())
893  {
894  string owner=it->second.owner;
895  if ((owner==OPT_OWNERSHIP_ALL) || (owner==agent))
896  {
897  it->second.owner=OPT_OWNERSHIP_ALL;
898  return true;
899  }
900  }
901 
902  return false;
903  }
904 
905  /************************************************************************/
906  bool owner(Bottle *content, Bottle &response)
907  {
908  if (content==NULL)
909  return false;
910 
911  if (!content->check(PROP_ID))
912  {
913  yWarning("%s field not present within the request!",PROP_ID);
914  return false;
915  }
916 
917  int id=content->find(PROP_ID).asInt32();
918 
919  lock_guard<mutex> lck(mtx);
920  map<int,Item>::iterator it=itemsMap.find(id);
921  if (it!=itemsMap.end())
922  {
923  response.clear();
924  string &owner=it->second.owner;
925  response.addString(owner);
926  return true;
927  }
928 
929  return false;
930  }
931 
932  /************************************************************************/
933  bool time(Bottle *content, Bottle &response)
934  {
935  if (content==NULL)
936  return false;
937 
938  if (!content->check(PROP_ID))
939  {
940  yWarning("%s field not present within the request!",PROP_ID);
941  return false;
942  }
943 
944  int id=content->find(PROP_ID).asInt32();
945 
946  lock_guard<mutex> lck(mtx);
947  map<int,Item>::iterator it=itemsMap.find(id);
948  if (it!=itemsMap.end())
949  {
950  response.clear();
951  if (it->second.lastUpdate<0.0)
952  response.addFloat64(it->second.lastUpdate);
953  else
954  {
955  double dt=Time::now()-it->second.lastUpdate;
956  response.addFloat64(dt);
957  }
958  return true;
959  }
960 
961  return false;
962  }
963 
964  /************************************************************************/
965  bool ask(Bottle *content, Bottle &response)
966  {
967  if (content==NULL)
968  return false;
969 
970  lock_guard<mutex> lck(mtx);
971  if (content->size()==1)
972  {
973  if (content->get(0).isVocab32() || content->get(0).isString())
974  {
975  if (content->get(0).asVocab32()==OPT_ALL)
976  {
977  response.clear();
978 
979  for (map<int,Item>::iterator it=itemsMap.begin(); it!=itemsMap.end(); it++)
980  response.addInt32(it->first);
981 
982  return true;
983  }
984  }
985  }
986 
987  deque<Condition> condList;
988  deque<string> opList;
989 
990  // we cannot accept a conditions string ending with
991  // a boolean operator
992  if (!(content->size()&0x01))
993  {
994  yWarning("uncorrect conditions received!");
995  return false;
996  }
997 
998  // parse the received conditions and build the lists
999  for (int i=0; i<content->size(); i+=2)
1000  {
1001  if (Bottle *b=content->get(i).asList())
1002  {
1003  Condition condition;
1004  string operation;
1005 
1006  if (b->size()==1)
1007  {
1008  condition.prop=b->get(0).asString();
1010  }
1011  else if (b->size()>2)
1012  {
1013  condition.prop=b->get(0).asString();
1014  operation=b->get(1).asString();
1015  condition.val=b->get(2);
1016 
1017  if (operation==">")
1019  else if (operation==">=")
1021  else if (operation=="<")
1023  else if (operation=="<=")
1025  else if (operation=="==")
1027  else if (operation=="!=")
1029  else
1030  {
1031  yWarning("unknown relational operator '%s'!",operation.c_str());
1032  return false;
1033  }
1034  }
1035  else
1036  {
1037  yWarning("wrong condition given!");
1038  return false;
1039  }
1040 
1041  condList.push_back(condition);
1042 
1043  if ((i+1)<content->size())
1044  {
1045  operation=content->get(i+1).asString();
1046  if ((operation!="||") && (operation!="&&"))
1047  {
1048  yWarning("unknown boolean operator '%s'!",operation.c_str());
1049  return false;
1050  }
1051  else
1052  opList.push_back(operation);
1053  }
1054  }
1055  else
1056  {
1057  yWarning("wrong condition given!");
1058  return false;
1059  }
1060  }
1061 
1062  response.clear();
1063 
1064  // apply the conditions to each item
1065  for (map<int,Item>::iterator it=itemsMap.begin(); it!=itemsMap.end(); it++)
1066  {
1067  // do recursion and keep only the item that
1068  // satisfies the whole list of conditions
1069  if (recursiveCheck(it->second.prop,condList,opList))
1070  response.addInt32(it->first);
1071  }
1072 
1073  return true;
1074  }
1075 
1076  /************************************************************************/
1077  void periodicHandler(const double dt) // manage the items life-timers
1078  {
1079  mtx.lock();
1080  bool erased=false;
1081  for (map<int,Item>::iterator it=itemsMap.begin(); it!=itemsMap.end(); it++)
1082  {
1083  Property *pProp=it->second.prop;
1084  if (pProp->check(PROP_LIFETIMER))
1085  {
1086  double lifeTimer=pProp->find(PROP_LIFETIMER).asFloat64()-dt;
1087  if (lifeTimer<=0.0)
1088  {
1089  eraseItem(it);
1090  erased=true;
1091  break;
1092  }
1093  else
1094  {
1095  pProp->unput(PROP_LIFETIMER);
1096  pProp->put(PROP_LIFETIMER,lifeTimer);
1097  }
1098  }
1099  }
1100  mtx.unlock();
1101 
1102  if (asyncBroadcast && erased)
1103  broadcast(BCTAG_ASYNC);
1104  }
1105 
1106  /************************************************************************/
1107  bool isQuitting() const
1108  {
1109  return quitting;
1110  }
1111 
1112  /************************************************************************/
1113  void respond(ConnectionReader &connection, const Bottle &command, Bottle &reply)
1114  {
1115  string agent=connection.getRemoteContact().getName();
1116  if (command.size()<1)
1117  {
1118  reply.addVocab32(REP_NACK);
1119  return;
1120  }
1121 
1122  reply.clear();
1123  int cmd=command.get(0).asVocab32();
1124  switch(cmd)
1125  {
1126  //-----------------
1127  case CMD_ADD:
1128  {
1129  if (command.size()<2)
1130  {
1131  reply.addVocab32(REP_NACK);
1132  break;
1133  }
1134 
1135  Bottle *content=command.get(1).asList();
1136  if (add(content))
1137  {
1138  reply.addVocab32(REP_ACK);
1139  Bottle &b=reply.addList();
1140  b.addString(PROP_ID);
1141  b.addInt32(idCnt);
1142  idCnt++;
1143 
1144  if (asyncBroadcast)
1145  broadcast(BCTAG_ASYNC);
1146  }
1147  else
1148  reply.addVocab32(REP_NACK);
1149 
1150  break;
1151  }
1152 
1153  //-----------------
1154  case CMD_DEL:
1155  {
1156  if (command.size()<2)
1157  {
1158  reply.addVocab32(REP_NACK);
1159  break;
1160  }
1161 
1162  Bottle *content=command.get(1).asList();
1163  if (remove(content))
1164  {
1165  reply.addVocab32(REP_ACK);
1166  if (asyncBroadcast)
1167  broadcast(BCTAG_ASYNC);
1168  }
1169  else
1170  reply.addVocab32(REP_NACK);
1171 
1172  break;
1173  }
1174 
1175  //-----------------
1176  case CMD_GET:
1177  {
1178  if (command.size()<2)
1179  {
1180  reply.addVocab32(REP_NACK);
1181  break;
1182  }
1183 
1184  Bottle response;
1185  Bottle *content=command.get(1).asList();
1186  if (get(content,response))
1187  {
1188  reply.addVocab32(REP_ACK);
1189  reply.addList()=response;
1190  }
1191  else
1192  reply.addVocab32(REP_NACK);
1193 
1194  break;
1195  }
1196 
1197  //-----------------
1198  case CMD_SET:
1199  {
1200  if (command.size()<2)
1201  {
1202  reply.addVocab32(REP_NACK);
1203  break;
1204  }
1205 
1206  Bottle *content=command.get(1).asList();
1207  if (set(content,agent))
1208  {
1209  reply.addVocab32(REP_ACK);
1210  if (asyncBroadcast)
1211  broadcast(BCTAG_ASYNC);
1212  }
1213  else
1214  reply.addVocab32(REP_NACK);
1215 
1216  break;
1217  }
1218 
1219  //-----------------
1220  case CMD_LOCK:
1221  {
1222  if (command.size()<2)
1223  {
1224  reply.addVocab32(REP_NACK);
1225  break;
1226  }
1227 
1228  Bottle *content=command.get(1).asList();
1229  if (lock(content,agent))
1230  reply.addVocab32(REP_ACK);
1231  else
1232  reply.addVocab32(REP_NACK);
1233 
1234  break;
1235  }
1236 
1237  //-----------------
1238  case CMD_UNLOCK:
1239  {
1240  if (command.size()<2)
1241  {
1242  reply.addVocab32(REP_NACK);
1243  break;
1244  }
1245 
1246  Bottle *content=command.get(1).asList();
1247  if (unlock(content,agent))
1248  reply.addVocab32(REP_ACK);
1249  else
1250  reply.addVocab32(REP_NACK);
1251 
1252  break;
1253  }
1254 
1255  //-----------------
1256  case CMD_OWNER:
1257  {
1258  if (command.size()<2)
1259  {
1260  reply.addVocab32(REP_NACK);
1261  break;
1262  }
1263 
1264  Bottle response;
1265  Bottle *content=command.get(1).asList();
1266  if (owner(content,response))
1267  {
1268  reply.addVocab32(REP_ACK);
1269  reply.addList()=response;
1270  }
1271  else
1272  reply.addVocab32(REP_NACK);
1273 
1274  break;
1275  }
1276 
1277  //-----------------
1278  case CMD_TIME:
1279  {
1280  if (command.size()<2)
1281  {
1282  reply.addVocab32(REP_NACK);
1283  break;
1284  }
1285 
1286  Bottle response;
1287  Bottle *content=command.get(1).asList();
1288  if (time(content,response))
1289  {
1290  reply.addVocab32(REP_ACK);
1291  reply.addList()=response;
1292  }
1293  else
1294  reply.addVocab32(REP_NACK);
1295 
1296  break;
1297  }
1298 
1299  //-----------------
1300  case CMD_DUMP:
1301  {
1302  dump();
1303  reply.addVocab32(REP_ACK);
1304  break;
1305  }
1306 
1307  //-----------------
1308  case CMD_SYNC:
1309  {
1310  if (command.size()<2)
1311  {
1312  reply.addVocab32(REP_NACK);
1313  break;
1314  }
1315 
1316  int opt=command.get(1).asVocab32();
1317  if (opt==Vocab32::encode("start"))
1318  {
1319  if (command.size()>=3)
1320  setPeriod(command.get(2).asFloat64());
1321 
1322  if (!isRunning())
1323  start();
1324  else if (isSuspended())
1325  resume();
1326 
1327  reply.addVocab32(REP_ACK);
1328  }
1329  else if (opt==Vocab32::encode("stop"))
1330  {
1331  if (isRunning() && !isSuspended())
1332  suspend();
1333 
1334  reply.addVocab32(REP_ACK);
1335  }
1336  else
1337  reply.addVocab32(REP_NACK);
1338 
1339  break;
1340  }
1341 
1342  //-----------------
1343  case CMD_ASYNC:
1344  {
1345  if (command.size()<2)
1346  {
1347  reply.addVocab32(REP_NACK);
1348  break;
1349  }
1350 
1351  int opt=command.get(1).asVocab32();
1352  if (opt==Vocab32::encode("on"))
1353  {
1354  asyncBroadcast=true;
1355  reply.addVocab32(REP_ACK);
1356  }
1357  else if (opt==Vocab32::encode("off"))
1358  {
1359  asyncBroadcast=false;
1360  reply.addVocab32(REP_ACK);
1361  }
1362  else
1363  reply.addVocab32(REP_NACK);
1364 
1365  break;
1366  }
1367 
1368  //-----------------
1369  case CMD_ASK:
1370  {
1371  if (command.size()<2)
1372  {
1373  reply.addVocab32(REP_NACK);
1374  break;
1375  }
1376 
1377  Bottle response;
1378  Bottle *content=command.get(1).asList();
1379  if (ask(content,response))
1380  {
1381  reply.addVocab32(REP_ACK);
1382  Bottle &b=reply.addList();
1383  b.addString(PROP_ID);
1384  b.addList()=response;
1385  }
1386  else
1387  reply.addVocab32(REP_NACK);
1388 
1389  break;
1390  }
1391 
1392  //-----------------
1393  case CMD_QUIT:
1394  case CMD_BYE:
1395  {
1396  quitting=true;
1397  reply.addVocab32(REP_ACK);
1398  break;
1399  }
1400 
1401  //-----------------
1402  default:
1403  {
1404  yWarning("received unknown command!");
1405  reply.addVocab32(REP_UNKNOWN);
1406  }
1407  }
1408  }
1409 
1410  /************************************************************************/
1411  bool modify(const Bottle &content)
1412  {
1413  if (content.size()==0)
1414  return false;
1415 
1416  string type=content.get(0).asString();
1417  if ((type!=BCTAG_EMPTY) && (type!=BCTAG_SYNC) && (type!=BCTAG_ASYNC))
1418  return false;
1419 
1420  mtx.lock();
1421  clear();
1422 
1423  if (type!=BCTAG_EMPTY)
1424  {
1425  idCnt=0;
1426  for (int i=1; i<content.size(); i++)
1427  {
1428  if (Bottle *item=content.get(i).asList())
1429  {
1430  if (Bottle *idList=item->get(0).asList())
1431  {
1432  if (idList->size()==2)
1433  {
1434  if (idList->get(0).asString()==PROP_ID)
1435  {
1436  int id=idList->get(1).asInt32();
1437  itemsMap[id].prop=new Property(item->tail().toString().c_str());
1438 
1439  if (idCnt<=id)
1440  idCnt=id+1;
1441  }
1442  }
1443  }
1444  }
1445  }
1446  }
1447 
1448  mtx.unlock();
1449 
1450  if (asyncBroadcast)
1451  broadcast(BCTAG_ASYNC);
1452 
1453  return true;
1454  }
1455 };
1456 
1457 
1458 /************************************************************************/
1459 class RpcProcessor : public PortReader
1460 {
1461 protected:
1463  unsigned int nCalls;
1464  double cumTime;
1465 
1466  /************************************************************************/
1467  bool read(ConnectionReader &connection)
1468  {
1469  Bottle command;
1470  if (!command.read(connection) || (pDataBase==NULL))
1471  return false;
1472 
1473  Bottle reply;
1474  double t0=Time::now();
1475  pDataBase->respond(connection,command,reply);
1476  cumTime+=Time::now()-t0;
1477  nCalls++;
1478 
1479  if (ConnectionWriter *writer=connection.getWriter())
1480  reply.write(*writer);
1481 
1482  return true;
1483  }
1484 
1485 public:
1486  /************************************************************************/
1488  {
1489  pDataBase=NULL;
1490  nCalls=0;
1491  cumTime=0.0;
1492  }
1493 
1494  /************************************************************************/
1495  void setDataBase(DataBase &dataBase)
1496  {
1497  pDataBase=&dataBase;
1498  }
1499 
1500  /************************************************************************/
1501  void getStats(unsigned int &nCalls, double &cumTime) const
1502  {
1503  nCalls=this->nCalls;
1504  cumTime=this->cumTime;
1505  }
1506 };
1507 
1508 
1509 /************************************************************************/
1510 class DataBaseModifyPort : public BufferedPort<Bottle>
1511 {
1512 protected:
1514 
1515  /************************************************************************/
1516  void onRead(Bottle &content)
1517  {
1518  if (pDataBase!=NULL)
1519  pDataBase->modify(content);
1520  }
1521 
1522 public:
1523  /************************************************************************/
1524  DataBaseModifyPort() : pDataBase(NULL) { }
1525 
1526  /************************************************************************/
1527  void setDataBase(DataBase &dataBase)
1528  {
1529  pDataBase=&dataBase;
1530  }
1531 };
1532 
1533 
1534 /************************************************************************/
1535 class objectsPropertiesCollectorModule: public RFModule
1536 {
1537 private:
1538  DataBase dataBase;
1539  RpcProcessor rpcProcessor;
1540  DataBaseModifyPort modifyPort;
1541  RpcServer rpcPort;
1542  BufferedPort<Bottle> bcPort;
1543 
1544  int cnt;
1545  bool stats;
1546  unsigned int nCallsOld;
1547  double cumTimeOld;
1548 
1549 public:
1550  /************************************************************************/
1551  bool configure(ResourceFinder &rf)
1552  {
1553  dataBase.configure(rf);
1554  stats=rf.check("stats");
1555 
1556  string name=rf.check("name",Value("objectsPropertiesCollector")).asString();
1557  dataBase.setBroadcastPort(bcPort);
1558  rpcProcessor.setDataBase(dataBase);
1559  rpcPort.setReader(rpcProcessor);
1560  modifyPort.setDataBase(dataBase);
1561  modifyPort.useCallback();
1562  rpcPort.open("/"+name+"/rpc");
1563  bcPort.open("/"+name+"/broadcast:o");
1564  modifyPort.open("/"+name+"/modify:i");
1565 
1566  cnt=0;
1567  nCallsOld=0;
1568  cumTimeOld=0.0;
1569 
1570  return true;
1571  }
1572 
1573  /************************************************************************/
1574  bool close()
1575  {
1576  modifyPort.disableCallback();
1577  dataBase.stop();
1578 
1579  rpcPort.interrupt();
1580  bcPort.interrupt();
1581  modifyPort.interrupt();
1582 
1583  rpcPort.close();
1584  bcPort.close();
1585  modifyPort.close();
1586 
1587  return true;
1588  }
1589 
1590  /************************************************************************/
1592  {
1593  dataBase.periodicHandler(getPeriod());
1594 
1595  // back-up straightaway the database each 15 minutes
1596  if ((++cnt)*getPeriod()>(15.0*60.0))
1597  {
1598  dataBase.save();
1599  cnt=0;
1600  }
1601 
1602  if (stats)
1603  {
1604  unsigned int nCalls; double cumTime;
1605  rpcProcessor.getStats(nCalls,cumTime);
1606 
1607  unsigned int calls=nCalls-nCallsOld;
1608  double timeSpent=cumTime-cumTimeOld;
1609  yInfo("*** Statistics: received %d/%g [requests/s]; %g [ms/request] spent on average",
1610  calls,getPeriod(),timeSpent==0.0?0.0:(1e3*timeSpent)/(double)calls);
1611 
1612  nCallsOld=nCalls;
1613  cumTimeOld=cumTime;
1614  }
1615 
1616  return !dataBase.isQuitting();
1617  }
1618 
1619  /************************************************************************/
1620  double getPeriod()
1621  {
1622  return 1.0;
1623  }
1624 };
1625 
1626 
1627 /************************************************************************/
1628 int main(int argc, char *argv[])
1629 {
1630  Network yarp;
1631 
1632  ResourceFinder rf;
1633  rf.setDefaultContext("objectsPropertiesCollector");
1634  rf.setDefault("db","dataBase.ini");
1635  rf.configure(argc,argv);
1636 
1637  if (rf.check("help"))
1638  {
1639  printf("Options\n");
1640  printf("\t--name <name>: collector name (default: objectsPropertiesCollector)\n");
1641  printf("\t--db <fileName>: database file name to load at startup/save at shutdown (default: dataBase.ini)\n");
1642  printf("\t--context <context>: context to search for database file (default: objectsPropertiesCollector)\n");
1643  printf("\t--no-load-db : start an empty database\n");
1644  printf("\t--no-save-db : prevent from saving the content of database at shutdown\n");
1645  printf("\t--sync-bc <T>: broadcast the database content each T seconds\n");
1646  printf("\t--async-bc : broadcast the database content whenever a change occurs\n");
1647  printf("\t--stats : enable statistics printouts\n");
1648  printf("\n");
1649  return 0;
1650  }
1651 
1652  if (!yarp.checkNetwork())
1653  {
1654  yError("YARP server not available!");
1655  return 1;
1656  }
1657 
1659  return collector.runModule(rf);
1660 }
1661 
1662 
DataBaseModifyPort::setDataBase
void setDataBase(DataBase &dataBase)
Definition: main.cpp:1527
PROP_ID
#define PROP_ID
Definition: main.cpp:294
DataBaseModifyPort::DataBaseModifyPort
DataBaseModifyPort()
Definition: main.cpp:1524
REP_ACK
#define REP_ACK
Definition: main.cpp:286
DataBase::owner
bool owner(Bottle *content, Bottle &response)
Definition: main.cpp:906
CMD_GET
#define CMD_GET
Definition: main.cpp:273
DataBase::configure
void configure(ResourceFinder &rf)
Definition: main.cpp:535
CMD_DUMP
#define CMD_DUMP
Definition: main.cpp:279
DataBaseModifyPort
Definition: main.cpp:1510
DataBase::Item::owner
string owner
Definition: main.cpp:407
DataBase::load
void load()
Definition: main.cpp:568
DataBase::add
bool add(Bottle *content)
Definition: main.cpp:687
objectsPropertiesCollectorModule::updateModule
bool updateModule()
Definition: main.cpp:1591
OPT_OWNERSHIP_ALL
#define OPT_OWNERSHIP_ALL
Definition: main.cpp:292
CMD_UNLOCK
#define CMD_UNLOCK
Definition: main.cpp:276
RpcProcessor::RpcProcessor
RpcProcessor()
Definition: main.cpp:1487
DataBase::unlock
bool unlock(Bottle *content, const string &agent)
Definition: main.cpp:877
CMD_TIME
#define CMD_TIME
Definition: main.cpp:278
DataBaseModifyPort::pDataBase
DataBase * pDataBase
Definition: main.cpp:1513
cmd
cmd
Definition: dataTypes.h:30
REP_NACK
#define REP_NACK
Definition: main.cpp:287
DataBase::eraseItem
void eraseItem(map< int, Item >::iterator &it)
Definition: main.cpp:443
main
int main(int argc, char *argv[])
Definition: main.cpp:31
DataBase::~DataBase
~DataBase()
Definition: main.cpp:525
CMD_QUIT
#define CMD_QUIT
Definition: main.cpp:283
CMD_ADD
#define CMD_ADD
Definition: main.cpp:271
set
src functionEncoder cpp set(folder_header ${folder_header} include/iCub/ctrl/functionEncoder.h) include_directories($
Definition: CMakeLists.txt:37
DataBase::mtx
mutex mtx
Definition: main.cpp:424
DataBase::ask
bool ask(Bottle *content, Bottle &response)
Definition: main.cpp:965
DataBase::nosavedb
bool nosavedb
Definition: main.cpp:427
DataBase::itemsMap
map< int, Item > itemsMap
Definition: main.cpp:423
stop
static int stop
Definition: iCub_Sim.cpp:41
DataBase::DataBase
DataBase()
Definition: main.cpp:514
relationalOperators::lowerEqual
bool lowerEqual(Value &a, Value &b)
Definition: main.cpp:349
DataBase::respond
void respond(ConnectionReader &connection, const Bottle &command, Bottle &reply)
Definition: main.cpp:1113
objectsPropertiesCollectorModule::getPeriod
double getPeriod()
Definition: main.cpp:1620
DataBase::broadcast
void broadcast(const string &type)
Definition: main.cpp:658
strain::regulation::write
bool write(const std::string filename, const FullRegulation &reg)
relationalOperators::equal
bool equal(Value &a, Value &b)
Definition: main.cpp:361
objectsPropertiesCollectorModule::close
bool close()
Definition: main.cpp:1574
BCTAG_ASYNC
#define BCTAG_ASYNC
Definition: main.cpp:299
objectsPropertiesCollectorModule::configure
bool configure(ResourceFinder &rf)
Definition: main.cpp:1551
objectsPropertiesCollectorModule
Definition: main.cpp:1535
DataBase::Condition
Definition: main.cpp:415
RpcProcessor::setDataBase
void setDataBase(DataBase &dataBase)
Definition: main.cpp:1495
CMD_SET
#define CMD_SET
Definition: main.cpp:274
relationalOperators::lower
bool lower(Value &a, Value &b)
Definition: main.cpp:337
OPT_DISABLED
#define OPT_DISABLED
Definition: main.cpp:291
DataBase::initialized
bool initialized
Definition: main.cpp:426
relationalOperators::alwaysTrue
bool alwaysTrue(Value &a, Value &b)
Definition: main.cpp:306
PROP_SET
#define PROP_SET
Definition: main.cpp:296
DataBase
Definition: main.cpp:399
RpcProcessor
Definition: main.cpp:1459
DataBase::Item::prop
Property * prop
Definition: main.cpp:405
CMD_ASYNC
#define CMD_ASYNC
Definition: main.cpp:282
DataBase::asyncBroadcast
bool asyncBroadcast
Definition: main.cpp:431
DataBase::set
bool set(Bottle *content, const string &agent)
Definition: main.cpp:798
DataBase::rf
ResourceFinder * rf
Definition: main.cpp:422
DataBase::idCnt
int idCnt
Definition: main.cpp:425
DataBase::modify
bool modify(const Bottle &content)
Definition: main.cpp:1411
REP_UNKNOWN
#define REP_UNKNOWN
Definition: main.cpp:288
relationalOperators
Definition: main.cpp:302
DataBase::Item
Definition: main.cpp:403
DataBase::setBroadcastPort
void setBroadcastPort(BufferedPort< Bottle > &broadcastPort)
Definition: main.cpp:562
relationalOperators::notEqual
bool notEqual(Value &a, Value &b)
Definition: main.cpp:379
RpcProcessor::cumTime
double cumTime
Definition: main.cpp:1464
DataBase::Item::lastUpdate
double lastUpdate
Definition: main.cpp:406
strain::dsp::q15::add
Q15 add(const Q15 a, const Q15 b)
Definition: strain.cpp:1209
CMD_ASK
#define CMD_ASK
Definition: main.cpp:280
relationalOperators::greaterEqual
bool greaterEqual(Value &a, Value &b)
Definition: main.cpp:325
CMD_SYNC
#define CMD_SYNC
Definition: main.cpp:281
OPT_ALL
#define OPT_ALL
Definition: main.cpp:290
DataBase::recursiveCheck
bool recursiveCheck(Property *item, deque< Condition > &condList, deque< string > &opList, const unsigned int i=0)
Definition: main.cpp:459
BCTAG_EMPTY
#define BCTAG_EMPTY
Definition: main.cpp:297
DataBase::dump
void dump()
Definition: main.cpp:646
DataBase::Condition::compare
bool(* compare)(Value &, Value &)
Definition: main.cpp:418
DataBaseModifyPort::onRead
void onRead(Bottle &content)
Definition: main.cpp:1516
RpcProcessor::pDataBase
DataBase * pDataBase
Definition: main.cpp:1462
DataBase::periodicHandler
void periodicHandler(const double dt)
Definition: main.cpp:1077
DataBase::run
void run()
Definition: main.cpp:507
DataBase::lock
bool lock(Bottle *content, const string &agent)
Definition: main.cpp:848
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
DataBase::pBroadcastPort
BufferedPort< Bottle > * pBroadcastPort
Definition: main.cpp:430
CMD_BYE
#define CMD_BYE
Definition: main.cpp:284
BCTAG_SYNC
#define BCTAG_SYNC
Definition: main.cpp:298
RpcProcessor::getStats
void getStats(unsigned int &nCalls, double &cumTime) const
Definition: main.cpp:1501
DataBase::write
void write(FILE *stream)
Definition: main.cpp:450
scripting.argc
argc
Definition: scripting.py:184
RpcProcessor::nCalls
unsigned int nCalls
Definition: main.cpp:1463
CMD_LOCK
#define CMD_LOCK
Definition: main.cpp:275
fprintf
fprintf(fid,'\n')
CMD_OWNER
#define CMD_OWNER
Definition: main.cpp:277
CMD_DEL
#define CMD_DEL
Definition: main.cpp:272
DataBase::Item::Item
Item()
Definition: main.cpp:409
equal
axis equal
Definition: show_eyes_axes.m:39
DataBase::time
bool time(Bottle *content, Bottle &response)
Definition: main.cpp:933
RpcProcessor::read
bool read(ConnectionReader &connection)
Definition: main.cpp:1467
DataBase::get
bool get(Bottle *content, Bottle &response)
Definition: main.cpp:755
DataBase::save
void save()
Definition: main.cpp:627
DataBase::quitting
bool quitting
Definition: main.cpp:428
fclose
fclose(fileID)
DataBase::clear
void clear()
Definition: main.cpp:434
DataBase::Condition::prop
string prop
Definition: main.cpp:417
time
degrees time
Definition: sine.m:5
DataBase::Condition::val
Value val
Definition: main.cpp:419
DataBase::remove
bool remove(Bottle *content)
Definition: main.cpp:706
PROP_LIFETIMER
#define PROP_LIFETIMER
Definition: main.cpp:295
relationalOperators::greater
bool greater(Value &a, Value &b)
Definition: main.cpp:313
DataBase::isQuitting
bool isQuitting() const
Definition: main.cpp:1107