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