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: Carlo Ciliberto, Vadim Tikhanoff
4 * email: carlo.ciliberto@iit.it vadim.tikhanoff@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 
342 #include <yarp/os/all.h>
343 #include <yarp/sig/Vector.h>
344 
345 #include <string>
346 
347 #include <iCub/utils.h>
348 #include <iCub/MotorThread.h>
349 #include <iCub/VisuoThread.h>
350 
351 
352 #define ACK yarp::os::createVocab32('a','c','k')
353 #define NACK yarp::os::createVocab32('n','a','c','k')
354 
355 #define RPC_HELP yarp::os::createVocab32('h','e','l','p')
356 #define RPC_GET yarp::os::createVocab32('g','e','t')
357 #define RPC_GET_STATUS yarp::os::createVocab32('s','t','a','t')
358 #define RPC_IMPEDANCE yarp::os::createVocab32('i','m','p','e')
359 #define RPC_S2C_MODE yarp::os::createVocab32('m','o','d','e')
360 #define RPC_INTERRUPT yarp::os::createVocab32('i','n','t','e')
361 #define RPC_REINSTATE yarp::os::createVocab32('r','e','i','n')
362 #define RPC_WAVEING yarp::os::createVocab32('w','a','v','e')
363 #define RPC_ELBOW yarp::os::createVocab32('e','l','b','o')
364 #define RPC_EXECTIME yarp::os::createVocab32('t','i','m','e')
365 
366 #define CMD_IDLE yarp::os::createVocab32('i','d','l','e')
367 #define CMD_HOME yarp::os::createVocab32('h','o','m','e')
368 #define CMD_CALIBRATE yarp::os::createVocab32('c','a','l','i')
369 #define CMD_EXPLORE yarp::os::createVocab32('e','x','p','l')
370 
371 #define CMD_OBSERVE yarp::os::createVocab32('o','b','s','e')
372 #define CMD_DROP yarp::os::createVocab32('d','r','o','p')
373 
374 #define CMD_HOLD yarp::os::createVocab32('h','o','l','d')
375 
376 #define CMD_LEARN_MIL yarp::os::createVocab32('l','e','a','r')
377 
378 #define CMD_GET yarp::os::createVocab32('g','e','t')
379 #define CMD_TAKE yarp::os::createVocab32('t','a','k','e')
380 #define CMD_GRASP yarp::os::createVocab32('g','r','a','s')
381 #define CMD_TOUCH yarp::os::createVocab32('t','o','u','c')
382 #define CMD_PICK yarp::os::createVocab32('p','i','c','k')
383 #define CMD_PUSH yarp::os::createVocab32('p','u','s','h')
384 #define CMD_POINT yarp::os::createVocab32('p','o','i','n')
385 #define CMD_POINT_FAR yarp::os::createVocab32('p','f','a','r')
386 #define CMD_LOOK yarp::os::createVocab32('l','o','o','k')
387 #define CMD_TRACK yarp::os::createVocab32('t','r','a','c')
388 #define CMD_EXPECT yarp::os::createVocab32('e','x','p','e')
389 #define CMD_GIVE yarp::os::createVocab32('g','i','v','e')
390 #define CMD_HAND yarp::os::createVocab32('h','a','n','d')
391 #define CMD_GAZE yarp::os::createVocab32('r','e','l','e')
392 
393 //commands for tool
394 #define CMD_TAKE_TOOL yarp::os::createVocab32('t','a','t','o')
395 
396 #define CMD_ACTION_TEACH yarp::os::createVocab32('t','e','a','c')
397 #define CMD_ACTION_IMITATE yarp::os::createVocab32('i','m','i','t')
398 
399 //sub commands: get
400 #define GET_S2C yarp::os::createVocab32('s','2','c')
401 #define GET_TABLE yarp::os::createVocab32('t','a','b','l')
402 #define GET_HOLDING yarp::os::createVocab32('h','o','l','d')
403 #define GET_HAND yarp::os::createVocab32('h','a','n','d')
404 #define GET_IMAGE yarp::os::createVocab32('i','m','a','g')
405 #define GET_IDLE yarp::os::createVocab32('i','d','l','e')
406 
407 //sub commands: calib
408 #define CALIB_TABLE yarp::os::createVocab32('t','a','b','l')
409 #define CALIB_FINGERS yarp::os::createVocab32('f','i','n','g')
410 #define CALIB_KIN_OFFSET yarp::os::createVocab32('k','i','n','e')
411 
412 //sub commands: explore
413 #define EXPLORE_TORSO yarp::os::createVocab32('t','o','r','s')
414 #define EXPLORE_HAND yarp::os::createVocab32('h','a','n','d')
415 
416 
417 #define PORT_TAG_CMD 0
418 #define PORT_TAG_GET 1
419 
420 #ifdef WIN32
421  #pragma warning(disable:4996)
422 #endif
423 
424 using namespace std;
425 using namespace yarp::os;
426 using namespace yarp::sig;
427 using namespace yarp::dev;
428 using namespace yarp::math;
429 using namespace iCub::ctrl;
430 using namespace iCub::action;
431 
432 class ActionsRenderingEngine
433 {
434 protected:
435  MotorThread *motorThr;
436  VisuoThread *visuoThr;
437 
438  Initializer *initializer;
439 
440  bool interrupted;
441  volatile bool closing;
442  bool idle;
443 
444 
445  bool check(Bottle &bot, const string &name)
446  {
447  bool found=false;
448  for(int i=0; i<bot.size(); i++)
449  {
450  if(bot.get(i).asString()==name)
451  {
452  found=true;
453  break;
454  }
455  }
456 
457  return found;
458  }
459 
460 
461 
462 public:
463  ActionsRenderingEngine()
464  {}
465 
466  bool initialize(ResourceFinder &rf)
467  {
468  string name=rf.check("name",Value("actionsRenderingEngine")).asString();
469 
470  initializer=new Initializer(rf);
471 
472  motorThr=new MotorThread(rf,initializer);
473  visuoThr=new VisuoThread(rf,initializer);
474 
475  if(!motorThr->start() || !visuoThr->start())
476  {
477  close();
478  return false;
479  }
480 
481  interrupted=false;
482  closing=false;
483  idle=true;
484 
485  return true;
486  }
487 
488 
489  bool close()
490  {
491  if(motorThr!=NULL)
492  {
493  motorThr->reinstate();
494  motorThr->stop();
495  delete motorThr;
496  }
497 
498  if(visuoThr!=NULL)
499  {
500  visuoThr->reinstate();
501  visuoThr->stop();
502  delete visuoThr;
503  }
504 
505  initializer->close();
506 
507  delete initializer;
508 
509  return true;
510  }
511 
512  bool interrupt()
513  {
514  closing=true;
515 
516  initializer->interrupt();
517 
518  if(visuoThr!=NULL)
519  visuoThr->interrupt();
520 
521  if(motorThr!=NULL)
522  motorThr->interrupt();
523 
524  return true;
525  }
526 
527  bool respond(const Bottle &command, Bottle &reply)
528  {
529  if(command.size()==0)
530  {
531  reply.addString("No command received.");
532  return true;
533  }
534 
535  switch(command.get(0).asVocab32())
536  {
537  case RPC_HELP:
538  {
539  string rep="Commands:\n";
540  rep+="\"interrupt\"\t\t\tinterrupts all the actions untill a 'reinstate' command is issued\n";
541  rep+="\"reinstate\"\t\t\treinstates the system (usually called after an 'interrupt' command\n";
542  rep+="\"impedance on/off\"\\off\t\t\tset the impedance control on or off\n";
543  rep+="\"mode homography\"\\disparity\\network\t\t\tset the 'stereo to cartesian' mode";
544  rep+="\"status\"\t\t\tget the module current status\n";
545  reply.addString(rep);
546  break;
547  }
548 
549  case RPC_GET:
550  {
551  if(command.size()>1)
552  {
553  switch(command.get(1).asVocab32())
554  {
555  case RPC_GET_STATUS:
556  {
557  motorThr->getStatus(reply);
558  break;
559  }
560  }
561  }
562  break;
563  }
564 
565  //set impedance on or off
566  case RPC_IMPEDANCE:
567  {
568  if(command.get(1).asString()=="on")
569  {
570  bool done=motorThr->setImpedance(true);
571  done?reply.addString("Impedance turned on"):reply.addString("Unable to set impedance on");
572  }
573  else if(command.get(1).asString()=="off")
574  {
575  motorThr->setImpedance(false);
576  reply.addString("Impedance turned off");
577  }
578  else
579  {
580  reply.addString("What do you want? Impedance on or off?");
581  }
582 
583  break;
584  }
585 
586  case RPC_WAVEING:
587  {
588  if(command.get(1).asString()=="on")
589  {
590  motorThr->setWaveing(true);
591  reply.addString("Waveing turned on");
592  }
593  else if(command.get(1).asString()=="off")
594  {
595  motorThr->setWaveing(false);
596  reply.addString("Waveing turned off");
597  }
598  else
599  {
600  reply.addString("What do you want? Waveing on or off?");
601  }
602 
603  break;
604  }
605 
606  //set the stereo2cartesian mode
607  case RPC_S2C_MODE:
608  {
609  motorThr->setStereoToCartesianMode(command.get(1).asVocab32(),reply);
610  break;
611  }
612 
613  case RPC_INTERRUPT:
614  {
615  interrupted=true;
616  motorThr->interrupt();
617  visuoThr->interrupt();
618  reply.addString("interrupted");
619  break;
620  }
621 
622  case RPC_REINSTATE:
623  {
624  interrupted=false;
625  motorThr->reinstate();
626  visuoThr->reinstate();
627  reply.addString("reinstated");
628  break;
629  }
630 
631  case RPC_ELBOW:
632  {
633  if (command.size()>=4)
634  {
635  string arm=command.get(1).asString();
636  double height=command.get(2).asFloat64();
637  double weight=command.get(3).asFloat64();
638 
639  if ((arm=="both") || (arm=="left"))
640  motorThr->changeElbowHeight(LEFT,height,weight);
641 
642  if ((arm=="both") || (arm=="right"))
643  motorThr->changeElbowHeight(RIGHT,height,weight);
644 
645  reply.addString("elbow parameters updated");
646  }
647  else
648  reply.addString("missing elbow parameters");
649 
650  break;
651  }
652 
653  case RPC_EXECTIME:
654  {
655  if (command.size()>1)
656  {
657  double execTime=command.get(1).asFloat64();
658  motorThr->changeExecTime(LEFT,execTime);
659  motorThr->changeExecTime(RIGHT,execTime);
660  reply.addString("execution time updated");
661  }
662  else
663  reply.addString("missing execution time");
664 
665  break;
666  }
667 
668  default:
669  {
670  reply.addString("command not recognized");
671  break;
672  }
673  }
674  return true;
675  }
676 
677  bool process(int &port_tag, Bottle &command, Bottle &reply)
678  {
679  if(closing)
680  {
681  reply.addVocab32(NACK);
682  reply.addString("Sorry. Module closing.");
683  return true;
684  }
685 
686  if(command.size()==0)
687  {
688  reply.addVocab32(NACK);
689  reply.addString("No command received.");
690  }
691  else if(interrupted)
692  {
693  reply.addVocab32(NACK);
694  reply.addString("Module currently interrupted. Reinstate for action.");
695  }
696  else
697  {
698  if(port_tag==PORT_TAG_GET)
699  {
700  if(command.get(0).asVocab32()!=CMD_GET)
701  {
702  reply.addVocab32(NACK);
703  reply.addString("This port can only process [get] commands");
704  }
705  else if(command.size()>1)
706  {
707  switch(command.get(1).asVocab32())
708  {
709  case GET_S2C:
710  {
711  if(command.size()>2)
712  {
713  for(int target_idx=2; target_idx<command.size(); target_idx++)
714  {
715  Vector xd;
716  Bottle tmp_command;
717  visuoThr->getTarget(command.get(target_idx),tmp_command);
718 
719  Bottle &tmp_reply=reply.addList();
720  if(motorThr->targetToCartesian(tmp_command.find("target").asList(),xd))
721  {
722  for(size_t i=0; i<xd.size(); i++)
723  tmp_reply.addFloat64(xd[i]);
724  }
725  else
726  tmp_reply.addVocab32(NACK);
727  }
728  }
729 
730  break;
731  }
732  case GET_IDLE:
733  {
734  reply.clear();
735  if(idle)
736  reply.addVocab32(ACK);
737  else
738  reply.addVocab32(NACK);
739 
740  break;
741  }
742 
743  case GET_TABLE:
744  {
745  reply.clear();
746  double table_height;
747  if(motorThr->getTableHeight(&table_height))
748  {
749  Bottle &tmp_reply=reply.addList();
750  tmp_reply.addString("table_height");
751  tmp_reply.addFloat64(table_height);
752  }
753  else
754  reply.addVocab32(NACK);
755 
756  break;
757  }
758 
759  case GET_HOLDING:
760  {
761  reply.clear();
762  if(motorThr->isHolding(command))
763  reply.addVocab32(ACK);
764  else
765  reply.addVocab32(NACK);
766 
767  break;
768  }
769 
770  case GET_HAND:
771  {
772  if(command.get(2).asVocab32()==GET_IMAGE)
773  {
774  reply.clear();
775  if(!motorThr->getHandImagePosition(reply))
776  reply.addVocab32(NACK);
777  }
778  else
779  reply.addVocab32(NACK);
780 
781  break;
782  }
783 
784 
785  default:
786  {
787  reply.addVocab32(NACK);
788  break;
789  }
790  }
791  }
792  else
793  reply.addVocab32(NACK);
794  }
795  else if(port_tag==PORT_TAG_CMD)
796  {
797  switch(command.get(0).asVocab32())
798  {
799  case CMD_IDLE:
800  {
801  motorThr->setGazeIdle();
802  reply.addVocab32(ACK);
803  break;
804  }
805 
806  case CMD_HOME:
807  {
808  motorThr->goHome(command);
809  reply.addVocab32(ACK);
810 
811  break;
812  }
813  case CMD_HAND:
814  {
815  bool holding;
816  if (motorThr->hand(command,"",&holding))
817  {
818  reply.addVocab32(ACK);
819  reply.addInt32(holding?1:0);
820  }
821  else
822  reply.addVocab32(NACK);
823 
824  break;
825  }
826  case CMD_GAZE:
827  {
828  motorThr->clearIt(command);
829  reply.addVocab32(ACK);
830 
831  break;
832  }
833 
834  //----------- for retro_compatibility -----------------
835  case CMD_GET:
836  {
837  if(command.size()>1)
838  {
839  switch(command.get(1).asVocab32())
840  {
841  case GET_S2C:
842  {
843  if(command.size()>2)
844  {
845  Vector xd;
846  visuoThr->getTarget(command.get(2),command);
847  if(motorThr->targetToCartesian(command.find("target").asList(),xd))
848  {
849  reply.clear();
850  for(size_t i=0; i<xd.size(); i++)
851  reply.addFloat64(xd[i]);
852  }
853  else
854  reply.addVocab32(NACK);
855  }
856 
857  break;
858  }
859  }
860 
861  break;
862  }
863 
864  break;
865  }
866  //-----------------------------------------------------
867 
868  case CMD_EXPLORE:
869  {
870  if(command.size()>1)
871  {
872  switch(command.get(1).asVocab32())
873  {
874  case EXPLORE_TORSO:
875  {
876  if(motorThr->exploreTorso(command))
877  reply.addVocab32(ACK);
878  else
879  reply.addVocab32(NACK);
880 
881  break;
882  }
883 
884  case EXPLORE_HAND:
885  {
886  if(motorThr->exploreHand(command))
887  reply.addVocab32(ACK);
888  else
889  reply.addVocab32(NACK);
890 
891  break;
892  }
893 
894  default:
895  {
896  string rep=command.get(1).asString();
897  reply.addVocab32(NACK);
898  reply.addString("parameter '"+rep+"' not supported by 'explore' command.");
899  break;
900  }
901  }
902  }
903  else
904  {
905  reply.addVocab32(NACK);
906  reply.addString("explore command needs further parameter (e.g. 'torso')");
907  }
908  break;
909  }
910 
911  case CMD_CALIBRATE:
912  {
913  switch(command.get(1).asVocab32())
914  {
915  case CALIB_TABLE:
916  {
917  idle = false;
918  if(motorThr->calibTable(command))
919  reply.addVocab32(ACK);
920  else
921  reply.addVocab32(NACK);
922  idle = true;
923  break;
924  }
925 
926  case CALIB_FINGERS:
927  {
928  if(motorThr->calibFingers(command))
929  reply.addVocab32(ACK);
930  else
931  reply.addVocab32(NACK);
932 
933  break;
934  }
935 
936  case CALIB_KIN_OFFSET:
937  {
938  if(command.get(2).asString()=="start")
939  {
940  if (command.size()>=4)
941  if (command.get(3).isList())
942  visuoThr->getTarget(command.get(3),command);
943  if (motorThr->startLearningModeKinOffset(command))
944  {
945  reply.addVocab32(ACK);
946  reply.addString("learn kinematic offset mode: on");
947  }
948  else
949  reply.addVocab32(NACK);
950  }
951  else if(command.get(2).asString()=="stop")
952  {
953  if(motorThr->suspendLearningModeKinOffset(command))
954  {
955  reply.addVocab32(ACK);
956  reply.addString("learn kinematic offset mode: off");
957  }
958  else
959  reply.addVocab32(NACK);
960  }
961 
962  break;
963  }
964  }
965 
966  break;
967  }
968 
969  //------ action "learning" ---------//
970  case CMD_ACTION_TEACH:
971  {
972  if(check(command,"start"))
973  {
974  string action_name=command.get(1).asString();
975 
976  Bottle &action=command.addList();
977  action.addString("action_name");
978  action.addString(action_name);
979 
980  if(!motorThr->startLearningModeAction(command))
981  {
982  reply.addVocab32(NACK);
983  reply.addString("action "+action_name+" already known");
984  }
985  else
986  {
987  motorThr->setGazeIdle();
988  motorThr->lookAtHand(command);
989  reply.addVocab32(ACK);
990  reply.addString("start teaching");
991  }
992  }
993 
994  if(check(command,"stop"))
995  {
996  if(motorThr->suspendLearningModeAction(command))
997  {
998  motorThr->setGazeIdle();
999  reply.addVocab32(ACK);
1000  reply.addString("stop teaching");
1001  }
1002  else
1003  reply.addVocab32(NACK);
1004  }
1005 
1006  break;
1007  }
1008 
1009  case CMD_ACTION_IMITATE:
1010  {
1011  string action_name=command.get(1).asString();
1012 
1013  Bottle &action=command.addList();
1014  action.addString("action_name");
1015  action.addString(action_name);
1016 
1017  motorThr->lookAtHand(command);
1018 
1019  if(!motorThr->imitateAction(command))
1020  {
1021  reply.addVocab32(NACK);
1022  reply.addString("action "+action_name+" unknown");
1023  }
1024  else
1025  {
1026  reply.addVocab32(ACK);
1027  reply.addString("action "+action_name+" done");
1028  }
1029  motorThr->setGazeIdle();
1030 
1031  break;
1032  }
1033  //-----------------------------------//
1034 
1035 
1036  case CMD_LEARN_MIL:
1037  {
1038  string obj_name=command.get(1).asString();
1039  if(motorThr->isHolding(command)) //
1040  motorThr->deploy(command);
1041 
1042  //if it is not currently tracking anything, start learning what it has in fixation
1043  Vector stereo;
1044  if(!visuoThr->isTracking())
1045  {
1046  Value v("fix");
1047  visuoThr->getTarget(v,command);
1048  }
1049 
1050  visuoThr->startLearningMIL(obj_name);
1051  motorThr->exploreTorso(command);
1052  visuoThr->trainMIL();
1053 
1054  reply.addString(obj_name + " learned");
1055  break;
1056  }
1057 
1058  case CMD_OBSERVE:
1059  {
1060  if(!motorThr->isHolding(command))
1061  {
1062  reply.addVocab32(NACK);
1063  reply.addString("Nothing to observe. Not holding anything");
1064  motorThr->release(command);
1065  motorThr->goHome(command);
1066  break;
1067  }
1068 
1069  motorThr->lookAtHand(command);
1070  motorThr->drawNear(command);
1071 
1072  reply.addVocab32(ACK);
1073 
1074  break;
1075  }
1076 
1077  case CMD_EXPECT:
1078  {
1079  if(!motorThr->expect(command))
1080  {
1081  motorThr->setGazeIdle();
1082  motorThr->release(command);
1083  motorThr->goHome(command);
1084  reply.addVocab32(NACK);
1085  break;
1086  }
1087 
1088  motorThr->grasp(command);
1089 
1090  if(motorThr->isHolding(command))
1091  {
1092  if (check(command,"near"))
1093  motorThr->drawNear(command);
1094  else
1095  {
1096  motorThr->setGazeIdle();
1097  Bottle b;
1098  b.addString("head");
1099  b.addString("arms");
1100  motorThr->goHome(b);
1101  }
1102 
1103  reply.addVocab32(ACK);
1104  }
1105  else
1106  {
1107  motorThr->setGazeIdle();
1108  motorThr->release(command);
1109  motorThr->goHome(command);
1110  reply.addVocab32(NACK);
1111  }
1112 
1113  break;
1114  }
1115 
1116  case CMD_GIVE:
1117  {
1118  if(!motorThr->isHolding(command))
1119  {
1120  reply.addVocab32(NACK);
1121  reply.addString("Nothing to give. Not holding anything");
1122  motorThr->release(command);
1123  motorThr->goHome(command);
1124  break;
1125  }
1126 
1127  motorThr->give(command);
1128 
1129  motorThr->setGazeIdle();
1130  motorThr->release(command);
1131  motorThr->goHome(command);
1132  reply.addVocab32(NACK);
1133 
1134  break;
1135  }
1136 
1137  case CMD_DROP:
1138  {
1139  idle = false;
1140  if(check(command,"over") && command.size()>2)
1141  visuoThr->getTarget(command.get(2),command);
1142 
1143  motorThr->setGazeIdle();
1144 
1145  motorThr->deploy(command);
1146  motorThr->keepFixation(command);
1147  motorThr->goUp(command);
1148 
1149  motorThr->goHome(command);
1150  motorThr->setGazeIdle();
1151 
1152  reply.addVocab32(ACK);
1153  idle = true;
1154  break;
1155  }
1156 
1157  case CMD_TAKE:
1158  {
1159  idle = false;
1160  if(command.size()<2)
1161  {
1162  reply.addVocab32(NACK);
1163  break;
1164  }
1165 
1166  visuoThr->getTarget(command.get(1),command);
1167 
1168  command.addString("pretake_hand");
1169  if(!motorThr->reach(command))
1170  {
1171  reply.addVocab32(NACK);
1172  break;
1173  }
1174 
1175  motorThr->lookAtHand(command);
1176  motorThr->shiftAndGrasp(command);
1177 
1178  if (motorThr->isHolding(command))
1179  {
1180  if (check(command,"near"))
1181  motorThr->drawNear(command);
1182  else
1183  {
1184  motorThr->goUp(command);
1185  motorThr->setGazeIdle();
1186 
1187  if (!check(command,"still"))
1188  {
1189  Bottle b;
1190  b.addString("head");
1191  b.addString("arms");
1192  motorThr->goHome(b);
1193  }
1194  }
1195 
1196  reply.addVocab32(ACK);
1197  }
1198  else
1199  {
1200  motorThr->setGazeIdle();
1201  motorThr->release(command);
1202 
1203  if (!check(command,"still"))
1204  {
1205  motorThr->goUp(command);
1206  motorThr->goHome(command);
1207  }
1208 
1209  reply.addVocab32(NACK);
1210  }
1211  idle = true;
1212 
1213  break;
1214  }
1215 
1216  case CMD_GRASP:
1217  {
1218  idle = false;
1219  if (command.size()<2)
1220  {
1221  reply.addVocab32(NACK);
1222  break;
1223  }
1224 
1225  visuoThr->getTarget(command.get(1),command);
1226  if (motorThr->powerGrasp(command))
1227  {
1228  motorThr->goUp(command);
1229  motorThr->setGazeIdle();
1230 
1231  if (!check(command,"still"))
1232  {
1233  Bottle b;
1234  b.addString("head");
1235  b.addString("arms");
1236  motorThr->goHome(b);
1237  }
1238 
1239  reply.addVocab32(ACK);
1240  }
1241  else
1242  {
1243  motorThr->setGazeIdle();
1244  motorThr->release(command);
1245  if (!check(command,"still"))
1246  motorThr->goHome(command);
1247 
1248  reply.addVocab32(NACK);
1249  }
1250 
1251  idle = true;
1252  break;
1253  }
1254 
1255  case CMD_TOUCH:
1256  {
1257  if(command.size()<2)
1258  {
1259  reply.addVocab32(NACK);
1260  break;
1261  }
1262 
1263  visuoThr->getTarget(command.get(1),command);
1264 
1265  command.addString("touch");
1266  if(!motorThr->reach(command))
1267  {
1268  reply.addVocab32(NACK);
1269  break;
1270  }
1271 
1272  if(!check(command,"still"))
1273  {
1274  Time::delay(2.0);
1275  motorThr->goUp(command);
1276  motorThr->goHome(command);
1277  }
1278 
1279  reply.addVocab32(ACK);
1280  break;
1281  }
1282 
1283  case CMD_PUSH:
1284  {
1285  if(command.size()<2)
1286  {
1287  reply.addVocab32(NACK);
1288  break;
1289  }
1290 
1291  visuoThr->getTarget(command.get(1),command);
1292 
1293  if (!motorThr->push(command))
1294  {
1295  reply.addVocab32(NACK);
1296  break;
1297  }
1298 
1299  if (!check(command,"still"))
1300  {
1301  motorThr->setGazeIdle();
1302  motorThr->goUp(command);
1303  motorThr->goHome(command);
1304  }
1305 
1306  reply.addVocab32(ACK);
1307  break;
1308  }
1309 
1310  case CMD_POINT:
1311  {
1312  if(command.size()<2)
1313  {
1314  reply.addVocab32(NACK);
1315  break;
1316  }
1317 
1318  visuoThr->getTarget(command.get(1),command);
1319 
1320  if(!motorThr->point(command))
1321  {
1322  reply.addVocab32(NACK);
1323  break;
1324  }
1325 
1326  if(!check(command,"still"))
1327  {
1328  Time::delay(2.0);
1329  motorThr->goHome(command);
1330  }
1331 
1332  reply.addVocab32(ACK);
1333  break;
1334  }
1335  case CMD_POINT_FAR:
1336  {
1337  if(command.size()<2)
1338  {
1339  reply.addVocab32(NACK);
1340  break;
1341  }
1342 
1343  visuoThr->getTarget(command.get(1),command);
1344 
1345  if(!motorThr->point_far(command))
1346  {
1347  reply.addVocab32(NACK);
1348  break;
1349  }
1350 
1351  if(!check(command,"still"))
1352  {
1353  Time::delay(2.0);
1354  motorThr->goHome(command);
1355  }
1356 
1357  reply.addVocab32(ACK);
1358  break;
1359  }
1360  case CMD_LOOK:
1361  {
1362  if(command.size()<2)
1363  {
1364  reply.addVocab32(NACK);
1365  break;
1366  }
1367 
1368  if(!check(command,"hand"))
1369  visuoThr->getTarget(command.get(1),command);
1370 
1371  if(!motorThr->look(command))
1372  {
1373  reply.addVocab32(NACK);
1374  break;
1375  }
1376 
1377  reply.addVocab32(ACK);
1378  break;
1379  }
1380 
1381  case CMD_TRACK:
1382  {
1383  if (command.get(1).asVocab32()!=Vocab32::encode("motion"))
1384  visuoThr->getTarget(command.get(1),command);
1385  else
1386  visuoThr->trackMotion();
1387 
1388  motorThr->track(command);
1389  reply.addVocab32(ACK);
1390  break;
1391  }
1392  case CMD_TAKE_TOOL:
1393  {
1394  if (command.size()<2)
1395  {
1396  reply.addVocab32(NACK);
1397  break;
1398  }
1399 
1400  motorThr->takeTool(command);
1401  reply.addVocab32(ACK);
1402 
1403  break;
1404  }
1405  default:
1406  {
1407  reply.addVocab32(NACK);
1408  break;
1409  }
1410  }
1411  }
1412 
1413  }
1414 
1415  if(reply.isNull() || reply.size()==0)
1416  {
1417  reply.addVocab32(NACK);
1418  reply.addString("Random Error");
1419  }
1420 
1421  return true;
1422  }
1423 
1424 
1425 };
1426 
1427 
1428 
1429 
1430 
1431 class ARE_PortReader: public PortReader
1432 {
1433 protected:
1434  ActionsRenderingEngine *are;
1435 
1436  int port_tag;
1437 
1438 
1439  bool read(ConnectionReader &connection)
1440  {
1441  Bottle command, reply;
1442  if (!command.read(connection))
1443  return false;
1444 
1445  //do stuff with the ARE here
1446  are->process(port_tag,command,reply);
1447  //-------------
1448 
1449  if (ConnectionWriter *writer=connection.getWriter())
1450  reply.write(*writer);
1451 
1452  return true;
1453  }
1454 
1455 public:
1456  ARE_PortReader(ActionsRenderingEngine *_are, int _port_tag)
1457  :are(_are),port_tag(_port_tag)
1458  {}
1459 
1460 };
1461 
1462 
1463 
1464 class ActionsRenderingEngineModule: public RFModule
1465 {
1466 protected:
1467  ActionsRenderingEngine *are;
1468 
1469  ARE_PortReader *port_reader_cmd;
1470  ARE_PortReader *port_reader_get;
1471 
1472  RpcServer port_cmd;
1473  RpcServer port_get;
1474  RpcServer port_rpc;
1475 
1476 
1477 public:
1478  ActionsRenderingEngineModule()
1479  {}
1480 
1481  virtual bool configure(ResourceFinder &rf)
1482  {
1483  string name=rf.check("name",Value("actionsRenderingEngine")).asString();
1484  setName(name.c_str());
1485 
1486  are=new ActionsRenderingEngine();
1487 
1488  if(!are->initialize(rf))
1489  {
1490  yError("ARE could not be initialized!");
1491  return false;
1492  }
1493 
1494  port_reader_cmd=new ARE_PortReader(are,PORT_TAG_CMD);
1495  port_reader_get=new ARE_PortReader(are,PORT_TAG_GET);
1496 
1497  port_cmd.setReader(*port_reader_cmd);
1498  port_get.setReader(*port_reader_get);
1499 
1500  port_cmd.open("/"+name+"/cmd:io");
1501  port_get.open("/"+name+"/get:io");
1502  port_rpc.open("/"+name+"/rpc");
1503 
1504  attach(port_rpc);
1505 
1506  return true;
1507  }
1508 
1509  virtual bool interruptModule()
1510  {
1511  port_cmd.interrupt();
1512  port_get.interrupt();
1513  port_rpc.interrupt();
1514 
1515  are->interrupt();
1516 
1517  return true;
1518  }
1519 
1520  virtual bool close()
1521  {
1522  port_cmd.close();
1523  port_get.close();
1524  port_rpc.close();
1525 
1526  are->close();
1527 
1528  delete port_reader_cmd;
1529  delete port_reader_get;
1530 
1531  delete are;
1532 
1533  return true;
1534  }
1535 
1536  virtual double getPeriod()
1537  {
1538  return 0.1;
1539  }
1540 
1541  virtual bool updateModule()
1542  {
1543  return true;
1544  }
1545 
1546  virtual bool respond(const Bottle &command, Bottle &reply)
1547  {
1548  if(are->respond(command,reply))
1549  return true;
1550 
1551  return RFModule::respond(command,reply);
1552  }
1553 };
1554 
1555 
1556 int main(int argc, char *argv[])
1557 {
1558  Network yarp;
1559 
1560  if (!yarp.checkNetwork())
1561  return 1;
1562 
1563  ResourceFinder rf;
1564  rf.setDefaultContext("actionsRenderingEngine");
1565  rf.setDefaultConfigFile("config.ini");
1566  rf.setDefault("name","actionsRenderingEngine");
1567  rf.configure(argc,argv);
1568 
1569  ActionsRenderingEngineModule mod;
1570 
1571  return mod.runModule(rf);
1572 }
ARE_PortReader
Definition: main.cpp:1432
ARE_PortReader::port_tag
int port_tag
Definition: main.cpp:1436
ARE_PortReader::ARE_PortReader
ARE_PortReader(ActionsRenderingEngine *_are, int _port_tag)
Definition: main.cpp:1456
ARE_PortReader::are
ActionsRenderingEngine * are
Definition: main.cpp:1434
ARE_PortReader::read
bool read(ConnectionReader &connection)
Definition: main.cpp:1439
ActionsRenderingEngineModule::configure
virtual bool configure(ResourceFinder &rf)
Definition: main.cpp:1481
ActionsRenderingEngineModule::close
virtual bool close()
Definition: main.cpp:1520
ActionsRenderingEngineModule::getPeriod
virtual double getPeriod()
Definition: main.cpp:1536
ActionsRenderingEngineModule::port_reader_get
ARE_PortReader * port_reader_get
Definition: main.cpp:1470
ActionsRenderingEngineModule::interruptModule
virtual bool interruptModule()
Definition: main.cpp:1509
ActionsRenderingEngineModule::updateModule
virtual bool updateModule()
Definition: main.cpp:1541
ActionsRenderingEngineModule::respond
virtual bool respond(const Bottle &command, Bottle &reply)
Definition: main.cpp:1546
ActionsRenderingEngineModule::are
ActionsRenderingEngine * are
Definition: main.cpp:1467
ActionsRenderingEngineModule::port_reader_cmd
ARE_PortReader * port_reader_cmd
Definition: main.cpp:1469
ActionsRenderingEngine::initializer
Initializer * initializer
Definition: main.cpp:438
ActionsRenderingEngine::check
bool check(Bottle &bot, const string &name)
Definition: main.cpp:445
ActionsRenderingEngine::visuoThr
VisuoThread * visuoThr
Definition: main.cpp:436
ActionsRenderingEngine::motorThr
MotorThread * motorThr
Definition: main.cpp:435
ActionsRenderingEngine::respond
bool respond(const Bottle &command, Bottle &reply)
Definition: main.cpp:527
ActionsRenderingEngine::closing
volatile bool closing
Definition: main.cpp:441
ActionsRenderingEngine::initialize
bool initialize(ResourceFinder &rf)
Definition: main.cpp:466
ActionsRenderingEngine::process
bool process(int &port_tag, Bottle &command, Bottle &reply)
Definition: main.cpp:677
Initializer::interrupt
bool interrupt()
Definition: utils.h:127
Initializer::close
bool close()
Definition: utils.h:133
MotorThread::push
bool push(Bottle &options)
Definition: MotorThread.cpp:1744
MotorThread::targetToCartesian
bool targetToCartesian(Bottle *target, Vector &xd)
Definition: MotorThread.cpp:340
MotorThread::calibTable
bool calibTable(Bottle &options)
Definition: MotorThread.cpp:2506
MotorThread::powerGrasp
bool powerGrasp(Bottle &options)
Definition: MotorThread.cpp:1678
MotorThread::keepFixation
void keepFixation(Bottle &options)
Definition: MotorThread.h:322
MotorThread::startLearningModeAction
bool startLearningModeAction(Bottle &options)
Definition: MotorThread.cpp:2917
MotorThread::reach
bool reach(Bottle &options)
Definition: MotorThread.cpp:1585
MotorThread::grasp
bool grasp(const Bottle &options)
Definition: MotorThread.cpp:2124
MotorThread::drawNear
bool drawNear(Bottle &options)
Definition: MotorThread.cpp:2421
MotorThread::isHolding
bool isHolding(const Bottle &options)
Definition: MotorThread.cpp:2490
MotorThread::track
void track(Bottle &options)
Definition: MotorThread.h:288
MotorThread::clearIt
bool clearIt(Bottle &options)
Definition: MotorThread.cpp:2078
MotorThread::getTableHeight
bool getTableHeight(double *_table_height)
Definition: MotorThread.h:374
MotorThread::getStatus
void getStatus(Bottle &status)
Definition: MotorThread.cpp:3193
MotorThread::point
bool point(Bottle &options)
Definition: MotorThread.cpp:1797
MotorThread::interrupt
void interrupt()
Definition: MotorThread.cpp:3284
MotorThread::startLearningModeKinOffset
bool startLearningModeKinOffset(Bottle &options)
Definition: MotorThread.cpp:3084
MotorThread::hand
bool hand(const Bottle &options, const string &type="", bool *holding=NULL)
Definition: MotorThread.cpp:2086
MotorThread::give
bool give(Bottle &options)
Definition: MotorThread.cpp:2034
MotorThread::setGazeIdle
void setGazeIdle()
Definition: MotorThread.h:331
MotorThread::lookAtHand
void lookAtHand(Bottle &options)
Definition: MotorThread.h:315
MotorThread::point_far
bool point_far(Bottle &options)
Definition: MotorThread.cpp:1842
MotorThread::imitateAction
bool imitateAction(Bottle &options)
Definition: MotorThread.cpp:3022
MotorThread::exploreHand
bool exploreHand(Bottle &options)
Definition: MotorThread.cpp:2814
MotorThread::look
bool look(Bottle &options)
Definition: MotorThread.cpp:1905
MotorThread::deploy
bool deploy(Bottle &options)
Definition: MotorThread.cpp:2342
MotorThread::suspendLearningModeKinOffset
bool suspendLearningModeKinOffset(Bottle &options)
Definition: MotorThread.cpp:3147
MotorThread::exploreTorso
bool exploreTorso(Bottle &options)
Definition: MotorThread.cpp:2682
MotorThread::changeElbowHeight
bool changeElbowHeight(const int arm, const double height, const double weight)
Definition: MotorThread.cpp:2141
MotorThread::suspendLearningModeAction
bool suspendLearningModeAction(Bottle &options)
Definition: MotorThread.cpp:2980
MotorThread::reinstate
void reinstate()
Definition: MotorThread.cpp:3317
MotorThread::setImpedance
bool setImpedance(bool turn_on)
Definition: MotorThread.cpp:204
MotorThread::changeExecTime
bool changeExecTime(const int arm, const double execTime)
Definition: MotorThread.cpp:2183
MotorThread::release
bool release(const Bottle &options)
Definition: MotorThread.cpp:2134
MotorThread::takeTool
bool takeTool(Bottle &options)
Definition: MotorThread.cpp:1944
MotorThread::calibFingers
bool calibFingers(Bottle &options)
Definition: MotorThread.cpp:2597
MotorThread::setWaveing
bool setWaveing(bool _waveing)
Definition: MotorThread.h:347
MotorThread::shiftAndGrasp
bool shiftAndGrasp(Bottle &options)
Definition: MotorThread.cpp:2444
MotorThread::getHandImagePosition
bool getHandImagePosition(Bottle &hand_image_pos)
Definition: MotorThread.cpp:2467
MotorThread::goHome
bool goHome(Bottle &options)
Definition: MotorThread.cpp:2250
MotorThread::setStereoToCartesianMode
int setStereoToCartesianMode(const int mode, Bottle &reply)
Definition: MotorThread.cpp:256
MotorThread::goUp
bool goUp(Bottle &options, const double h=std::numeric_limits< double >::quiet_NaN())
Definition: MotorThread.cpp:1563
MotorThread::expect
bool expect(Bottle &options)
Definition: MotorThread.cpp:1989
VisuoThread::trackMotion
bool trackMotion()
Definition: VisuoThread.h:167
VisuoThread::isTracking
bool isTracking()
Definition: VisuoThread.h:155
VisuoThread::trainMIL
bool trainMIL()
Definition: VisuoThread.cpp:798
VisuoThread::interrupt
void interrupt()
Definition: VisuoThread.cpp:810
VisuoThread::startLearningMIL
bool startLearningMIL(const string &obj_name)
Definition: VisuoThread.cpp:764
VisuoThread::getTarget
bool getTarget(Value &type, Bottle &target)
Definition: VisuoThread.cpp:438
VisuoThread::reinstate
void reinstate()
Definition: VisuoThread.cpp:839
main
int main(int argc, char *argv[])
Definition: main.cpp:31
GET_HOLDING
#define GET_HOLDING
Definition: main.cpp:402
RPC_GET_STATUS
#define RPC_GET_STATUS
Definition: main.cpp:357
RPC_INTERRUPT
#define RPC_INTERRUPT
Definition: main.cpp:360
GET_IDLE
#define GET_IDLE
Definition: main.cpp:405
CMD_EXPECT
#define CMD_EXPECT
Definition: main.cpp:388
CALIB_KIN_OFFSET
#define CALIB_KIN_OFFSET
Definition: main.cpp:410
RPC_GET
#define RPC_GET
Definition: main.cpp:356
GET_TABLE
#define GET_TABLE
Definition: main.cpp:401
PORT_TAG_CMD
#define PORT_TAG_CMD
Definition: main.cpp:417
RPC_WAVEING
#define RPC_WAVEING
Definition: main.cpp:362
RPC_HELP
#define RPC_HELP
Definition: main.cpp:355
CMD_EXPLORE
#define CMD_EXPLORE
Definition: main.cpp:369
GET_IMAGE
#define GET_IMAGE
Definition: main.cpp:404
CMD_POINT
#define CMD_POINT
Definition: main.cpp:384
CMD_POINT_FAR
#define CMD_POINT_FAR
Definition: main.cpp:385
CMD_GET
#define CMD_GET
Definition: main.cpp:378
RPC_EXECTIME
#define RPC_EXECTIME
Definition: main.cpp:364
RPC_REINSTATE
#define RPC_REINSTATE
Definition: main.cpp:361
PORT_TAG_GET
#define PORT_TAG_GET
Definition: main.cpp:418
CMD_IDLE
#define CMD_IDLE
Definition: main.cpp:366
CMD_LOOK
#define CMD_LOOK
Definition: main.cpp:386
CALIB_TABLE
#define CALIB_TABLE
Definition: main.cpp:408
RPC_IMPEDANCE
#define RPC_IMPEDANCE
Definition: main.cpp:358
CMD_HOME
#define CMD_HOME
Definition: main.cpp:367
CMD_ACTION_IMITATE
#define CMD_ACTION_IMITATE
Definition: main.cpp:397
CMD_TRACK
#define CMD_TRACK
Definition: main.cpp:387
EXPLORE_HAND
#define EXPLORE_HAND
Definition: main.cpp:414
GET_HAND
#define GET_HAND
Definition: main.cpp:403
ACK
#define ACK
Definition: main.cpp:352
GET_S2C
#define GET_S2C
Definition: main.cpp:400
RPC_S2C_MODE
#define RPC_S2C_MODE
Definition: main.cpp:359
CMD_TOUCH
#define CMD_TOUCH
Definition: main.cpp:381
EXPLORE_TORSO
#define EXPLORE_TORSO
Definition: main.cpp:413
CMD_DROP
#define CMD_DROP
Definition: main.cpp:372
CMD_HAND
#define CMD_HAND
Definition: main.cpp:390
CMD_GAZE
#define CMD_GAZE
Definition: main.cpp:391
CALIB_FINGERS
#define CALIB_FINGERS
Definition: main.cpp:409
NACK
#define NACK
Definition: main.cpp:353
CMD_GIVE
#define CMD_GIVE
Definition: main.cpp:389
CMD_TAKE_TOOL
#define CMD_TAKE_TOOL
Definition: main.cpp:394
CMD_CALIBRATE
#define CMD_CALIBRATE
Definition: main.cpp:368
CMD_TAKE
#define CMD_TAKE
Definition: main.cpp:379
CMD_OBSERVE
#define CMD_OBSERVE
Definition: main.cpp:371
CMD_GRASP
#define CMD_GRASP
Definition: main.cpp:380
CMD_LEARN_MIL
#define CMD_LEARN_MIL
Definition: main.cpp:376
RPC_ELBOW
#define RPC_ELBOW
Definition: main.cpp:363
CMD_ACTION_TEACH
#define CMD_ACTION_TEACH
Definition: main.cpp:396
CMD_PUSH
#define CMD_PUSH
Definition: main.cpp:383
done
bool done
Definition: main.cpp:42
LEFT
#define LEFT
Definition: utils.h:33
RIGHT
#define RIGHT
Definition: utils.h:34
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
Generated on Wed Nov 13 2024 21:11:39 for iCub-main by doxygen 1.9.1