iCub-main
Loading...
Searching...
No Matches
main.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Ugo Pattacini, 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
578using namespace std;
579using namespace yarp::os;
580using namespace yarp::dev;
581using namespace yarp::sig;
582
583
584/************************************************************************/
585class GazeModule: public RFModule
586{
587protected:
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
1078 auto ver_out=d.begin()->second;
1079 if (ver_out!=ver_in)
1080 {
1081 yWarning("Unknown \"head_version\" %s requested => used \"head_version\" %s instead",
1082 ver_in.get_version().c_str(),ver_out.get_version().c_str());
1083 }
1084
1085 return ver_out;
1086 }
1087
1088public:
1089 /************************************************************************/
1090 GazeModule() : rf{nullptr},
1091 contextIdCnt{0},
1092 loc{nullptr},
1093 eyesRefGen{nullptr},
1094 slv{nullptr},
1095 ctrl{nullptr},
1096 drvTorso{nullptr},
1097 drvHead{nullptr},
1098 interrupting{false},
1099 doSaveTweakFile{false},
1100 iGyro{nullptr},
1101 iAccel{nullptr}
1102 {
1103 }
1104
1105 /************************************************************************/
1106 bool configure(ResourceFinder &rf)
1107 {
1108 string ctrlName;
1109 string headName;
1110 string torsoName;
1111 double neckTime;
1112 double eyesTime;
1113 double min_abs_vel;
1114 double ping_robot_tmo;
1115 Vector counterRotGain(2);
1116
1117 // save pointer to rf
1118 this->rf=&rf;
1119
1120 // retrieve groups
1121 Bottle &imuGroup=rf.findGroup("imu");
1122 Bottle &eyeTiltGroup=rf.findGroup("eye_tilt");
1123 Bottle &trajTimeGroup=rf.findGroup("trajectory_time");
1124 Bottle &camerasGroup=rf.findGroup("cameras");
1125 Bottle &tweakGroup=rf.findGroup("tweak");
1126
1127 // get params from the command-line
1128 ctrlName=rf.check("name",Value("iKinGazeCtrl")).asString();
1129 headName=rf.check("head",Value("head")).asString();
1130 torsoName=rf.check("torso",Value("torso")).asString();
1131 neckTime=trajTimeGroup.check("neck",Value(0.75)).asFloat64();
1132 eyesTime=trajTimeGroup.check("eyes",Value(0.25)).asFloat64();
1133 min_abs_vel=CTRL_DEG2RAD*fabs(rf.check("min_abs_vel",Value(0.0)).asFloat64());
1134 ping_robot_tmo=rf.check("ping_robot_tmo",Value(40.0)).asFloat64();
1135
1136 auto head_version=rf.check("head_version",Value("v1.0")).asString();
1137 if ((head_version.length()<2) || (tolower(head_version[0])!='v'))
1138 {
1139 yWarning("Unrecognized \"head_version\" %s; going with default version",head_version.c_str());
1140 head_version="v1.0";
1141 }
1142 commData.head_version = constrainHeadVersion(iKinLimbVersion(head_version.substr(1)));
1143
1144 commData.robotName=rf.check("robot",Value("icub")).asString();
1145 commData.eyeTiltLim[0]=eyeTiltGroup.check("min",Value(-20.0)).asFloat64();
1146 commData.eyeTiltLim[1]=eyeTiltGroup.check("max",Value(15.0)).asFloat64();
1147 commData.verbose=rf.check("verbose");
1148 commData.saccadesOn=(rf.check("saccades",Value("on")).asString()=="on");
1149 commData.neckPosCtrlOn=(rf.check("neck_position_control",Value("on")).asString()=="on");
1150 commData.stabilizationOn=(imuGroup.check("mode",Value("on")).asString()=="on");
1151 commData.stabilizationGain=imuGroup.check("stabilization_gain",Value(11.0)).asFloat64();
1152 commData.gyro_noise_threshold=CTRL_DEG2RAD*imuGroup.check("gyro_noise_threshold",Value(5.0)).asFloat64();
1153 commData.debugInfoEnabled=rf.check("debugInfo",Value("off")).asString()=="on";
1154
1155 if (commData.stabilizationOn)
1156 {
1157 counterRotGain[0]=imuGroup.check("vor",Value(1.0)).asFloat64();
1158 counterRotGain[1]=rf.check("ocr",Value(0.0)).asFloat64();
1159 }
1160 else
1161 {
1162 counterRotGain[0]=imuGroup.check("vor",Value(0.0)).asFloat64();
1163 counterRotGain[1]=rf.check("ocr",Value(1.0)).asFloat64();
1164 }
1165
1166 if (camerasGroup.check("file"))
1167 {
1168 camerasGroup.check("context")?
1169 commData.rf_cameras.setDefaultContext(camerasGroup.find("context").asString()):
1170 commData.rf_cameras.setDefaultContext(rf.getContext());
1171 commData.rf_cameras.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
1172 commData.rf_cameras.configure(0,nullptr);
1173 }
1174
1175 commData.rf_tweak.setDefaultContext(rf.getContext());
1176 commData.tweakFile=tweakGroup.check("file",Value("tweak.ini")).asString();
1177 commData.tweakOverwrite=(tweakGroup.check("overwrite",Value("on")).asString()=="on");
1178 commData.rf_tweak.setDefaultConfigFile(commData.tweakFile.c_str());
1179 commData.rf_tweak.configure(0,nullptr);
1180
1181 yInfo("Controller configured for head version %s",commData.head_version.get_version().c_str());
1182
1183 commData.localStemName="/"+ctrlName;
1184 string remoteHeadName="/"+commData.robotName+"/"+headName;
1185 string localHeadName=commData.localStemName+"/"+headName;
1186 string remoteTorsoName="/"+commData.robotName+"/"+torsoName;
1187 string localTorsoName=commData.localStemName+"/"+torsoName;
1188 string remoteInertialName="/"+commData.robotName+"/head/inertials";
1189 string localInertialName=commData.localStemName+"/head/inertials";
1190
1191 float imuTimeout {0.04F};
1192 // check if we have to retrieve IMU data from a different port
1193 if (imuGroup.check("source_port_name"))
1194 remoteInertialName=imuGroup.find("source_port_name").asString();
1195 if (imuGroup.check("timeout"))
1196 imuTimeout=imuGroup.find("timeout").asFloat32();
1197
1198 Property optTorso("(device remote_controlboard)");
1199 optTorso.put("remote",remoteTorsoName);
1200 optTorso.put("local",localTorsoName);
1201 optTorso.put("part",torsoName);
1202
1203 Property optHead("(device remote_controlboard)");
1204 optHead.put("remote",remoteHeadName);
1205 optHead.put("local",localHeadName);
1206 optHead.put("part",headName);
1207 // mixed position/velocity control entails
1208 // to send two packets per control slot
1209 optHead.put("writeStrict","on");
1210
1211 if (torsoName!="off")
1212 {
1213 drvTorso=(ping_robot_tmo>0.0)?
1214 waitPart(optTorso,ping_robot_tmo):
1215 new PolyDriver(optTorso);
1216
1217 if (!drvTorso->isValid())
1218 {
1219 yWarning("Torso device driver not available!");
1220 yWarning("Perhaps only the head is running; trying to continue ...");
1221
1222 delete drvTorso;
1223 drvTorso=nullptr;
1224 }
1225 }
1226 else
1227 {
1228 yWarning("Torso device is off!");
1229 drvTorso=nullptr;
1230 }
1231
1232 drvHead=(ping_robot_tmo>0.0)?
1233 waitPart(optHead,ping_robot_tmo):
1234 new PolyDriver(optHead);
1235
1236 if (!drvHead->isValid())
1237 {
1238 yError("Head device driver not available!");
1239 dispose();
1240 return false;
1241 }
1242 if (commData.stabilizationOn)
1243 {
1244 Property mas_conf{{"device", Value("multipleanalogsensorsclient")},
1245 {"remote", Value(remoteInertialName)},
1246 {"local", Value(localInertialName)},
1247 {"timeout",Value(imuTimeout)}};
1248
1249 if (!(mas_client.open(mas_conf)))
1250 {
1251 yError("Unable to open the MAS client");
1252 dispose();
1253 return false;
1254 }
1255
1256 if (!(mas_client.view(iGyro)) ||
1257 !(mas_client.view(iAccel))) {
1258
1259 yError("View failed of the MAS interfaces");
1260 dispose();
1261 return false;
1262 }
1263
1264 commData.iGyro = iGyro;
1265 commData.iAccel = iAccel;
1266 }
1267 else {
1268 yWarning("IMU data will be not received/used");
1269 }
1270
1271 if (commData.debugInfoEnabled)
1272 yDebug("Commands to robot will be also streamed out on debug port");
1273
1274 // create and start threads
1275 // creation order does matter (for the minimum allowed vergence computation) !!
1276 ctrl=new Controller(drvTorso,drvHead,&commData,neckTime,eyesTime,min_abs_vel,10);
1277 loc=new Localizer(&commData,10);
1278 eyesRefGen=new EyePinvRefGen(drvTorso,drvHead,&commData,ctrl,counterRotGain,20);
1279 slv=new Solver(drvTorso,drvHead,&commData,eyesRefGen,loc,ctrl,20);
1280
1281 commData.port_xd=new xdPort(slv);
1282 commData.port_xd->open(commData.localStemName+"/xd:i");
1283
1284 // this switch-on order does matter !!
1285 eyesRefGen->start();
1286 slv->start();
1287 ctrl->start();
1288 loc->start();
1289
1290 rpcPort.open(commData.localStemName+"/rpc");
1291 attach(rpcPort);
1292
1293 contextIdCnt=0;
1294
1295 // reserve id==0 for start-up context
1296 int id0;
1297 storeContext(&id0);
1298
1299 return true;
1300 }
1301
1302 /************************************************************************/
1303 bool respond(const Bottle &command, Bottle &reply) override
1304 {
1305 int ack=Vocab32::encode("ack");
1306 int nack=Vocab32::encode("nack");
1307
1308 if (command.size()>0)
1309 {
1310 switch (command.get(0).asVocab32())
1311 {
1312 //-----------------
1313 case createVocab32('g','e','t'):
1314 {
1315 if (command.size()>1)
1316 {
1317 int type=command.get(1).asVocab32();
1318
1319 if (type==createVocab32('T','n','e','c'))
1320 {
1321 reply.addVocab32(ack);
1322 reply.addFloat64(ctrl->getTneck());
1323 return true;
1324 }
1325 else if (type==createVocab32('T','e','y','e'))
1326 {
1327 reply.addVocab32(ack);
1328 reply.addFloat64(ctrl->getTeyes());
1329 return true;
1330 }
1331 else if (type==createVocab32('v','o','r'))
1332 {
1333 Vector gain=eyesRefGen->getCounterRotGain();
1334 reply.addVocab32(ack);
1335 reply.addFloat64(gain[0]);
1336 return true;
1337 }
1338 else if (type==createVocab32('o','c','r'))
1339 {
1340 Vector gain=eyesRefGen->getCounterRotGain();
1341 reply.addVocab32(ack);
1342 reply.addFloat64(gain[1]);
1343 return true;
1344 }
1345 else if (type==createVocab32('s','a','c','c'))
1346 {
1347 reply.addVocab32(ack);
1348 reply.addInt32((int)commData.saccadesOn);
1349 return true;
1350 }
1351 else if (type==createVocab32('s','i','n','h'))
1352 {
1353 reply.addVocab32(ack);
1354 reply.addFloat64(commData.saccadesInhibitionPeriod);
1355 return true;
1356 }
1357 else if (type==createVocab32('s','a','c','t'))
1358 {
1359 reply.addVocab32(ack);
1360 reply.addFloat64(commData.saccadesActivationAngle);
1361 return true;
1362 }
1363 else if (type==createVocab32('t','r','a','c'))
1364 {
1365 reply.addVocab32(ack);
1366 reply.addInt32((int)ctrl->getTrackingMode());
1367 return true;
1368 }
1369 else if (type==createVocab32('s','t','a','b'))
1370 {
1371 reply.addVocab32(ack);
1372 reply.addInt32((int)ctrl->getGazeStabilization());
1373 return true;
1374 }
1375 else if (type==createVocab32('d','o','n','e'))
1376 {
1377 reply.addVocab32(ack);
1378 reply.addInt32((int)ctrl->isMotionDone());
1379 return true;
1380 }
1381 else if (type==createVocab32('s','d','o','n'))
1382 {
1383 reply.addVocab32(ack);
1384 reply.addInt32((int)!commData.saccadeUnderway);
1385 return true;
1386 }
1387 else if (type==createVocab32('p','i','t','c'))
1388 {
1389 double min_deg,max_deg;
1390 slv->getCurNeckPitchRange(min_deg,max_deg);
1391
1392 reply.addVocab32(ack);
1393 reply.addFloat64(min_deg);
1394 reply.addFloat64(max_deg);
1395 return true;
1396 }
1397 else if (type==createVocab32('r','o','l','l'))
1398 {
1399 double min_deg,max_deg;
1400 slv->getCurNeckRollRange(min_deg,max_deg);
1401
1402 reply.addVocab32(ack);
1403 reply.addFloat64(min_deg);
1404 reply.addFloat64(max_deg);
1405 return true;
1406 }
1407 else if (type==createVocab32('y','a','w'))
1408 {
1409 double min_deg,max_deg;
1410 slv->getCurNeckYawRange(min_deg,max_deg);
1411
1412 reply.addVocab32(ack);
1413 reply.addFloat64(min_deg);
1414 reply.addFloat64(max_deg);
1415 return true;
1416 }
1417 else if (type==createVocab32('e','y','e','s'))
1418 {
1419 reply.addVocab32(ack);
1420 reply.addFloat64(commData.eyesBoundVer);
1421 return true;
1422 }
1423 else if (type==createVocab32('n','t','o','l'))
1424 {
1425 double angle=slv->getNeckAngleUserTolerance();
1426
1427 reply.addVocab32(ack);
1428 reply.addFloat64(angle);
1429 return true;
1430 }
1431 else if (type==createVocab32('d','e','s'))
1432 {
1433 Vector des;
1434 if (ctrl->getDesired(des))
1435 {
1436 reply.addVocab32(ack);
1437 reply.addList().read(des);
1438 return true;
1439 }
1440 }
1441 else if (type==createVocab32('v','e','l'))
1442 {
1443 Vector vel;
1444 if (ctrl->getVelocity(vel))
1445 {
1446 reply.addVocab32(ack);
1447 reply.addList().read(vel);
1448 return true;
1449 }
1450 }
1451 else if ((type==createVocab32('p','o','s','e')) && (command.size()>2))
1452 {
1453 string poseSel=command.get(2).asString();
1454 Vector x;
1455 Stamp stamp;
1456
1457 if (ctrl->getPose(poseSel,x,stamp))
1458 {
1459 reply.addVocab32(ack);
1460 reply.addList().read(x);
1461
1462 Bottle &bStamp=reply.addList();
1463 bStamp.addInt32(stamp.getCount());
1464 bStamp.addFloat64(stamp.getTime());
1465
1466 return true;
1467 }
1468 }
1469 else if ((type==createVocab32('2','D')) && (command.size()>2))
1470 {
1471 if (Bottle *bOpt=command.get(2).asList())
1472 {
1473 if (bOpt->size()>3)
1474 {
1475 Vector x(3);
1476 string eye=bOpt->get(0).asString();
1477 x[0]=bOpt->get(1).asFloat64();
1478 x[1]=bOpt->get(2).asFloat64();
1479 x[2]=bOpt->get(3).asFloat64();
1480
1481 Vector px;
1482 if (loc->projectPoint(eye,x,px))
1483 {
1484 reply.addVocab32(ack);
1485 reply.addList().read(px);
1486 return true;
1487 }
1488 }
1489 }
1490 }
1491 else if ((type==createVocab32('3','D')) && (command.size()>3))
1492 {
1493 int subType=command.get(2).asVocab32();
1494 if (subType==createVocab32('m','o','n','o'))
1495 {
1496 if (Bottle *bOpt=command.get(3).asList())
1497 {
1498 if (bOpt->size()>3)
1499 {
1500 string eye=bOpt->get(0).asString();
1501 double u=bOpt->get(1).asFloat64();
1502 double v=bOpt->get(2).asFloat64();
1503 double z=bOpt->get(3).asFloat64();
1504
1505 Vector x;
1506 if (loc->projectPoint(eye,u,v,z,x))
1507 {
1508 reply.addVocab32(ack);
1509 reply.addList().read(x);
1510 return true;
1511 }
1512 }
1513 }
1514 }
1515 else if (subType==createVocab32('s','t','e','r'))
1516 {
1517 if (Bottle *bOpt=command.get(3).asList())
1518 {
1519 if (bOpt->size()>3)
1520 {
1521 Vector pxl(2),pxr(2);
1522 pxl[0]=bOpt->get(0).asFloat64();
1523 pxl[1]=bOpt->get(1).asFloat64();
1524 pxr[0]=bOpt->get(2).asFloat64();
1525 pxr[1]=bOpt->get(3).asFloat64();
1526
1527 Vector x;
1528 if (loc->triangulatePoint(pxl,pxr,x))
1529 {
1530 reply.addVocab32(ack);
1531 reply.addList().read(x);
1532 return true;
1533 }
1534 }
1535 }
1536 }
1537 else if (subType==createVocab32('p','r','o','j'))
1538 {
1539 if (Bottle *bOpt=command.get(3).asList())
1540 {
1541 if (bOpt->size()>6)
1542 {
1543 Vector plane(4);
1544 string eye=bOpt->get(0).asString();
1545 double u=bOpt->get(1).asFloat64();
1546 double v=bOpt->get(2).asFloat64();
1547 plane[0]=bOpt->get(3).asFloat64();
1548 plane[1]=bOpt->get(4).asFloat64();
1549 plane[2]=bOpt->get(5).asFloat64();
1550 plane[3]=bOpt->get(6).asFloat64();
1551
1552 Vector x;
1553 if (loc->projectPoint(eye,u,v,plane,x))
1554 {
1555 reply.addVocab32(ack);
1556 reply.addList().read(x);
1557 return true;
1558 }
1559 }
1560 }
1561 }
1562 else if (subType==createVocab32('a','n','g'))
1563 {
1564 if (Bottle *bOpt=command.get(3).asList())
1565 {
1566 if (bOpt->size()>3)
1567 {
1568 Vector ang(3);
1569 string type=bOpt->get(0).asString();
1570 ang[0]=CTRL_DEG2RAD*bOpt->get(1).asFloat64();
1571 ang[1]=CTRL_DEG2RAD*bOpt->get(2).asFloat64();
1572 ang[2]=CTRL_DEG2RAD*bOpt->get(3).asFloat64();
1573
1574 Vector x=loc->get3DPoint(type,ang);
1575 reply.addVocab32(ack);
1576 reply.addList().read(x);
1577 return true;
1578 }
1579 }
1580 }
1581 }
1582 else if ((type==createVocab32('a','n','g')) && (command.size()>2))
1583 {
1584 if (Bottle *bOpt=command.get(2).asList())
1585 {
1586 if (bOpt->size()>2)
1587 {
1588 Vector x(3);
1589 x[0]=bOpt->get(0).asFloat64();
1590 x[1]=bOpt->get(1).asFloat64();
1591 x[2]=bOpt->get(2).asFloat64();
1592
1593 Vector ang=CTRL_RAD2DEG*loc->getAbsAngles(x);
1594 reply.addVocab32(ack);
1595 reply.addList().read(ang);
1596 return true;
1597 }
1598 }
1599 }
1600 else if (type==createVocab32('p','i','d'))
1601 {
1602 Bottle options;
1603 loc->getPidOptions(options);
1604
1605 reply.addVocab32(ack);
1606 reply.addList()=options;
1607 return true;
1608 }
1609 else if (type==createVocab32('i','n','f','o'))
1610 {
1611 Bottle info;
1612 if (getInfo(info))
1613 {
1614 reply.addVocab32(ack);
1615 reply.addList()=info;
1616 return true;
1617 }
1618 }
1619 else if (type==createVocab32('t','w','e','a'))
1620 {
1621 Bottle options;
1622 if (tweakGet(options))
1623 {
1624 reply.addVocab32(ack);
1625 reply.addList()=options;
1626 return true;
1627 }
1628 }
1629 }
1630
1631 break;
1632 }
1633
1634 //-----------------
1635 case createVocab32('s','e','t'):
1636 {
1637 if (command.size()>2)
1638 {
1639 int type=command.get(1).asVocab32();
1640 if (type==createVocab32('T','n','e','c'))
1641 {
1642 double execTime=command.get(2).asFloat64();
1643 ctrl->setTneck(execTime);
1644 reply.addVocab32(ack);
1645 return true;
1646 }
1647 else if (type==createVocab32('T','e','y','e'))
1648 {
1649 double execTime=command.get(2).asFloat64();
1650 ctrl->setTeyes(execTime);
1651 reply.addVocab32(ack);
1652 return true;
1653 }
1654 else if (type==createVocab32('v','o','r'))
1655 {
1656 Vector gain=eyesRefGen->getCounterRotGain();
1657 gain[0]=command.get(2).asFloat64();
1658 eyesRefGen->setCounterRotGain(gain);
1659 reply.addVocab32(ack);
1660 return true;
1661 }
1662 else if (type==createVocab32('o','c','r'))
1663 {
1664 Vector gain=eyesRefGen->getCounterRotGain();
1665 gain[1]=command.get(2).asFloat64();
1666 eyesRefGen->setCounterRotGain(gain);
1667 reply.addVocab32(ack);
1668 return true;
1669 }
1670 else if (type==createVocab32('s','a','c','c'))
1671 {
1672 commData.saccadesOn=(command.get(2).asInt32()>0);
1673 reply.addVocab32(ack);
1674 return true;
1675 }
1676 else if (type==createVocab32('s','i','n','h'))
1677 {
1678 double period=command.get(2).asFloat64();
1679 commData.saccadesInhibitionPeriod=period;
1680 reply.addVocab32(ack);
1681 return true;
1682 }
1683 else if (type==createVocab32('s','a','c','t'))
1684 {
1685 double angle=command.get(2).asFloat64();
1686 commData.saccadesActivationAngle=angle;
1687 reply.addVocab32(ack);
1688 return true;
1689 }
1690 else if (type==createVocab32('n','t','o','l'))
1691 {
1692 double angle=command.get(2).asFloat64();
1693 slv->setNeckAngleUserTolerance(angle);
1694 reply.addVocab32(ack);
1695 return true;
1696 }
1697 else if (type==createVocab32('t','r','a','c'))
1698 {
1699 bool mode=(command.get(2).asInt32()>0);
1700 ctrl->setTrackingMode(mode);
1701 reply.addVocab32(ack);
1702 return true;
1703 }
1704 else if (type==createVocab32('s','t','a','b'))
1705 {
1706 bool mode=(command.get(2).asInt32()>0);
1707 reply.addVocab32(ctrl->setGazeStabilization(mode)?ack:nack);
1708 return true;
1709 }
1710 else if (type==createVocab32('p','i','d'))
1711 {
1712 if (Bottle *bOpt=command.get(2).asList())
1713 {
1714 loc->setPidOptions(*bOpt);
1715 reply.addVocab32(ack);
1716 return true;
1717 }
1718 }
1719 else if (type==createVocab32('t','w','e','a'))
1720 {
1721 if (Bottle *bOpt=command.get(2).asList())
1722 {
1723 reply.addVocab32(tweakSet(*bOpt)?ack:nack);
1724 return true;
1725 }
1726 }
1727 }
1728
1729 break;
1730 }
1731
1732 //-----------------
1733 case createVocab32('l','o','o','k'):
1734 {
1735 if (command.size()>2)
1736 {
1737 int type=command.get(1).asVocab32();
1738 if (type==createVocab32('3','D'))
1739 {
1740 if (Bottle *bOpt=command.get(2).asList())
1741 {
1742 if (bOpt->size()>2)
1743 {
1744 Vector x(3);
1745 x[0]=bOpt->get(0).asFloat64();
1746 x[1]=bOpt->get(1).asFloat64();
1747 x[2]=bOpt->get(2).asFloat64();
1748
1749 if (ctrl->look(x))
1750 {
1751 reply.addVocab32(ack);
1752 return true;
1753 }
1754 }
1755 }
1756 }
1757 else if (type==createVocab32('m','o','n','o'))
1758 {
1759 if (Bottle *bOpt=command.get(2).asList())
1760 {
1761 if (bOpt->size()>3)
1762 {
1763 string eye=bOpt->get(0).asString();
1764 double u=bOpt->get(1).asFloat64();
1765 double v=bOpt->get(2).asFloat64();
1766 double z;
1767
1768 bool ok=false;
1769 if (bOpt->get(3).isFloat64())
1770 {
1771 z=bOpt->get(3).asFloat64();
1772 ok=true;
1773 }
1774 else if ((bOpt->get(3).asString()=="ver") && (bOpt->size()>4))
1775 {
1776 double ver=bOpt->get(4).asFloat64();
1777 z=loc->getDistFromVergence(ver);
1778 ok=true;
1779 }
1780
1781 if (ok)
1782 {
1783 Vector x;
1784 if (loc->projectPoint(eye,u,v,z,x))
1785 {
1786 if (ctrl->look(x))
1787 {
1788 reply.addVocab32(ack);
1789 return true;
1790 }
1791 }
1792 }
1793 }
1794 }
1795 }
1796 else if (type==createVocab32('s','t','e','r'))
1797 {
1798 if (Bottle *bOpt=command.get(2).asList())
1799 {
1800 if (bOpt->size()>3)
1801 {
1802 Vector pxl(2),pxr(2);
1803 pxl[0]=bOpt->get(0).asFloat64();
1804 pxl[1]=bOpt->get(1).asFloat64();
1805 pxr[0]=bOpt->get(2).asFloat64();
1806 pxr[1]=bOpt->get(3).asFloat64();
1807
1808 Vector x;
1809 if (loc->triangulatePoint(pxl,pxr,x))
1810 {
1811 if (ctrl->look(x))
1812 {
1813 reply.addVocab32(ack);
1814 return true;
1815 }
1816 }
1817 }
1818 }
1819 }
1820 else if (type==createVocab32('a','n','g'))
1821 {
1822 if (Bottle *bOpt=command.get(2).asList())
1823 {
1824 if (bOpt->size()>3)
1825 {
1826 Vector ang(3);
1827 string type=bOpt->get(0).asString();
1828 ang[0]=CTRL_DEG2RAD*bOpt->get(1).asFloat64();
1829 ang[1]=CTRL_DEG2RAD*bOpt->get(2).asFloat64();
1830 ang[2]=CTRL_DEG2RAD*bOpt->get(3).asFloat64();
1831
1832 Vector x=loc->get3DPoint(type,ang);
1833 if (ctrl->look(x))
1834 {
1835 reply.addVocab32(ack);
1836 return true;
1837 }
1838 }
1839 }
1840 }
1841 }
1842
1843 break;
1844 }
1845
1846 //-----------------
1847 case createVocab32('s','t','o','p'):
1848 {
1849 ctrl->stopControl();
1850 reply.addVocab32(ack);
1851 return true;
1852 }
1853
1854 //-----------------
1855 case createVocab32('s','t','o','r'):
1856 {
1857 int id;
1858 storeContext(&id);
1859 reply.addVocab32(ack);
1860 reply.addInt32(id);
1861 return true;
1862 }
1863
1864 //-----------------
1865 case createVocab32('r','e','s','t'):
1866 {
1867 if (command.size()>1)
1868 {
1869 int id=command.get(1).asInt32();
1870 if (restoreContext(id))
1871 {
1872 reply.addVocab32(ack);
1873 return true;
1874 }
1875 }
1876
1877 break;
1878 }
1879
1880 //-----------------
1881 case createVocab32('d','e','l'):
1882 {
1883 if (command.size()>1)
1884 {
1885 Bottle *ids=command.get(1).asList();
1886 if (deleteContexts(ids))
1887 {
1888 reply.addVocab32(ack);
1889 return true;
1890 }
1891 }
1892
1893 break;
1894 }
1895
1896 //-----------------
1897 case createVocab32('b','i','n','d'):
1898 {
1899 if (command.size()>2)
1900 {
1901 int joint=command.get(1).asVocab32();
1902 if (joint==createVocab32('p','i','t','c'))
1903 {
1904 double min=command.get(2).asFloat64();
1905 double max=command.get(3).asFloat64();
1906 slv->bindNeckPitch(min,max);
1907 reply.addVocab32(ack);
1908 return true;
1909 }
1910 else if (joint==createVocab32('r','o','l','l'))
1911 {
1912 double min=command.get(2).asFloat64();
1913 double max=command.get(3).asFloat64();
1914 slv->bindNeckRoll(min,max);
1915 reply.addVocab32(ack);
1916 return true;
1917 }
1918 else if (joint==createVocab32('y','a','w'))
1919 {
1920 double min=command.get(2).asFloat64();
1921 double max=command.get(3).asFloat64();
1922 slv->bindNeckYaw(min,max);
1923 reply.addVocab32(ack);
1924 return true;
1925 }
1926 else if (joint==createVocab32('e','y','e','s'))
1927 {
1928 double ver=command.get(2).asFloat64();
1929 eyesRefGen->bindEyes(ver);
1930 reply.addVocab32(ack);
1931 return true;
1932 }
1933 }
1934
1935 break;
1936 }
1937
1938 //-----------------
1939 case createVocab32('c','l','e','a'):
1940 {
1941 if (command.size()>1)
1942 {
1943 int joint=command.get(1).asVocab32();
1944 if (joint==createVocab32('p','i','t','c'))
1945 {
1946 slv->clearNeckPitch();
1947 reply.addVocab32(ack);
1948 return true;
1949 }
1950 else if (joint==createVocab32('r','o','l','l'))
1951 {
1952 slv->clearNeckRoll();
1953 reply.addVocab32(ack);
1954 return true;
1955 }
1956 else if (joint==createVocab32('y','a','w'))
1957 {
1958 slv->clearNeckYaw();
1959 reply.addVocab32(ack);
1960 return true;
1961 }
1962 else if (joint==createVocab32('e','y','e','s'))
1963 {
1964 eyesRefGen->clearEyes();
1965 reply.addVocab32(ack);
1966 return true;
1967 }
1968 }
1969
1970 break;
1971 }
1972
1973 //-----------------
1974 case createVocab32('r','e','g','i'):
1975 {
1976 if (command.size()>1)
1977 {
1978 int type=command.get(1).asVocab32();
1979 if (type==createVocab32('o','n','g','o'))
1980 {
1981 if (command.size()>2)
1982 {
1983 double checkPoint=command.get(2).asFloat64();
1984 if (ctrl->registerMotionOngoingEvent(checkPoint))
1985 {
1986 reply.addVocab32(ack);
1987 return true;
1988 }
1989 }
1990 }
1991 }
1992
1993 break;
1994 }
1995
1996 //-----------------
1997 case createVocab32('u','n','r','e'):
1998 {
1999 if (command.size()>1)
2000 {
2001 int type=command.get(1).asVocab32();
2002 if (type==createVocab32('o','n','g','o'))
2003 {
2004 if (command.size()>2)
2005 {
2006 double checkPoint=command.get(2).asFloat64();
2007 if (ctrl->unregisterMotionOngoingEvent(checkPoint))
2008 {
2009 reply.addVocab32(ack);
2010 return true;
2011 }
2012 }
2013 }
2014 }
2015
2016 break;
2017 }
2018
2019 //-----------------
2020 case createVocab32('l','i','s','t'):
2021 {
2022 if (command.size()>1)
2023 {
2024 int type=command.get(1).asVocab32();
2025 if (type==createVocab32('o','n','g','o'))
2026 {
2027 reply.addVocab32(ack);
2028 reply.addList()=ctrl->listMotionOngoingEvents();
2029 return true;
2030 }
2031 }
2032
2033 break;
2034 }
2035
2036 //-----------------
2037 case createVocab32('s','u','s','p'):
2038 {
2039 ctrl->suspend();
2040 eyesRefGen->suspend();
2041 slv->suspend();
2042 reply.addVocab32(ack);
2043 return true;
2044 }
2045
2046 //-----------------
2047 case createVocab32('r','u','n'):
2048 {
2049 slv->resume();
2050 eyesRefGen->resume();
2051 ctrl->resume();
2052 reply.addVocab32(ack);
2053 return true;
2054 }
2055
2056 //-----------------
2057 case createVocab32('s','t','a','t'):
2058 {
2059 reply.addVocab32(ack);
2060 if (ctrl->isSuspended())
2061 reply.addString("suspended");
2062 else
2063 reply.addString("running");
2064 return true;
2065 }
2066
2067 //-----------------
2068 default:
2069 return RFModule::respond(command,reply);
2070 }
2071 }
2072
2073 reply.addVocab32(nack);
2074 return true;
2075 }
2076
2077 /************************************************************************/
2078 void dispose()
2079 {
2080 if (loc!=nullptr)
2081 loc->stop();
2082
2083 if (eyesRefGen!=nullptr)
2084 eyesRefGen->stop();
2085
2086 if (slv!=nullptr)
2087 slv->stop();
2088
2089 if (ctrl!=nullptr)
2090 ctrl->stop();
2091
2092 if (drvTorso!=nullptr)
2093 drvTorso->close();
2094
2095 if (drvHead!=nullptr)
2096 drvHead->close();
2097
2098 if (mas_client.isValid())
2099 mas_client.close();
2100
2101 if (commData.port_xd!=nullptr)
2102 if (!commData.port_xd->isClosed())
2103 commData.port_xd->close();
2104 if (rpcPort.asPort().isOpen())
2105 rpcPort.close();
2106
2107 // this switch-off order does matter !!
2108 delete commData.port_xd;
2109 delete loc;
2110 delete eyesRefGen;
2111 delete slv;
2112 delete ctrl;
2113 delete drvTorso;
2114 delete drvHead;
2115
2116 contextMap.clear();
2117 }
2118
2119 /************************************************************************/
2120 bool interruptModule() override
2121 {
2122 interrupting=true;
2123
2124 if (commData.port_xd!=nullptr)
2125 commData.port_xd->interrupt();
2126 rpcPort.interrupt();
2127
2128 return true;
2129 }
2130
2131 /************************************************************************/
2132 bool close() override
2133 {
2134 dispose();
2135 return true;
2136 }
2137
2138 /************************************************************************/
2139 double getPeriod() override
2140 {
2141 return 1.0;
2142 }
2143
2144 /************************************************************************/
2145 bool updateModule() override
2146 {
2147 if (doSaveTweakFile)
2148 {
2149 lock_guard<mutex> lg(mutexTweak);
2150 saveTweakFile();
2151 doSaveTweakFile=false;
2152 }
2153
2154 return true;
2155 }
2156};
2157
2158
2159/************************************************************************/
2160int main(int argc, char *argv[])
2161{
2162 ResourceFinder rf;
2163 rf.setDefaultContext("iKinGazeCtrl");
2164 rf.setDefaultConfigFile("config.ini");
2165 rf.configure(argc,argv);
2166
2167 Network yarp;
2168 if (!yarp.checkNetwork())
2169 {
2170 yError("YARP server not available!");
2171 return 1;
2172 }
2173
2174 GazeModule mod;
2175 return mod.runModule(rf);
2176}
nack
@ nack
Definition ST_M1_dataType.h:66
ack
@ ack
Definition ST_M1_dataType.h:65
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::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:1106
GazeModule::tweakSet
bool tweakSet(const Bottle &options)
Definition main.cpp:795
GazeModule::waitPart
PolyDriver * waitPart(const Property &partOpt, const double ping_robot_tmo)
Definition main.cpp:636
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:1303
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:2145
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:2078
GazeModule::rpcPort
RpcServer rpcPort
Definition main.cpp:604
GazeModule::saveTweakFile
void saveTweakFile()
Definition main.cpp:999
GazeModule::mutexTweak
mutex mutexTweak
Definition main.cpp:599
GazeModule::getPeriod
double getPeriod() override
Definition main.cpp:2139
GazeModule::commData
ExchangeData commData
Definition main.cpp:595
GazeModule::interruptModule
bool interruptModule() override
Definition main.cpp:2120
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::drvTorso
PolyDriver * drvTorso
Definition main.cpp:593
GazeModule::close
bool close() override
Definition main.cpp:2132
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::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
GAZECTRL_SERVER_VER
#define GAZECTRL_SERVER_VER
Definition main.cpp:576
main
int main()
Definition main.cpp:67
iCub::ctrl::CTRL_RAD2DEG
constexpr double CTRL_RAD2DEG
180/PI.
Definition math.h:61
iCub::ctrl::CTRL_DEG2RAD
constexpr double CTRL_DEG2RAD
PI/180.
Definition math.h:66
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition DebugInterfaces.h:51
z
z
Definition show_eyes_axes.m:22
GazeModule::Context::saccadesInhibitionPeriod
double saccadesInhibitionPeriod
Definition main.cpp:619
GazeModule::Context::gazeStabilizationOn
bool gazeStabilizationOn
Definition main.cpp:626
GazeModule::Context::eyesBoundVer
double eyesBoundVer
Definition main.cpp:616
GazeModule::Context::neckPitchMin
double neckPitchMin
Definition main.cpp:609
GazeModule::Context::neckPitchMax
double neckPitchMax
Definition main.cpp:610
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
Generated on Fri Sep 26 2025 14:18:33 for iCub-main by doxygen 1.9.8