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, Alessandro Roncone
4  * email: ugo.pattacini@iit.it, alessandro.roncone@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 
563 #include <mutex>
564 #include <cmath>
565 #include <algorithm>
566 #include <cctype>
567 #include <string>
568 #include <fstream>
569 #include <iomanip>
570 #include <map>
571 
572 #include <yarp/os/all.h>
573 #include <yarp/sig/all.h>
574 #include <yarp/dev/all.h>
575 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
576 
577 #include <iCub/localizer.h>
578 #include <iCub/solver.h>
579 #include <iCub/controller.h>
580 
581 #define GAZECTRL_SERVER_VER "2.0"
582 
583 using namespace std;
584 using namespace yarp::os;
585 using namespace yarp::dev;
586 using namespace yarp::sig;
587 
588 
589 /************************************************************************/
590 class GazeModule: public RFModule
591 {
592 protected:
593  ResourceFinder *rf;
598  PolyDriver *drvTorso, *drvHead;
599  PolyDriver mas_client;
604  mutex mutexTweak;
605 
606  IThreeAxisGyroscopes* iGyro;
607  IThreeAxisLinearAccelerometers* iAccel;
608 
609  RpcServer rpcPort;
610 
611  struct Context
612  {
613  // solver part
614  double neckPitchMin;
615  double neckPitchMax;
616  double neckRollMin;
617  double neckRollMax;
618  double neckYawMin;
619  double neckYawMax;
621  double eyesBoundVer;
626 
627  // controller part
628  double eyesTime;
629  double neckTime;
632 
633  // localizer part
634  Bottle pidOptions;
635  };
636 
638  std::map<int,Context> contextMap;
639 
640  /************************************************************************/
641  PolyDriver *waitPart(const Property &partOpt, const double ping_robot_tmo)
642  {
643  string partName=partOpt.find("part").asString();
644  PolyDriver *pDrv=nullptr;
645 
646  double t0=Time::now();
647  while (Time::now()-t0<ping_robot_tmo)
648  {
649  delete pDrv;
650  pDrv=new PolyDriver(const_cast<Property&>(partOpt));
651  bool ok=pDrv->isValid();
652 
653  if (ok)
654  {
655  yInfo("Checking if %s part is active ... yes",partName.c_str());
656  return pDrv;
657  }
658  else
659  {
660  double dt=ping_robot_tmo-(Time::now()-t0);
661  yInfo("Checking if %s part is active ... not yet: still %.1f [s] to timeout expiry",
662  partName.c_str(),dt>0.0?dt:0.0);
663 
664  double t1=Time::now();
665  while (Time::now()-t1<1.0)
666  Time::delay(0.1);
667  }
668 
669  if (interrupting)
670  break;
671  }
672 
673  return pDrv;
674  }
675 
676  /************************************************************************/
677  void storeContext(int *id)
678  {
679  lock_guard<mutex> lg(mutexContext);
680  Context &context=contextMap[contextIdCnt];
681 
682  // solver part
683  slv->getCurNeckPitchRange(context.neckPitchMin,context.neckPitchMax);
684  slv->getCurNeckRollRange(context.neckRollMin,context.neckRollMax);
685  slv->getCurNeckYawRange(context.neckYawMin,context.neckYawMax);
687  context.eyesBoundVer=commData.eyesBoundVer;
688  context.counterRotGain=eyesRefGen->getCounterRotGain();
689  context.saccadesOn=commData.saccadesOn;
692 
693  // controller part
694  context.eyesTime=ctrl->getTeyes();
695  context.neckTime=ctrl->getTneck();
696  context.trackingOn=ctrl->getTrackingMode();
698 
699  // localizer part
700  loc->getPidOptions(context.pidOptions);
701 
702  if (id!=nullptr)
703  *id=contextIdCnt++;
704  }
705 
706  /************************************************************************/
707  bool restoreContext(const int id)
708  {
709  lock_guard<mutex> lg(mutexContext);
710  auto itr=contextMap.find(id);
711  if (itr!=contextMap.end())
712  {
713  Context &context=itr->second;
714 
715  // solver part
716  slv->bindNeckPitch(context.neckPitchMin,context.neckPitchMax);
717  slv->bindNeckRoll(context.neckRollMin,context.neckRollMax);
718  slv->bindNeckYaw(context.neckYawMin,context.neckYawMax);
720  eyesRefGen->manageBindEyes(context.eyesBoundVer);
721  eyesRefGen->setCounterRotGain(context.counterRotGain);
722  commData.saccadesOn=context.saccadesOn;
725 
726  // controller part
727  ctrl->setTeyes(context.eyesTime);
728  ctrl->setTneck(context.neckTime);
729  ctrl->setTrackingMode(context.trackingOn);
731 
732  // localizer part
733  loc->setPidOptions(context.pidOptions);
734 
735  return true;
736  }
737  else
738  return false;
739  }
740 
741  /************************************************************************/
742  bool deleteContexts(Bottle *contextIdList)
743  {
744  if (contextIdList!=nullptr)
745  {
746  lock_guard<mutex> lg(mutexContext);
747  for (int i=0; i<contextIdList->size(); i++)
748  {
749  int id=contextIdList->get(i).asInt32();
750  auto itr=contextMap.find(id);
751  if (itr!=contextMap.end())
752  contextMap.erase(itr);
753  }
754 
755  return true;
756  }
757  else
758  return false;
759  }
760 
761  /************************************************************************/
762  bool getInfo(Bottle &info)
763  {
764  info.clear();
765 
766  Bottle &serverVer=info.addList();
767  serverVer.addString("server_version");
768  serverVer.addString(GAZECTRL_SERVER_VER);
769 
770  Bottle &headVer=info.addList();
771  headVer.addString("head_version");
772  headVer.addString(commData.head_version.get_version());
773 
774  Bottle &minVer=info.addList();
775  minVer.addString("min_allowed_vergence");
776  minVer.addFloat64(CTRL_RAD2DEG*commData.minAllowedVergence);
777 
778  Bottle &events=info.addList();
779  events.addString("events");
780  Bottle &eventsList=events.addList();
781  eventsList.addString("motion-onset");
782  eventsList.addString("motion-done");
783  eventsList.addString("motion-ongoing");
784  eventsList.addString("saccade-onset");
785  eventsList.addString("saccade-done");
786  eventsList.addString("stabilization-on");
787  eventsList.addString("stabilization-off");
788  eventsList.addString("closing");
789  eventsList.addString("suspended");
790  eventsList.addString("resumed");
791  eventsList.addString("comm-timeout");
792  eventsList.addString("*");
793 
794  tweakGet(info);
795 
796  return true;
797  }
798 
799  /************************************************************************/
800  bool tweakSet(const Bottle &options)
801  {
802  lock_guard<mutex> lg(mutexTweak);
803  if (Bottle *pB=options.find("camera_intrinsics_left").asList())
804  {
805  Matrix Prj=zeros(3,4); int w,h;
806  loc->getIntrinsicsMatrix("left",Prj,w,h);
807 
808  int r=0; int c=0;
809  for (int i=0; i<pB->size(); i++)
810  {
811  Prj(r,c)=pB->get(i).asFloat64();
812  if (++c>=Prj.cols())
813  {
814  c=0;
815  if (++r>=Prj.rows())
816  break;
817  }
818  }
819 
820  loc->setIntrinsicsMatrix("left",Prj,w,h);
821  doSaveTweakFile=commData.tweakOverwrite;
822  }
823 
824  if (Bottle *pB=options.find("camera_intrinsics_right").asList())
825  {
826  Matrix Prj=zeros(3,4); int w,h;
827  loc->getIntrinsicsMatrix("right",Prj,w,h);
828 
829  int r=0; int c=0;
830  for (int i=0; i<pB->size(); i++)
831  {
832  Prj(r,c)=pB->get(i).asFloat64();
833  if (++c>=Prj.cols())
834  {
835  c=0;
836  if (++r>=Prj.rows())
837  break;
838  }
839  }
840 
841  loc->setIntrinsicsMatrix("right",Prj,w,h);
842  doSaveTweakFile=commData.tweakOverwrite;
843  }
844 
845  if (options.check("camera_width_left"))
846  {
847  Matrix Prj=zeros(3,4); int w,h;
848  loc->getIntrinsicsMatrix("left",Prj,w,h);
849  w=options.find("camera_width_left").asInt32();
850  loc->setIntrinsicsMatrix("left",Prj,w,h);
851  }
852 
853  if (options.check("camera_height_left"))
854  {
855  Matrix Prj=zeros(3,4); int w,h;
856  loc->getIntrinsicsMatrix("left",Prj,w,h);
857  h=options.find("camera_height_left").asInt32();
858  loc->setIntrinsicsMatrix("left",Prj,w,h);
859  }
860 
861  if (options.check("camera_width_right"))
862  {
863  Matrix Prj=zeros(3,4); int w,h;
864  loc->getIntrinsicsMatrix("right",Prj,w,h);
865  w=options.find("camera_width_right").asInt32();
866  loc->setIntrinsicsMatrix("right",Prj,w,h);
867  }
868 
869  if (options.check("camera_height_right"))
870  {
871  Matrix Prj=zeros(3,4); int w,h;
872  loc->getIntrinsicsMatrix("right",Prj,w,h);
873  h=options.find("camera_height_right").asInt32();
874  loc->setIntrinsicsMatrix("right",Prj,w,h);
875  }
876 
877  bool doMinAllowedVer=false;
878  if (Bottle *pB=options.find("camera_extrinsics_left").asList())
879  {
880  Matrix HN=eye(4,4);
881  loc->getExtrinsicsMatrix("left",HN);
882 
883  int r=0; int c=0;
884  for (int i=0; i<pB->size(); i++)
885  {
886  HN(r,c)=pB->get(i).asFloat64();
887  if (++c>=HN.cols())
888  {
889  c=0;
890  if (++r>=HN.rows())
891  break;
892  }
893  }
894 
895  // enforce the homogeneous property
896  HN(3,0)=HN(3,1)=HN(3,2)=0.0;
897  HN(3,3)=1.0;
898 
899  loc->setExtrinsicsMatrix("left",HN);
900  ctrl->setExtrinsicsMatrix("left",HN);
901  slv->setExtrinsicsMatrix("left",HN);
902  eyesRefGen->setExtrinsicsMatrix("left",HN);
903  doSaveTweakFile=commData.tweakOverwrite;
904  doMinAllowedVer=true;
905  }
906 
907  if (Bottle *pB=options.find("camera_extrinsics_right").asList())
908  {
909  Matrix HN=eye(4,4);
910  loc->getExtrinsicsMatrix("right",HN);
911 
912  int r=0; int c=0;
913  for (int i=0; i<pB->size(); i++)
914  {
915  HN(r,c)=pB->get(i).asFloat64();
916  if (++c>=HN.cols())
917  {
918  c=0;
919  if (++r>=HN.rows())
920  break;
921  }
922  }
923 
924  // enforce the homogeneous property
925  HN(3,0)=HN(3,1)=HN(3,2)=0.0;
926  HN(3,3)=1.0;
927 
928  loc->setExtrinsicsMatrix("right",HN);
929  ctrl->setExtrinsicsMatrix("right",HN);
930  slv->setExtrinsicsMatrix("right",HN);
931  eyesRefGen->setExtrinsicsMatrix("right",HN);
932  doSaveTweakFile=commData.tweakOverwrite;
933  doMinAllowedVer=true;
934  }
935 
936  if (doMinAllowedVer)
937  {
940  eyesRefGen->minAllowedVergenceChanged();
941  }
942 
943  return true;
944  }
945 
946  /************************************************************************/
947  bool tweakGet(Bottle &options)
948  {
949  lock_guard<mutex> lg(mutexTweak);
950  Bottle &intrinsicsLeft=options.addList();
951  intrinsicsLeft.addString("camera_intrinsics_left");
952  Bottle &intrinsicsLeftValues=intrinsicsLeft.addList();
953  Matrix PrjL; int wL,hL;
954  if (loc->getIntrinsicsMatrix("left",PrjL,wL,hL))
955  for (int r=0; r<PrjL.rows(); r++)
956  for (int c=0; c<PrjL.cols(); c++)
957  intrinsicsLeftValues.addFloat64(PrjL(r,c));
958 
959  Bottle &widthLeft=options.addList();
960  widthLeft.addString("camera_width_left");
961  widthLeft.addInt32(wL);
962  Bottle &heightLeft=options.addList();
963  heightLeft.addString("camera_height_left");
964  heightLeft.addInt32(hL);
965 
966  Bottle &intrinsicsRight=options.addList();
967  intrinsicsRight.addString("camera_intrinsics_right");
968  Bottle &intrinsicsRightValues=intrinsicsRight.addList();
969  Matrix PrjR; int wR,hR;
970  if (loc->getIntrinsicsMatrix("right",PrjR,wR,hR))
971  for (int r=0; r<PrjR.rows(); r++)
972  for (int c=0; c<PrjR.cols(); c++)
973  intrinsicsRightValues.addFloat64(PrjR(r,c));
974 
975  Bottle &widthRight=options.addList();
976  widthRight.addString("camera_width_right");
977  widthRight.addInt32(wR);
978  Bottle &heightRight=options.addList();
979  heightRight.addString("camera_height_right");
980  heightRight.addInt32(hR);
981 
982  Bottle &extrinsicsLeft=options.addList();
983  extrinsicsLeft.addString("camera_extrinsics_left");
984  Bottle &extrinsicsLeftValues=extrinsicsLeft.addList();
985  Matrix HL;
986  if (loc->getExtrinsicsMatrix("left",HL))
987  for (int r=0; r<HL.rows(); r++)
988  for (int c=0; c<HL.cols(); c++)
989  extrinsicsLeftValues.addFloat64(HL(r,c));
990 
991  Bottle &extrinsicsRight=options.addList();
992  extrinsicsRight.addString("camera_extrinsics_right");
993  Bottle &extrinsicsRightValues=extrinsicsRight.addList();
994  Matrix HR;
995  if (loc->getExtrinsicsMatrix("right",HR))
996  for (int r=0; r<HR.rows(); r++)
997  for (int c=0; c<HR.cols(); c++)
998  extrinsicsRightValues.addFloat64(HR(r,c));
999 
1000  return true;
1001  }
1002 
1003  /************************************************************************/
1005  {
1006  Matrix PrjL; int wL,hL;
1007  bool validIntrinsicsL=loc->getIntrinsicsMatrix("left",PrjL,wL,hL);
1008 
1009  Matrix PrjR; int wR,hR;
1010  bool validIntrinsicsR=loc->getIntrinsicsMatrix("right",PrjR,wR,hR);
1011 
1012  Matrix HNL;
1013  loc->getExtrinsicsMatrix("left",HNL);
1014 
1015  Matrix HNR;
1016  loc->getExtrinsicsMatrix("right",HNR);
1017 
1018  ofstream fout;
1019  string tweakFile=rf->getHomeContextPath();
1020  tweakFile+="/"+commData.tweakFile;
1021  fout.open(tweakFile.c_str());
1022  if (fout.is_open())
1023  {
1024  if (validIntrinsicsL)
1025  {
1026  fout<<"[CAMERA_CALIBRATION_LEFT]"<<endl;
1027  fout<<"w "<<wL<<endl;
1028  fout<<"h "<<hL<<endl;
1029  fout<<"fx "<<PrjL(0,0)<<endl;
1030  fout<<"fy "<<PrjL(1,1)<<endl;
1031  fout<<"cx "<<PrjL(0,2)<<endl;
1032  fout<<"cy "<<PrjL(1,2)<<endl;
1033  fout<<endl;
1034  }
1035 
1036  if (validIntrinsicsR)
1037  {
1038  fout<<"[CAMERA_CALIBRATION_RIGHT]"<<endl;
1039  fout<<"w "<<wR<<endl;
1040  fout<<"h "<<hR<<endl;
1041  fout<<"fx "<<PrjR(0,0)<<endl;
1042  fout<<"fy "<<PrjR(1,1)<<endl;
1043  fout<<"cx "<<PrjR(0,2)<<endl;
1044  fout<<"cy "<<PrjR(1,2)<<endl;
1045  fout<<endl;
1046  }
1047 
1048  fout.precision(16);
1049  fout<<"[ALIGN_KIN_LEFT]"<<endl;
1050  fout<<"HN (";
1051  for (int r=0; r<HNL.rows(); r++)
1052  for (int c=0; c<HNL.cols(); c++)
1053  fout<<HNL(r,c)<<((r==HNL.rows()-1)&&(c==HNL.cols()-1)?"":" ");
1054  fout<<")"<<endl;
1055  fout<<endl;
1056 
1057  fout<<"[ALIGN_KIN_RIGHT]"<<endl;
1058  fout<<"HN (";
1059  for (int r=0; r<HNR.rows(); r++)
1060  for (int c=0; c<HNR.cols(); c++)
1061  fout<<HNR(r,c)<<((r==HNR.rows()-1)&&(c==HNR.cols()-1)?"":" ");
1062  fout<<")"<<endl;
1063  fout<<endl;
1064 
1065  fout.close();
1066  }
1067  }
1068 
1069  /************************************************************************/
1071  {
1072  // std::map<k,v> is ordered based on std::less<k>
1073  map<iKinLimbVersion,iKinLimbVersion> d;
1074  d[iKinLimbVersion("1.0")-ver_in]=iKinLimbVersion("1.0");
1075  d[iKinLimbVersion("2.0")-ver_in]=iKinLimbVersion("2.0");
1076  d[iKinLimbVersion("2.5")-ver_in]=iKinLimbVersion("2.5");
1077  d[iKinLimbVersion("2.6")-ver_in]=iKinLimbVersion("2.6");
1078  d[iKinLimbVersion("2.7")-ver_in]=iKinLimbVersion("2.7");
1079  d[iKinLimbVersion("2.8")-ver_in]=iKinLimbVersion("2.8");
1080  d[iKinLimbVersion("2.10")-ver_in]=iKinLimbVersion("2.10");
1081  d[iKinLimbVersion("3.0")-ver_in]=iKinLimbVersion("3.0");
1082  d[iKinLimbVersion("3.1")-ver_in]=iKinLimbVersion("3.1");
1083 
1084  auto ver_out=d.begin()->second;
1085  if (ver_out!=ver_in)
1086  {
1087  yWarning("Unknown \"head_version\" %s requested => used \"head_version\" %s instead",
1088  ver_in.get_version().c_str(),ver_out.get_version().c_str());
1089  }
1090 
1091  return ver_out;
1092  }
1093 
1094 public:
1095  /************************************************************************/
1096  GazeModule() : rf{nullptr},
1097  contextIdCnt{0},
1098  loc{nullptr},
1099  eyesRefGen{nullptr},
1100  slv{nullptr},
1101  ctrl{nullptr},
1102  drvTorso{nullptr},
1103  drvHead{nullptr},
1104  interrupting{false},
1105  doSaveTweakFile{false},
1106  iGyro{nullptr},
1107  iAccel{nullptr}
1108  {
1109  }
1110 
1111  /************************************************************************/
1112  bool configure(ResourceFinder &rf)
1113  {
1114  string ctrlName;
1115  string headName;
1116  string torsoName;
1117  double neckTime;
1118  double eyesTime;
1119  double min_abs_vel;
1120  double ping_robot_tmo;
1121  Vector counterRotGain(2);
1122 
1123  // save pointer to rf
1124  this->rf=&rf;
1125 
1126  // retrieve groups
1127  Bottle &imuGroup=rf.findGroup("imu");
1128  Bottle &eyeTiltGroup=rf.findGroup("eye_tilt");
1129  Bottle &trajTimeGroup=rf.findGroup("trajectory_time");
1130  Bottle &camerasGroup=rf.findGroup("cameras");
1131  Bottle &tweakGroup=rf.findGroup("tweak");
1132 
1133  // get params from the command-line
1134  ctrlName=rf.check("name",Value("iKinGazeCtrl")).asString();
1135  headName=rf.check("head",Value("head")).asString();
1136  torsoName=rf.check("torso",Value("torso")).asString();
1137  neckTime=trajTimeGroup.check("neck",Value(0.75)).asFloat64();
1138  eyesTime=trajTimeGroup.check("eyes",Value(0.25)).asFloat64();
1139  min_abs_vel=CTRL_DEG2RAD*fabs(rf.check("min_abs_vel",Value(0.0)).asFloat64());
1140  ping_robot_tmo=rf.check("ping_robot_tmo",Value(40.0)).asFloat64();
1141 
1142  auto head_version=rf.check("head_version",Value("v1.0")).asString();
1143  if ((head_version.length()<2) || (tolower(head_version[0])!='v'))
1144  {
1145  yWarning("Unrecognized \"head_version\" %s; going with default version",head_version.c_str());
1146  head_version="v1.0";
1147  }
1148  commData.head_version = constrainHeadVersion(iKinLimbVersion(head_version.substr(1)));
1149 
1150  commData.robotName=rf.check("robot",Value("icub")).asString();
1151  commData.eyeTiltLim[0]=eyeTiltGroup.check("min",Value(-20.0)).asFloat64();
1152  commData.eyeTiltLim[1]=eyeTiltGroup.check("max",Value(15.0)).asFloat64();
1153  commData.verbose=rf.check("verbose");
1154  commData.saccadesOn=(rf.check("saccades",Value("on")).asString()=="on");
1155  commData.neckPosCtrlOn=(rf.check("neck_position_control",Value("on")).asString()=="on");
1156  commData.stabilizationOn=(imuGroup.check("mode",Value("on")).asString()=="on");
1157  commData.stabilizationGain=imuGroup.check("stabilization_gain",Value(11.0)).asFloat64();
1158  commData.gyro_noise_threshold=CTRL_DEG2RAD*imuGroup.check("gyro_noise_threshold",Value(5.0)).asFloat64();
1159  commData.debugInfoEnabled=rf.check("debugInfo",Value("off")).asString()=="on";
1160 
1161  if (commData.stabilizationOn)
1162  {
1163  counterRotGain[0]=imuGroup.check("vor",Value(1.0)).asFloat64();
1164  counterRotGain[1]=rf.check("ocr",Value(0.0)).asFloat64();
1165  }
1166  else
1167  {
1168  counterRotGain[0]=imuGroup.check("vor",Value(0.0)).asFloat64();
1169  counterRotGain[1]=rf.check("ocr",Value(1.0)).asFloat64();
1170  }
1171 
1172  if (camerasGroup.check("file"))
1173  {
1174  camerasGroup.check("context")?
1175  commData.rf_cameras.setDefaultContext(camerasGroup.find("context").asString()):
1176  commData.rf_cameras.setDefaultContext(rf.getContext());
1177  commData.rf_cameras.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
1178  commData.rf_cameras.configure(0,nullptr);
1179  }
1180 
1181  commData.rf_tweak.setDefaultContext(rf.getContext());
1182  commData.tweakFile=tweakGroup.check("file",Value("tweak.ini")).asString();
1183  commData.tweakOverwrite=(tweakGroup.check("overwrite",Value("on")).asString()=="on");
1184  commData.rf_tweak.setDefaultConfigFile(commData.tweakFile.c_str());
1185  commData.rf_tweak.configure(0,nullptr);
1186 
1187  yInfo("Controller configured for head version %s",commData.head_version.get_version().c_str());
1188 
1189  commData.localStemName="/"+ctrlName;
1190  string remoteHeadName="/"+commData.robotName+"/"+headName;
1191  string localHeadName=commData.localStemName+"/"+headName;
1192  string remoteTorsoName="/"+commData.robotName+"/"+torsoName;
1193  string localTorsoName=commData.localStemName+"/"+torsoName;
1194  string remoteInertialName="/"+commData.robotName+"/head/inertials";
1195  string localInertialName=commData.localStemName+"/head/inertials";
1196 
1197  float imuTimeout {0.04F};
1198  // check if we have to retrieve IMU data from a different port
1199  if (imuGroup.check("source_port_name"))
1200  remoteInertialName=imuGroup.find("source_port_name").asString();
1201  if (imuGroup.check("timeout"))
1202  imuTimeout=imuGroup.find("timeout").asFloat32();
1203 
1204  Property optTorso("(device remote_controlboard)");
1205  optTorso.put("remote",remoteTorsoName);
1206  optTorso.put("local",localTorsoName);
1207  optTorso.put("part",torsoName);
1208 
1209  Property optHead("(device remote_controlboard)");
1210  optHead.put("remote",remoteHeadName);
1211  optHead.put("local",localHeadName);
1212  optHead.put("part",headName);
1213  // mixed position/velocity control entails
1214  // to send two packets per control slot
1215  optHead.put("writeStrict","on");
1216 
1217  if (torsoName!="off")
1218  {
1219  drvTorso=(ping_robot_tmo>0.0)?
1220  waitPart(optTorso,ping_robot_tmo):
1221  new PolyDriver(optTorso);
1222 
1223  if (!drvTorso->isValid())
1224  {
1225  yWarning("Torso device driver not available!");
1226  yWarning("Perhaps only the head is running; trying to continue ...");
1227 
1228  delete drvTorso;
1229  drvTorso=nullptr;
1230  }
1231  }
1232  else
1233  {
1234  yWarning("Torso device is off!");
1235  drvTorso=nullptr;
1236  }
1237 
1238  drvHead=(ping_robot_tmo>0.0)?
1239  waitPart(optHead,ping_robot_tmo):
1240  new PolyDriver(optHead);
1241 
1242  if (!drvHead->isValid())
1243  {
1244  yError("Head device driver not available!");
1245  dispose();
1246  return false;
1247  }
1248  if (commData.stabilizationOn)
1249  {
1250  Property mas_conf{{"device", Value("multipleanalogsensorsclient")},
1251  {"remote", Value(remoteInertialName)},
1252  {"local", Value(localInertialName)},
1253  {"timeout",Value(imuTimeout)}};
1254 
1255  if (!(mas_client.open(mas_conf)))
1256  {
1257  yError("Unable to open the MAS client");
1258  dispose();
1259  return false;
1260  }
1261 
1262  if (!(mas_client.view(iGyro)) ||
1263  !(mas_client.view(iAccel))) {
1264 
1265  yError("View failed of the MAS interfaces");
1266  dispose();
1267  return false;
1268  }
1269 
1270  commData.iGyro = iGyro;
1271  commData.iAccel = iAccel;
1272  }
1273  else {
1274  yWarning("IMU data will be not received/used");
1275  }
1276 
1277  if (commData.debugInfoEnabled)
1278  yDebug("Commands to robot will be also streamed out on debug port");
1279 
1280  // create and start threads
1281  // creation order does matter (for the minimum allowed vergence computation) !!
1282  ctrl=new Controller(drvTorso,drvHead,&commData,neckTime,eyesTime,min_abs_vel,10);
1283  loc=new Localizer(&commData,10);
1284  eyesRefGen=new EyePinvRefGen(drvTorso,drvHead,&commData,ctrl,counterRotGain,20);
1285  slv=new Solver(drvTorso,drvHead,&commData,eyesRefGen,loc,ctrl,20);
1286 
1287  commData.port_xd=new xdPort(slv);
1288  commData.port_xd->open(commData.localStemName+"/xd:i");
1289 
1290  // this switch-on order does matter !!
1291  eyesRefGen->start();
1292  slv->start();
1293  ctrl->start();
1294  loc->start();
1295 
1296  rpcPort.open(commData.localStemName+"/rpc");
1297  attach(rpcPort);
1298 
1299  contextIdCnt=0;
1300 
1301  // reserve id==0 for start-up context
1302  int id0;
1303  storeContext(&id0);
1304 
1305  return true;
1306  }
1307 
1308  /************************************************************************/
1309  bool respond(const Bottle &command, Bottle &reply) override
1310  {
1311  int ack=Vocab32::encode("ack");
1312  int nack=Vocab32::encode("nack");
1313 
1314  if (command.size()>0)
1315  {
1316  switch (command.get(0).asVocab32())
1317  {
1318  //-----------------
1319  case createVocab32('g','e','t'):
1320  {
1321  if (command.size()>1)
1322  {
1323  int type=command.get(1).asVocab32();
1324 
1325  if (type==createVocab32('T','n','e','c'))
1326  {
1327  reply.addVocab32(ack);
1328  reply.addFloat64(ctrl->getTneck());
1329  return true;
1330  }
1331  else if (type==createVocab32('T','e','y','e'))
1332  {
1333  reply.addVocab32(ack);
1334  reply.addFloat64(ctrl->getTeyes());
1335  return true;
1336  }
1337  else if (type==createVocab32('v','o','r'))
1338  {
1339  Vector gain=eyesRefGen->getCounterRotGain();
1340  reply.addVocab32(ack);
1341  reply.addFloat64(gain[0]);
1342  return true;
1343  }
1344  else if (type==createVocab32('o','c','r'))
1345  {
1346  Vector gain=eyesRefGen->getCounterRotGain();
1347  reply.addVocab32(ack);
1348  reply.addFloat64(gain[1]);
1349  return true;
1350  }
1351  else if (type==createVocab32('s','a','c','c'))
1352  {
1353  reply.addVocab32(ack);
1354  reply.addInt32((int)commData.saccadesOn);
1355  return true;
1356  }
1357  else if (type==createVocab32('s','i','n','h'))
1358  {
1359  reply.addVocab32(ack);
1360  reply.addFloat64(commData.saccadesInhibitionPeriod);
1361  return true;
1362  }
1363  else if (type==createVocab32('s','a','c','t'))
1364  {
1365  reply.addVocab32(ack);
1366  reply.addFloat64(commData.saccadesActivationAngle);
1367  return true;
1368  }
1369  else if (type==createVocab32('t','r','a','c'))
1370  {
1371  reply.addVocab32(ack);
1372  reply.addInt32((int)ctrl->getTrackingMode());
1373  return true;
1374  }
1375  else if (type==createVocab32('s','t','a','b'))
1376  {
1377  reply.addVocab32(ack);
1378  reply.addInt32((int)ctrl->getGazeStabilization());
1379  return true;
1380  }
1381  else if (type==createVocab32('d','o','n','e'))
1382  {
1383  reply.addVocab32(ack);
1384  reply.addInt32((int)ctrl->isMotionDone());
1385  return true;
1386  }
1387  else if (type==createVocab32('s','d','o','n'))
1388  {
1389  reply.addVocab32(ack);
1390  reply.addInt32((int)!commData.saccadeUnderway);
1391  return true;
1392  }
1393  else if (type==createVocab32('p','i','t','c'))
1394  {
1395  double min_deg,max_deg;
1396  slv->getCurNeckPitchRange(min_deg,max_deg);
1397 
1398  reply.addVocab32(ack);
1399  reply.addFloat64(min_deg);
1400  reply.addFloat64(max_deg);
1401  return true;
1402  }
1403  else if (type==createVocab32('r','o','l','l'))
1404  {
1405  double min_deg,max_deg;
1406  slv->getCurNeckRollRange(min_deg,max_deg);
1407 
1408  reply.addVocab32(ack);
1409  reply.addFloat64(min_deg);
1410  reply.addFloat64(max_deg);
1411  return true;
1412  }
1413  else if (type==createVocab32('y','a','w'))
1414  {
1415  double min_deg,max_deg;
1416  slv->getCurNeckYawRange(min_deg,max_deg);
1417 
1418  reply.addVocab32(ack);
1419  reply.addFloat64(min_deg);
1420  reply.addFloat64(max_deg);
1421  return true;
1422  }
1423  else if (type==createVocab32('e','y','e','s'))
1424  {
1425  reply.addVocab32(ack);
1426  reply.addFloat64(commData.eyesBoundVer);
1427  return true;
1428  }
1429  else if (type==createVocab32('n','t','o','l'))
1430  {
1431  double angle=slv->getNeckAngleUserTolerance();
1432 
1433  reply.addVocab32(ack);
1434  reply.addFloat64(angle);
1435  return true;
1436  }
1437  else if (type==createVocab32('d','e','s'))
1438  {
1439  Vector des;
1440  if (ctrl->getDesired(des))
1441  {
1442  reply.addVocab32(ack);
1443  reply.addList().read(des);
1444  return true;
1445  }
1446  }
1447  else if (type==createVocab32('v','e','l'))
1448  {
1449  Vector vel;
1450  if (ctrl->getVelocity(vel))
1451  {
1452  reply.addVocab32(ack);
1453  reply.addList().read(vel);
1454  return true;
1455  }
1456  }
1457  else if ((type==createVocab32('p','o','s','e')) && (command.size()>2))
1458  {
1459  string poseSel=command.get(2).asString();
1460  Vector x;
1461  Stamp stamp;
1462 
1463  if (ctrl->getPose(poseSel,x,stamp))
1464  {
1465  reply.addVocab32(ack);
1466  reply.addList().read(x);
1467 
1468  Bottle &bStamp=reply.addList();
1469  bStamp.addInt32(stamp.getCount());
1470  bStamp.addFloat64(stamp.getTime());
1471 
1472  return true;
1473  }
1474  }
1475  else if ((type==createVocab32('2','D')) && (command.size()>2))
1476  {
1477  if (Bottle *bOpt=command.get(2).asList())
1478  {
1479  if (bOpt->size()>3)
1480  {
1481  Vector x(3);
1482  string eye=bOpt->get(0).asString();
1483  x[0]=bOpt->get(1).asFloat64();
1484  x[1]=bOpt->get(2).asFloat64();
1485  x[2]=bOpt->get(3).asFloat64();
1486 
1487  Vector px;
1488  if (loc->projectPoint(eye,x,px))
1489  {
1490  reply.addVocab32(ack);
1491  reply.addList().read(px);
1492  return true;
1493  }
1494  }
1495  }
1496  }
1497  else if ((type==createVocab32('3','D')) && (command.size()>3))
1498  {
1499  int subType=command.get(2).asVocab32();
1500  if (subType==createVocab32('m','o','n','o'))
1501  {
1502  if (Bottle *bOpt=command.get(3).asList())
1503  {
1504  if (bOpt->size()>3)
1505  {
1506  string eye=bOpt->get(0).asString();
1507  double u=bOpt->get(1).asFloat64();
1508  double v=bOpt->get(2).asFloat64();
1509  double z=bOpt->get(3).asFloat64();
1510 
1511  Vector x;
1512  if (loc->projectPoint(eye,u,v,z,x))
1513  {
1514  reply.addVocab32(ack);
1515  reply.addList().read(x);
1516  return true;
1517  }
1518  }
1519  }
1520  }
1521  else if (subType==createVocab32('s','t','e','r'))
1522  {
1523  if (Bottle *bOpt=command.get(3).asList())
1524  {
1525  if (bOpt->size()>3)
1526  {
1527  Vector pxl(2),pxr(2);
1528  pxl[0]=bOpt->get(0).asFloat64();
1529  pxl[1]=bOpt->get(1).asFloat64();
1530  pxr[0]=bOpt->get(2).asFloat64();
1531  pxr[1]=bOpt->get(3).asFloat64();
1532 
1533  Vector x;
1534  if (loc->triangulatePoint(pxl,pxr,x))
1535  {
1536  reply.addVocab32(ack);
1537  reply.addList().read(x);
1538  return true;
1539  }
1540  }
1541  }
1542  }
1543  else if (subType==createVocab32('p','r','o','j'))
1544  {
1545  if (Bottle *bOpt=command.get(3).asList())
1546  {
1547  if (bOpt->size()>6)
1548  {
1549  Vector plane(4);
1550  string eye=bOpt->get(0).asString();
1551  double u=bOpt->get(1).asFloat64();
1552  double v=bOpt->get(2).asFloat64();
1553  plane[0]=bOpt->get(3).asFloat64();
1554  plane[1]=bOpt->get(4).asFloat64();
1555  plane[2]=bOpt->get(5).asFloat64();
1556  plane[3]=bOpt->get(6).asFloat64();
1557 
1558  Vector x;
1559  if (loc->projectPoint(eye,u,v,plane,x))
1560  {
1561  reply.addVocab32(ack);
1562  reply.addList().read(x);
1563  return true;
1564  }
1565  }
1566  }
1567  }
1568  else if (subType==createVocab32('a','n','g'))
1569  {
1570  if (Bottle *bOpt=command.get(3).asList())
1571  {
1572  if (bOpt->size()>3)
1573  {
1574  Vector ang(3);
1575  string type=bOpt->get(0).asString();
1576  ang[0]=CTRL_DEG2RAD*bOpt->get(1).asFloat64();
1577  ang[1]=CTRL_DEG2RAD*bOpt->get(2).asFloat64();
1578  ang[2]=CTRL_DEG2RAD*bOpt->get(3).asFloat64();
1579 
1580  Vector x=loc->get3DPoint(type,ang);
1581  reply.addVocab32(ack);
1582  reply.addList().read(x);
1583  return true;
1584  }
1585  }
1586  }
1587  }
1588  else if ((type==createVocab32('a','n','g')) && (command.size()>2))
1589  {
1590  if (Bottle *bOpt=command.get(2).asList())
1591  {
1592  if (bOpt->size()>2)
1593  {
1594  Vector x(3);
1595  x[0]=bOpt->get(0).asFloat64();
1596  x[1]=bOpt->get(1).asFloat64();
1597  x[2]=bOpt->get(2).asFloat64();
1598 
1599  Vector ang=CTRL_RAD2DEG*loc->getAbsAngles(x);
1600  reply.addVocab32(ack);
1601  reply.addList().read(ang);
1602  return true;
1603  }
1604  }
1605  }
1606  else if (type==createVocab32('p','i','d'))
1607  {
1608  Bottle options;
1609  loc->getPidOptions(options);
1610 
1611  reply.addVocab32(ack);
1612  reply.addList()=options;
1613  return true;
1614  }
1615  else if (type==createVocab32('i','n','f','o'))
1616  {
1617  Bottle info;
1618  if (getInfo(info))
1619  {
1620  reply.addVocab32(ack);
1621  reply.addList()=info;
1622  return true;
1623  }
1624  }
1625  else if (type==createVocab32('t','w','e','a'))
1626  {
1627  Bottle options;
1628  if (tweakGet(options))
1629  {
1630  reply.addVocab32(ack);
1631  reply.addList()=options;
1632  return true;
1633  }
1634  }
1635  }
1636 
1637  break;
1638  }
1639 
1640  //-----------------
1641  case createVocab32('s','e','t'):
1642  {
1643  if (command.size()>2)
1644  {
1645  int type=command.get(1).asVocab32();
1646  if (type==createVocab32('T','n','e','c'))
1647  {
1648  double execTime=command.get(2).asFloat64();
1649  ctrl->setTneck(execTime);
1650  reply.addVocab32(ack);
1651  return true;
1652  }
1653  else if (type==createVocab32('T','e','y','e'))
1654  {
1655  double execTime=command.get(2).asFloat64();
1656  ctrl->setTeyes(execTime);
1657  reply.addVocab32(ack);
1658  return true;
1659  }
1660  else if (type==createVocab32('v','o','r'))
1661  {
1662  Vector gain=eyesRefGen->getCounterRotGain();
1663  gain[0]=command.get(2).asFloat64();
1664  eyesRefGen->setCounterRotGain(gain);
1665  reply.addVocab32(ack);
1666  return true;
1667  }
1668  else if (type==createVocab32('o','c','r'))
1669  {
1670  Vector gain=eyesRefGen->getCounterRotGain();
1671  gain[1]=command.get(2).asFloat64();
1672  eyesRefGen->setCounterRotGain(gain);
1673  reply.addVocab32(ack);
1674  return true;
1675  }
1676  else if (type==createVocab32('s','a','c','c'))
1677  {
1678  commData.saccadesOn=(command.get(2).asInt32()>0);
1679  reply.addVocab32(ack);
1680  return true;
1681  }
1682  else if (type==createVocab32('s','i','n','h'))
1683  {
1684  double period=command.get(2).asFloat64();
1685  commData.saccadesInhibitionPeriod=period;
1686  reply.addVocab32(ack);
1687  return true;
1688  }
1689  else if (type==createVocab32('s','a','c','t'))
1690  {
1691  double angle=command.get(2).asFloat64();
1692  commData.saccadesActivationAngle=angle;
1693  reply.addVocab32(ack);
1694  return true;
1695  }
1696  else if (type==createVocab32('n','t','o','l'))
1697  {
1698  double angle=command.get(2).asFloat64();
1699  slv->setNeckAngleUserTolerance(angle);
1700  reply.addVocab32(ack);
1701  return true;
1702  }
1703  else if (type==createVocab32('t','r','a','c'))
1704  {
1705  bool mode=(command.get(2).asInt32()>0);
1706  ctrl->setTrackingMode(mode);
1707  reply.addVocab32(ack);
1708  return true;
1709  }
1710  else if (type==createVocab32('s','t','a','b'))
1711  {
1712  bool mode=(command.get(2).asInt32()>0);
1713  reply.addVocab32(ctrl->setGazeStabilization(mode)?ack:nack);
1714  return true;
1715  }
1716  else if (type==createVocab32('p','i','d'))
1717  {
1718  if (Bottle *bOpt=command.get(2).asList())
1719  {
1720  loc->setPidOptions(*bOpt);
1721  reply.addVocab32(ack);
1722  return true;
1723  }
1724  }
1725  else if (type==createVocab32('t','w','e','a'))
1726  {
1727  if (Bottle *bOpt=command.get(2).asList())
1728  {
1729  reply.addVocab32(tweakSet(*bOpt)?ack:nack);
1730  return true;
1731  }
1732  }
1733  }
1734 
1735  break;
1736  }
1737 
1738  //-----------------
1739  case createVocab32('l','o','o','k'):
1740  {
1741  if (command.size()>2)
1742  {
1743  int type=command.get(1).asVocab32();
1744  if (type==createVocab32('3','D'))
1745  {
1746  if (Bottle *bOpt=command.get(2).asList())
1747  {
1748  if (bOpt->size()>2)
1749  {
1750  Vector x(3);
1751  x[0]=bOpt->get(0).asFloat64();
1752  x[1]=bOpt->get(1).asFloat64();
1753  x[2]=bOpt->get(2).asFloat64();
1754 
1755  if (ctrl->look(x))
1756  {
1757  reply.addVocab32(ack);
1758  return true;
1759  }
1760  }
1761  }
1762  }
1763  else if (type==createVocab32('m','o','n','o'))
1764  {
1765  if (Bottle *bOpt=command.get(2).asList())
1766  {
1767  if (bOpt->size()>3)
1768  {
1769  string eye=bOpt->get(0).asString();
1770  double u=bOpt->get(1).asFloat64();
1771  double v=bOpt->get(2).asFloat64();
1772  double z;
1773 
1774  bool ok=false;
1775  if (bOpt->get(3).isFloat64())
1776  {
1777  z=bOpt->get(3).asFloat64();
1778  ok=true;
1779  }
1780  else if ((bOpt->get(3).asString()=="ver") && (bOpt->size()>4))
1781  {
1782  double ver=bOpt->get(4).asFloat64();
1783  z=loc->getDistFromVergence(ver);
1784  ok=true;
1785  }
1786 
1787  if (ok)
1788  {
1789  Vector x;
1790  if (loc->projectPoint(eye,u,v,z,x))
1791  {
1792  if (ctrl->look(x))
1793  {
1794  reply.addVocab32(ack);
1795  return true;
1796  }
1797  }
1798  }
1799  }
1800  }
1801  }
1802  else if (type==createVocab32('s','t','e','r'))
1803  {
1804  if (Bottle *bOpt=command.get(2).asList())
1805  {
1806  if (bOpt->size()>3)
1807  {
1808  Vector pxl(2),pxr(2);
1809  pxl[0]=bOpt->get(0).asFloat64();
1810  pxl[1]=bOpt->get(1).asFloat64();
1811  pxr[0]=bOpt->get(2).asFloat64();
1812  pxr[1]=bOpt->get(3).asFloat64();
1813 
1814  Vector x;
1815  if (loc->triangulatePoint(pxl,pxr,x))
1816  {
1817  if (ctrl->look(x))
1818  {
1819  reply.addVocab32(ack);
1820  return true;
1821  }
1822  }
1823  }
1824  }
1825  }
1826  else if (type==createVocab32('a','n','g'))
1827  {
1828  if (Bottle *bOpt=command.get(2).asList())
1829  {
1830  if (bOpt->size()>3)
1831  {
1832  Vector ang(3);
1833  string type=bOpt->get(0).asString();
1834  ang[0]=CTRL_DEG2RAD*bOpt->get(1).asFloat64();
1835  ang[1]=CTRL_DEG2RAD*bOpt->get(2).asFloat64();
1836  ang[2]=CTRL_DEG2RAD*bOpt->get(3).asFloat64();
1837 
1838  Vector x=loc->get3DPoint(type,ang);
1839  if (ctrl->look(x))
1840  {
1841  reply.addVocab32(ack);
1842  return true;
1843  }
1844  }
1845  }
1846  }
1847  }
1848 
1849  break;
1850  }
1851 
1852  //-----------------
1853  case createVocab32('s','t','o','p'):
1854  {
1855  ctrl->stopControl();
1856  reply.addVocab32(ack);
1857  return true;
1858  }
1859 
1860  //-----------------
1861  case createVocab32('s','t','o','r'):
1862  {
1863  int id;
1864  storeContext(&id);
1865  reply.addVocab32(ack);
1866  reply.addInt32(id);
1867  return true;
1868  }
1869 
1870  //-----------------
1871  case createVocab32('r','e','s','t'):
1872  {
1873  if (command.size()>1)
1874  {
1875  int id=command.get(1).asInt32();
1876  if (restoreContext(id))
1877  {
1878  reply.addVocab32(ack);
1879  return true;
1880  }
1881  }
1882 
1883  break;
1884  }
1885 
1886  //-----------------
1887  case createVocab32('d','e','l'):
1888  {
1889  if (command.size()>1)
1890  {
1891  Bottle *ids=command.get(1).asList();
1892  if (deleteContexts(ids))
1893  {
1894  reply.addVocab32(ack);
1895  return true;
1896  }
1897  }
1898 
1899  break;
1900  }
1901 
1902  //-----------------
1903  case createVocab32('b','i','n','d'):
1904  {
1905  if (command.size()>2)
1906  {
1907  int joint=command.get(1).asVocab32();
1908  if (joint==createVocab32('p','i','t','c'))
1909  {
1910  double min=command.get(2).asFloat64();
1911  double max=command.get(3).asFloat64();
1912  slv->bindNeckPitch(min,max);
1913  reply.addVocab32(ack);
1914  return true;
1915  }
1916  else if (joint==createVocab32('r','o','l','l'))
1917  {
1918  double min=command.get(2).asFloat64();
1919  double max=command.get(3).asFloat64();
1920  slv->bindNeckRoll(min,max);
1921  reply.addVocab32(ack);
1922  return true;
1923  }
1924  else if (joint==createVocab32('y','a','w'))
1925  {
1926  double min=command.get(2).asFloat64();
1927  double max=command.get(3).asFloat64();
1928  slv->bindNeckYaw(min,max);
1929  reply.addVocab32(ack);
1930  return true;
1931  }
1932  else if (joint==createVocab32('e','y','e','s'))
1933  {
1934  double ver=command.get(2).asFloat64();
1935  eyesRefGen->bindEyes(ver);
1936  reply.addVocab32(ack);
1937  return true;
1938  }
1939  }
1940 
1941  break;
1942  }
1943 
1944  //-----------------
1945  case createVocab32('c','l','e','a'):
1946  {
1947  if (command.size()>1)
1948  {
1949  int joint=command.get(1).asVocab32();
1950  if (joint==createVocab32('p','i','t','c'))
1951  {
1952  slv->clearNeckPitch();
1953  reply.addVocab32(ack);
1954  return true;
1955  }
1956  else if (joint==createVocab32('r','o','l','l'))
1957  {
1958  slv->clearNeckRoll();
1959  reply.addVocab32(ack);
1960  return true;
1961  }
1962  else if (joint==createVocab32('y','a','w'))
1963  {
1964  slv->clearNeckYaw();
1965  reply.addVocab32(ack);
1966  return true;
1967  }
1968  else if (joint==createVocab32('e','y','e','s'))
1969  {
1970  eyesRefGen->clearEyes();
1971  reply.addVocab32(ack);
1972  return true;
1973  }
1974  }
1975 
1976  break;
1977  }
1978 
1979  //-----------------
1980  case createVocab32('r','e','g','i'):
1981  {
1982  if (command.size()>1)
1983  {
1984  int type=command.get(1).asVocab32();
1985  if (type==createVocab32('o','n','g','o'))
1986  {
1987  if (command.size()>2)
1988  {
1989  double checkPoint=command.get(2).asFloat64();
1990  if (ctrl->registerMotionOngoingEvent(checkPoint))
1991  {
1992  reply.addVocab32(ack);
1993  return true;
1994  }
1995  }
1996  }
1997  }
1998 
1999  break;
2000  }
2001 
2002  //-----------------
2003  case createVocab32('u','n','r','e'):
2004  {
2005  if (command.size()>1)
2006  {
2007  int type=command.get(1).asVocab32();
2008  if (type==createVocab32('o','n','g','o'))
2009  {
2010  if (command.size()>2)
2011  {
2012  double checkPoint=command.get(2).asFloat64();
2013  if (ctrl->unregisterMotionOngoingEvent(checkPoint))
2014  {
2015  reply.addVocab32(ack);
2016  return true;
2017  }
2018  }
2019  }
2020  }
2021 
2022  break;
2023  }
2024 
2025  //-----------------
2026  case createVocab32('l','i','s','t'):
2027  {
2028  if (command.size()>1)
2029  {
2030  int type=command.get(1).asVocab32();
2031  if (type==createVocab32('o','n','g','o'))
2032  {
2033  reply.addVocab32(ack);
2034  reply.addList()=ctrl->listMotionOngoingEvents();
2035  return true;
2036  }
2037  }
2038 
2039  break;
2040  }
2041 
2042  //-----------------
2043  case createVocab32('s','u','s','p'):
2044  {
2045  ctrl->suspend();
2046  eyesRefGen->suspend();
2047  slv->suspend();
2048  reply.addVocab32(ack);
2049  return true;
2050  }
2051 
2052  //-----------------
2053  case createVocab32('r','u','n'):
2054  {
2055  slv->resume();
2056  eyesRefGen->resume();
2057  ctrl->resume();
2058  reply.addVocab32(ack);
2059  return true;
2060  }
2061 
2062  //-----------------
2063  case createVocab32('s','t','a','t'):
2064  {
2065  reply.addVocab32(ack);
2066  if (ctrl->isSuspended())
2067  reply.addString("suspended");
2068  else
2069  reply.addString("running");
2070  return true;
2071  }
2072 
2073  //-----------------
2074  default:
2075  return RFModule::respond(command,reply);
2076  }
2077  }
2078 
2079  reply.addVocab32(nack);
2080  return true;
2081  }
2082 
2083  /************************************************************************/
2084  void dispose()
2085  {
2086  if (loc!=nullptr)
2087  loc->stop();
2088 
2089  if (eyesRefGen!=nullptr)
2090  eyesRefGen->stop();
2091 
2092  if (slv!=nullptr)
2093  slv->stop();
2094 
2095  if (ctrl!=nullptr)
2096  ctrl->stop();
2097 
2098  if (drvTorso!=nullptr)
2099  drvTorso->close();
2100 
2101  if (drvHead!=nullptr)
2102  drvHead->close();
2103 
2104  if (mas_client.isValid())
2105  mas_client.close();
2106 
2107  if (commData.port_xd!=nullptr)
2108  if (!commData.port_xd->isClosed())
2109  commData.port_xd->close();
2110  if (rpcPort.asPort().isOpen())
2111  rpcPort.close();
2112 
2113  // this switch-off order does matter !!
2114  delete commData.port_xd;
2115  delete loc;
2116  delete eyesRefGen;
2117  delete slv;
2118  delete ctrl;
2119  delete drvTorso;
2120  delete drvHead;
2121 
2122  contextMap.clear();
2123  }
2124 
2125  /************************************************************************/
2126  bool interruptModule() override
2127  {
2128  interrupting=true;
2129 
2130  if (commData.port_xd!=nullptr)
2131  commData.port_xd->interrupt();
2132  rpcPort.interrupt();
2133 
2134  return true;
2135  }
2136 
2137  /************************************************************************/
2138  bool close() override
2139  {
2140  dispose();
2141  return true;
2142  }
2143 
2144  /************************************************************************/
2145  double getPeriod() override
2146  {
2147  return 1.0;
2148  }
2149 
2150  /************************************************************************/
2151  bool updateModule() override
2152  {
2153  if (doSaveTweakFile)
2154  {
2155  lock_guard<mutex> lg(mutexTweak);
2156  saveTweakFile();
2157  doSaveTweakFile=false;
2158  }
2159 
2160  return true;
2161  }
2162 };
2163 
2164 
2165 /************************************************************************/
2166 int main(int argc, char *argv[])
2167 {
2168  ResourceFinder rf;
2169  rf.setDefaultContext("iKinGazeCtrl");
2170  rf.setDefaultConfigFile("config.ini");
2171  rf.configure(argc,argv);
2172 
2173  Network yarp;
2174  if (!yarp.checkNetwork())
2175  {
2176  yError("YARP server not available!");
2177  return 1;
2178  }
2179 
2180  GazeModule mod;
2181  return mod.runModule(rf);
2182 }
@ nack
@ ack
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
bool unregisterMotionOngoingEvent(const double checkPoint)
bool setGazeStabilization(const bool f)
Definition: controller.cpp:600
bool getPose(const string &poseSel, Vector &x, Stamp &stamp)
double getTeyes() const
void resume()
void suspend()
void stopControl()
Definition: controller.cpp:321
void setTeyes(const double execTime)
bool look(const Vector &x)
Definition: controller.cpp:519
void minAllowedVergenceChanged() override
Definition: controller.cpp:210
void setTneck(const double execTime)
bool getVelocity(Vector &vel)
bool isMotionDone()
bool getDesired(Vector &des)
void findMinimumAllowedVergence()
Definition: controller.cpp:173
void setTrackingMode(const bool f)
bool getGazeStabilization() const
Definition: controller.cpp:635
double getTneck() const
bool getTrackingMode() const
Bottle listMotionOngoingEvents()
bool registerMotionOngoingEvent(const double checkPoint)
ResourceFinder rf_cameras
Definition: utils.h:156
double saccadesActivationAngle
Definition: utils.h:145
string robotName
Definition: utils.h:136
bool stabilizationOn
Definition: utils.h:154
double stabilizationGain
Definition: utils.h:142
IThreeAxisLinearAccelerometers * iAccel
Definition: utils.h:162
double gyro_noise_threshold
Definition: utils.h:141
double saccadesInhibitionPeriod
Definition: utils.h:144
bool tweakOverwrite
Definition: utils.h:151
ResourceFinder rf_tweak
Definition: utils.h:157
Vector eyeTiltLim
Definition: utils.h:138
string tweakFile
Definition: utils.h:158
double minAllowedVergence
Definition: utils.h:139
xdPort * port_xd
Definition: utils.h:135
IThreeAxisGyroscopes * iGyro
Definition: utils.h:161
bool debugInfoEnabled
Definition: utils.h:159
bool saccadesOn
Definition: utils.h:152
iKinLimbVersion head_version
Definition: utils.h:143
bool neckPosCtrlOn
Definition: utils.h:153
bool saccadeUnderway
Definition: utils.h:149
double eyesBoundVer
Definition: utils.h:140
bool verbose
Definition: utils.h:150
string localStemName
Definition: utils.h:137
void minAllowedVergenceChanged() override
Definition: solver.cpp:136
bool clearEyes()
Definition: solver.cpp:182
Vector getCounterRotGain()
Definition: solver.cpp:218
void manageBindEyes(const double ver)
Definition: solver.cpp:210
bool bindEyes(const double ver)
Definition: solver.cpp:146
void suspend()
Definition: solver.cpp:444
void setCounterRotGain(const Vector &gain)
Definition: solver.cpp:226
void resume()
Definition: solver.cpp:452
virtual bool getExtrinsicsMatrix(const string &type, Matrix &M)
Definition: utils.cpp:379
virtual bool setExtrinsicsMatrix(const string &type, const Matrix &M)
Definition: utils.cpp:397
bool restoreContext(const int id)
Definition: main.cpp:707
void storeContext(int *id)
Definition: main.cpp:677
ResourceFinder * rf
Definition: main.cpp:593
PolyDriver * drvHead
Definition: main.cpp:598
bool configure(ResourceFinder &rf)
Definition: main.cpp:1112
bool tweakSet(const Bottle &options)
Definition: main.cpp:800
Solver * slv
Definition: main.cpp:596
EyePinvRefGen * eyesRefGen
Definition: main.cpp:595
bool getInfo(Bottle &info)
Definition: main.cpp:762
bool respond(const Bottle &command, Bottle &reply) override
Definition: main.cpp:1309
bool deleteContexts(Bottle *contextIdList)
Definition: main.cpp:742
bool doSaveTweakFile
Definition: main.cpp:602
bool updateModule() override
Definition: main.cpp:2151
IThreeAxisLinearAccelerometers * iAccel
Definition: main.cpp:607
std::map< int, Context > contextMap
Definition: main.cpp:638
iKinLimbVersion constrainHeadVersion(const iKinLimbVersion &ver_in)
Definition: main.cpp:1070
mutex mutexContext
Definition: main.cpp:603
void dispose()
Definition: main.cpp:2084
RpcServer rpcPort
Definition: main.cpp:609
void saveTweakFile()
Definition: main.cpp:1004
PolyDriver * waitPart(const Property &partOpt, const double ping_robot_tmo)
Definition: main.cpp:641
mutex mutexTweak
Definition: main.cpp:604
double getPeriod() override
Definition: main.cpp:2145
GazeModule()
Definition: main.cpp:1096
ExchangeData commData
Definition: main.cpp:600
bool interruptModule() override
Definition: main.cpp:2126
IThreeAxisGyroscopes * iGyro
Definition: main.cpp:606
Controller * ctrl
Definition: main.cpp:597
bool tweakGet(Bottle &options)
Definition: main.cpp:947
bool close() override
Definition: main.cpp:2138
PolyDriver mas_client
Definition: main.cpp:599
int contextIdCnt
Definition: main.cpp:637
bool interrupting
Definition: main.cpp:601
Localizer * loc
Definition: main.cpp:594
void getPidOptions(Bottle &options)
Definition: localizer.cpp:185
bool setIntrinsicsMatrix(const string &type, const Matrix &M, const int w, const int h)
Definition: localizer.cpp:683
double getDistFromVergence(const double ver)
Definition: localizer.cpp:515
Vector get3DPoint(const string &type, const Vector &ang)
Definition: localizer.cpp:229
void setPidOptions(const Bottle &options)
Definition: localizer.cpp:196
bool getIntrinsicsMatrix(const string &type, Matrix &M, int &w, int &h)
Definition: localizer.cpp:650
bool triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
Definition: localizer.cpp:450
bool projectPoint(const string &type, const Vector &x, Vector &px)
Definition: localizer.cpp:296
Vector getAbsAngles(const Vector &x)
Definition: localizer.cpp:210
Definition: solver.h:120
void bindNeckPitch(const double min_deg, const double max_deg)
Definition: solver.cpp:569
void getCurNeckYawRange(double &min_deg, double &max_deg)
Definition: solver.cpp:629
void clearNeckYaw()
Definition: solver.cpp:660
void getCurNeckPitchRange(double &min_deg, double &max_deg)
Definition: solver.cpp:611
void suspend()
Definition: solver.cpp:872
void clearNeckRoll()
Definition: solver.cpp:649
double getNeckAngleUserTolerance() const
Definition: solver.cpp:671
void bindNeckYaw(const double min_deg, const double max_deg)
Definition: solver.cpp:597
void clearNeckPitch()
Definition: solver.cpp:638
void setNeckAngleUserTolerance(const double angle)
Definition: solver.cpp:678
void resume()
Definition: solver.cpp:881
void getCurNeckRollRange(double &min_deg, double &max_deg)
Definition: solver.cpp:620
void bindNeckRoll(const double min_deg, const double max_deg)
Definition: solver.cpp:583
A class for defining the versions of the iCub limbs.
Definition: iKinFwd.h:1046
std::string get_version() const
Return the version string.
Definition: iKinFwd.cpp:1600
Definition: utils.h:56
zeros(2, 2) eye(2
int main(int argc, char *argv[])
Definition: main.cpp:31
#define GAZECTRL_SERVER_VER
Definition: main.cpp:581
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
Copyright (C) 2008 RobotCub Consortium.
double neckRollMin
Definition: main.cpp:616
double saccadesInhibitionPeriod
Definition: main.cpp:624
Bottle pidOptions
Definition: main.cpp:634
bool gazeStabilizationOn
Definition: main.cpp:631
double eyesBoundVer
Definition: main.cpp:621
double neckYawMin
Definition: main.cpp:618
double neckPitchMin
Definition: main.cpp:614
double neckPitchMax
Definition: main.cpp:615
double neckYawMax
Definition: main.cpp:619
double saccadesActivationAngle
Definition: main.cpp:625
double neckAngleUserTolerance
Definition: main.cpp:620
Vector counterRotGain
Definition: main.cpp:622
double neckRollMax
Definition: main.cpp:617