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 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
1089public:
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/************************************************************************/
2161int 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
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:1107
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: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::mutexTweak
mutex mutexTweak
Definition main.cpp:599
GazeModule::getPeriod
double getPeriod() override
Definition main.cpp:2140
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::drvTorso
PolyDriver * drvTorso
Definition main.cpp:593
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::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 Wed Dec 18 2024 16:34:46 for iCub-main by doxygen 1.9.8