iCub-main
Loading...
Searching...
No Matches
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
268using namespace std;
269using 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/************************************************************************/
306bool alwaysTrue(Value &a, Value &b)
307{
308 return true;
309}
310
311
312/************************************************************************/
313bool 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/************************************************************************/
325bool 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/************************************************************************/
337bool 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/************************************************************************/
349bool 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/************************************************************************/
361bool 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/************************************************************************/
379bool 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/************************************************************************/
399class DataBase : public PeriodicThread
400{
401protected:
402 /************************************************************************/
403 struct Item
404 {
405 Property *prop;
407 string owner;
408
412 };
413
414 /************************************************************************/
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;
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 {
510 }
511
512public:
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)
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)
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)
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)
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)
1452
1453 return true;
1454 }
1455};
1456
1457
1458/************************************************************************/
1459class RpcProcessor : public PortReader
1460{
1461protected:
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
1485public:
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/************************************************************************/
1510class DataBaseModifyPort : public BufferedPort<Bottle>
1511{
1512protected:
1514
1515 /************************************************************************/
1516 void onRead(Bottle &content)
1517 {
1518 if (pDataBase!=NULL)
1519 pDataBase->modify(content);
1520 }
1521
1522public:
1523 /************************************************************************/
1525
1526 /************************************************************************/
1527 void setDataBase(DataBase &dataBase)
1528 {
1529 pDataBase=&dataBase;
1530 }
1531};
1532
1533
1534/************************************************************************/
1536{
1537private:
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
1549public:
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/************************************************************************/
1628int 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
void setDataBase(DataBase &dataBase)
Definition main.cpp:1527
void onRead(Bottle &content)
Definition main.cpp:1516
DataBase * pDataBase
Definition main.cpp:1513
BufferedPort< Bottle > * pBroadcastPort
Definition main.cpp:430
void run()
Definition main.cpp:507
map< int, Item > itemsMap
Definition main.cpp:423
bool owner(Bottle *content, Bottle &response)
Definition main.cpp:906
void broadcast(const string &type)
Definition main.cpp:658
bool add(Bottle *content)
Definition main.cpp:687
bool asyncBroadcast
Definition main.cpp:431
bool modify(const Bottle &content)
Definition main.cpp:1411
bool remove(Bottle *content)
Definition main.cpp:706
int idCnt
Definition main.cpp:425
void load()
Definition main.cpp:568
void respond(ConnectionReader &connection, const Bottle &command, Bottle &reply)
Definition main.cpp:1113
bool time(Bottle *content, Bottle &response)
Definition main.cpp:933
bool isQuitting() const
Definition main.cpp:1107
bool initialized
Definition main.cpp:426
bool get(Bottle *content, Bottle &response)
Definition main.cpp:755
bool recursiveCheck(Property *item, deque< Condition > &condList, deque< string > &opList, const unsigned int i=0)
Definition main.cpp:459
mutex mtx
Definition main.cpp:424
void write(FILE *stream)
Definition main.cpp:450
bool unlock(Bottle *content, const string &agent)
Definition main.cpp:877
void setBroadcastPort(BufferedPort< Bottle > &broadcastPort)
Definition main.cpp:562
void save()
Definition main.cpp:627
bool quitting
Definition main.cpp:428
bool set(Bottle *content, const string &agent)
Definition main.cpp:798
void periodicHandler(const double dt)
Definition main.cpp:1077
~DataBase()
Definition main.cpp:525
bool nosavedb
Definition main.cpp:427
DataBase()
Definition main.cpp:514
void dump()
Definition main.cpp:646
void configure(ResourceFinder &rf)
Definition main.cpp:535
void eraseItem(map< int, Item >::iterator &it)
Definition main.cpp:443
ResourceFinder * rf
Definition main.cpp:422
bool ask(Bottle *content, Bottle &response)
Definition main.cpp:965
void clear()
Definition main.cpp:434
bool lock(Bottle *content, const string &agent)
Definition main.cpp:848
DataBase * pDataBase
Definition main.cpp:1462
double cumTime
Definition main.cpp:1464
void getStats(unsigned int &nCalls, double &cumTime) const
Definition main.cpp:1501
bool read(ConnectionReader &connection)
Definition main.cpp:1467
unsigned int nCalls
Definition main.cpp:1463
void setDataBase(DataBase &dataBase)
Definition main.cpp:1495
bool configure(ResourceFinder &rf)
Definition main.cpp:1551
cmd
Definition dataTypes.h:30
#define CMD_QUIT
Definition main.cpp:283
#define BCTAG_SYNC
Definition main.cpp:298
#define CMD_SYNC
Definition main.cpp:281
#define BCTAG_EMPTY
Definition main.cpp:297
#define REP_UNKNOWN
Definition main.cpp:288
#define CMD_DEL
Definition main.cpp:272
#define OPT_ALL
Definition main.cpp:290
#define CMD_GET
Definition main.cpp:273
#define REP_ACK
Definition main.cpp:286
#define CMD_TIME
Definition main.cpp:278
#define PROP_SET
Definition main.cpp:296
#define OPT_DISABLED
Definition main.cpp:291
#define CMD_ADD
Definition main.cpp:271
#define CMD_OWNER
Definition main.cpp:277
#define BCTAG_ASYNC
Definition main.cpp:299
#define CMD_BYE
Definition main.cpp:284
#define PROP_ID
Definition main.cpp:294
#define CMD_DUMP
Definition main.cpp:279
#define CMD_UNLOCK
Definition main.cpp:276
#define CMD_LOCK
Definition main.cpp:275
#define CMD_ASK
Definition main.cpp:280
#define CMD_SET
Definition main.cpp:274
#define PROP_LIFETIMER
Definition main.cpp:295
#define CMD_ASYNC
Definition main.cpp:282
#define REP_NACK
Definition main.cpp:287
#define OPT_OWNERSHIP_ALL
Definition main.cpp:292
int main()
Definition main.cpp:67
bool equal(Value &a, Value &b)
Definition main.cpp:361
bool lower(Value &a, Value &b)
Definition main.cpp:337
bool greater(Value &a, Value &b)
Definition main.cpp:313
bool alwaysTrue(Value &a, Value &b)
Definition main.cpp:306
bool greaterEqual(Value &a, Value &b)
Definition main.cpp:325
bool notEqual(Value &a, Value &b)
Definition main.cpp:379
bool lowerEqual(Value &a, Value &b)
Definition main.cpp:349
Copyright (C) 2008 RobotCub Consortium.
fprintf(fid,'\n')
axis equal
fclose(fileID)
degrees time
Definition sine.m:5
bool(* compare)(Value &, Value &)
Definition main.cpp:418
double lastUpdate
Definition main.cpp:406
Property * prop
Definition main.cpp:405
string owner
Definition main.cpp:407