572 #include <yarp/os/all.h>
573 #include <yarp/sig/all.h>
574 #include <yarp/dev/all.h>
575 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
581 #define GAZECTRL_SERVER_VER "2.0"
584 using namespace yarp::os;
586 using namespace yarp::sig;
641 PolyDriver *
waitPart(
const Property &partOpt,
const double ping_robot_tmo)
643 string partName=partOpt.find(
"part").asString();
644 PolyDriver *pDrv=
nullptr;
646 double t0=Time::now();
647 while (Time::now()-t0<ping_robot_tmo)
650 pDrv=
new PolyDriver(
const_cast<Property&
>(partOpt));
651 bool ok=pDrv->isValid();
655 yInfo(
"Checking if %s part is active ... yes",partName.c_str());
660 double dt=ping_robot_tmo-(Time::now()-t0);
661 yInfo(
"Checking if %s part is active ... not yet: still %.1f [s] to timeout expiry",
662 partName.c_str(),dt>0.0?dt:0.0);
664 double t1=Time::now();
665 while (Time::now()-t1<1.0)
679 lock_guard<mutex> lg(mutexContext);
680 Context &context=contextMap[contextIdCnt];
709 lock_guard<mutex> lg(mutexContext);
710 auto itr=contextMap.find(
id);
711 if (itr!=contextMap.end())
744 if (contextIdList!=
nullptr)
746 lock_guard<mutex> lg(mutexContext);
747 for (
int i=0; i<contextIdList->size(); i++)
749 int id=contextIdList->get(i).asInt32();
750 auto itr=contextMap.find(
id);
751 if (itr!=contextMap.end())
752 contextMap.erase(itr);
766 Bottle &serverVer=
info.addList();
767 serverVer.addString(
"server_version");
770 Bottle &headVer=
info.addList();
771 headVer.addString(
"head_version");
774 Bottle &minVer=
info.addList();
775 minVer.addString(
"min_allowed_vergence");
778 Bottle &events=
info.addList();
779 events.addString(
"events");
780 Bottle &eventsList=events.addList();
781 eventsList.addString(
"motion-onset");
782 eventsList.addString(
"motion-done");
783 eventsList.addString(
"motion-ongoing");
784 eventsList.addString(
"saccade-onset");
785 eventsList.addString(
"saccade-done");
786 eventsList.addString(
"stabilization-on");
787 eventsList.addString(
"stabilization-off");
788 eventsList.addString(
"closing");
789 eventsList.addString(
"suspended");
790 eventsList.addString(
"resumed");
791 eventsList.addString(
"comm-timeout");
792 eventsList.addString(
"*");
802 lock_guard<mutex> lg(mutexTweak);
803 if (Bottle *pB=options.find(
"camera_intrinsics_left").asList())
805 Matrix Prj=
zeros(3,4);
int w,
h;
809 for (
int i=0; i<pB->size(); i++)
811 Prj(r,c)=pB->get(i).asFloat64();
824 if (Bottle *pB=options.find(
"camera_intrinsics_right").asList())
826 Matrix Prj=
zeros(3,4);
int w,
h;
830 for (
int i=0; i<pB->size(); i++)
832 Prj(r,c)=pB->get(i).asFloat64();
845 if (options.check(
"camera_width_left"))
847 Matrix Prj=
zeros(3,4);
int w,
h;
849 w=options.find(
"camera_width_left").asInt32();
853 if (options.check(
"camera_height_left"))
855 Matrix Prj=
zeros(3,4);
int w,
h;
857 h=options.find(
"camera_height_left").asInt32();
861 if (options.check(
"camera_width_right"))
863 Matrix Prj=
zeros(3,4);
int w,
h;
865 w=options.find(
"camera_width_right").asInt32();
869 if (options.check(
"camera_height_right"))
871 Matrix Prj=
zeros(3,4);
int w,
h;
873 h=options.find(
"camera_height_right").asInt32();
877 bool doMinAllowedVer=
false;
878 if (Bottle *pB=options.find(
"camera_extrinsics_left").asList())
884 for (
int i=0; i<pB->size(); i++)
886 HN(r,c)=pB->get(i).asFloat64();
896 HN(3,0)=HN(3,1)=HN(3,2)=0.0;
904 doMinAllowedVer=
true;
907 if (Bottle *pB=options.find(
"camera_extrinsics_right").asList())
913 for (
int i=0; i<pB->size(); i++)
915 HN(r,c)=pB->get(i).asFloat64();
925 HN(3,0)=HN(3,1)=HN(3,2)=0.0;
933 doMinAllowedVer=
true;
949 lock_guard<mutex> lg(mutexTweak);
950 Bottle &intrinsicsLeft=options.addList();
951 intrinsicsLeft.addString(
"camera_intrinsics_left");
952 Bottle &intrinsicsLeftValues=intrinsicsLeft.addList();
953 Matrix PrjL;
int wL,hL;
955 for (
int r=0; r<PrjL.rows(); r++)
956 for (
int c=0; c<PrjL.cols(); c++)
957 intrinsicsLeftValues.addFloat64(PrjL(r,c));
959 Bottle &widthLeft=options.addList();
960 widthLeft.addString(
"camera_width_left");
961 widthLeft.addInt32(wL);
962 Bottle &heightLeft=options.addList();
963 heightLeft.addString(
"camera_height_left");
964 heightLeft.addInt32(hL);
966 Bottle &intrinsicsRight=options.addList();
967 intrinsicsRight.addString(
"camera_intrinsics_right");
968 Bottle &intrinsicsRightValues=intrinsicsRight.addList();
969 Matrix PrjR;
int wR,hR;
971 for (
int r=0; r<PrjR.rows(); r++)
972 for (
int c=0; c<PrjR.cols(); c++)
973 intrinsicsRightValues.addFloat64(PrjR(r,c));
975 Bottle &widthRight=options.addList();
976 widthRight.addString(
"camera_width_right");
977 widthRight.addInt32(wR);
978 Bottle &heightRight=options.addList();
979 heightRight.addString(
"camera_height_right");
980 heightRight.addInt32(hR);
982 Bottle &extrinsicsLeft=options.addList();
983 extrinsicsLeft.addString(
"camera_extrinsics_left");
984 Bottle &extrinsicsLeftValues=extrinsicsLeft.addList();
987 for (
int r=0; r<HL.rows(); r++)
988 for (
int c=0; c<HL.cols(); c++)
989 extrinsicsLeftValues.addFloat64(HL(r,c));
991 Bottle &extrinsicsRight=options.addList();
992 extrinsicsRight.addString(
"camera_extrinsics_right");
993 Bottle &extrinsicsRightValues=extrinsicsRight.addList();
996 for (
int r=0; r<HR.rows(); r++)
997 for (
int c=0; c<HR.cols(); c++)
998 extrinsicsRightValues.addFloat64(HR(r,c));
1006 Matrix PrjL;
int wL,hL;
1009 Matrix PrjR;
int wR,hR;
1019 string tweakFile=rf->getHomeContextPath();
1021 fout.open(tweakFile.c_str());
1024 if (validIntrinsicsL)
1026 fout<<
"[CAMERA_CALIBRATION_LEFT]"<<endl;
1027 fout<<
"w "<<wL<<endl;
1028 fout<<
"h "<<hL<<endl;
1029 fout<<
"fx "<<PrjL(0,0)<<endl;
1030 fout<<
"fy "<<PrjL(1,1)<<endl;
1031 fout<<
"cx "<<PrjL(0,2)<<endl;
1032 fout<<
"cy "<<PrjL(1,2)<<endl;
1036 if (validIntrinsicsR)
1038 fout<<
"[CAMERA_CALIBRATION_RIGHT]"<<endl;
1039 fout<<
"w "<<wR<<endl;
1040 fout<<
"h "<<hR<<endl;
1041 fout<<
"fx "<<PrjR(0,0)<<endl;
1042 fout<<
"fy "<<PrjR(1,1)<<endl;
1043 fout<<
"cx "<<PrjR(0,2)<<endl;
1044 fout<<
"cy "<<PrjR(1,2)<<endl;
1049 fout<<
"[ALIGN_KIN_LEFT]"<<endl;
1051 for (
int r=0; r<HNL.rows(); r++)
1052 for (
int c=0; c<HNL.cols(); c++)
1053 fout<<HNL(r,c)<<((r==HNL.rows()-1)&&(c==HNL.cols()-1)?
"":
" ");
1057 fout<<
"[ALIGN_KIN_RIGHT]"<<endl;
1059 for (
int r=0; r<HNR.rows(); r++)
1060 for (
int c=0; c<HNR.cols(); c++)
1061 fout<<HNR(r,c)<<((r==HNR.rows()-1)&&(c==HNR.cols()-1)?
"":
" ");
1073 map<iKinLimbVersion,iKinLimbVersion> d;
1084 auto ver_out=d.begin()->second;
1085 if (ver_out!=ver_in)
1087 yWarning(
"Unknown \"head_version\" %s requested => used \"head_version\" %s instead",
1088 ver_in.
get_version().c_str(),ver_out.get_version().c_str());
1099 eyesRefGen{nullptr},
1104 interrupting{false},
1105 doSaveTweakFile{false},
1120 double ping_robot_tmo;
1121 Vector counterRotGain(2);
1127 Bottle &imuGroup=rf.findGroup(
"imu");
1128 Bottle &eyeTiltGroup=rf.findGroup(
"eye_tilt");
1129 Bottle &trajTimeGroup=rf.findGroup(
"trajectory_time");
1130 Bottle &camerasGroup=rf.findGroup(
"cameras");
1131 Bottle &tweakGroup=rf.findGroup(
"tweak");
1134 ctrlName=rf.check(
"name",Value(
"iKinGazeCtrl")).asString();
1135 headName=rf.check(
"head",Value(
"head")).asString();
1136 torsoName=rf.check(
"torso",Value(
"torso")).asString();
1137 neckTime=trajTimeGroup.check(
"neck",Value(0.75)).asFloat64();
1138 eyesTime=trajTimeGroup.check(
"eyes",Value(0.25)).asFloat64();
1139 min_abs_vel=
CTRL_DEG2RAD*fabs(rf.check(
"min_abs_vel",Value(0.0)).asFloat64());
1140 ping_robot_tmo=rf.check(
"ping_robot_tmo",Value(40.0)).asFloat64();
1142 auto head_version=rf.check(
"head_version",Value(
"v1.0")).asString();
1143 if ((head_version.length()<2) || (tolower(head_version[0])!=
'v'))
1145 yWarning(
"Unrecognized \"head_version\" %s; going with default version",head_version.c_str());
1146 head_version=
"v1.0";
1150 commData.
robotName=rf.check(
"robot",Value(
"icub")).asString();
1151 commData.
eyeTiltLim[0]=eyeTiltGroup.check(
"min",Value(-20.0)).asFloat64();
1152 commData.
eyeTiltLim[1]=eyeTiltGroup.check(
"max",Value(15.0)).asFloat64();
1153 commData.
verbose=rf.check(
"verbose");
1154 commData.
saccadesOn=(rf.check(
"saccades",Value(
"on")).asString()==
"on");
1155 commData.
neckPosCtrlOn=(rf.check(
"neck_position_control",Value(
"on")).asString()==
"on");
1156 commData.
stabilizationOn=(imuGroup.check(
"mode",Value(
"on")).asString()==
"on");
1157 commData.
stabilizationGain=imuGroup.check(
"stabilization_gain",Value(11.0)).asFloat64();
1159 commData.
debugInfoEnabled=rf.check(
"debugInfo",Value(
"off")).asString()==
"on";
1163 counterRotGain[0]=imuGroup.check(
"vor",Value(1.0)).asFloat64();
1164 counterRotGain[1]=rf.check(
"ocr",Value(0.0)).asFloat64();
1168 counterRotGain[0]=imuGroup.check(
"vor",Value(0.0)).asFloat64();
1169 counterRotGain[1]=rf.check(
"ocr",Value(1.0)).asFloat64();
1172 if (camerasGroup.check(
"file"))
1174 camerasGroup.check(
"context")?
1175 commData.
rf_cameras.setDefaultContext(camerasGroup.find(
"context").asString()):
1176 commData.
rf_cameras.setDefaultContext(rf.getContext());
1177 commData.
rf_cameras.setDefaultConfigFile(camerasGroup.find(
"file").asString().c_str());
1181 commData.
rf_tweak.setDefaultContext(rf.getContext());
1182 commData.
tweakFile=tweakGroup.check(
"file",Value(
"tweak.ini")).asString();
1183 commData.
tweakOverwrite=(tweakGroup.check(
"overwrite",Value(
"on")).asString()==
"on");
1185 commData.
rf_tweak.configure(0,
nullptr);
1190 string remoteHeadName=
"/"+commData.
robotName+
"/"+headName;
1192 string remoteTorsoName=
"/"+commData.
robotName+
"/"+torsoName;
1194 string remoteInertialName=
"/"+commData.
robotName+
"/head/inertials";
1195 string localInertialName=commData.
localStemName+
"/head/inertials";
1197 float imuTimeout {0.04F};
1199 if (imuGroup.check(
"source_port_name"))
1200 remoteInertialName=imuGroup.find(
"source_port_name").asString();
1201 if (imuGroup.check(
"timeout"))
1202 imuTimeout=imuGroup.find(
"timeout").asFloat32();
1204 Property optTorso(
"(device remote_controlboard)");
1205 optTorso.put(
"remote",remoteTorsoName);
1206 optTorso.put(
"local",localTorsoName);
1207 optTorso.put(
"part",torsoName);
1209 Property optHead(
"(device remote_controlboard)");
1210 optHead.put(
"remote",remoteHeadName);
1211 optHead.put(
"local",localHeadName);
1212 optHead.put(
"part",headName);
1215 optHead.put(
"writeStrict",
"on");
1217 if (torsoName!=
"off")
1219 drvTorso=(ping_robot_tmo>0.0)?
1220 waitPart(optTorso,ping_robot_tmo):
1221 new PolyDriver(optTorso);
1223 if (!drvTorso->isValid())
1225 yWarning(
"Torso device driver not available!");
1226 yWarning(
"Perhaps only the head is running; trying to continue ...");
1234 yWarning(
"Torso device is off!");
1238 drvHead=(ping_robot_tmo>0.0)?
1239 waitPart(optHead,ping_robot_tmo):
1240 new PolyDriver(optHead);
1242 if (!drvHead->isValid())
1244 yError(
"Head device driver not available!");
1250 Property mas_conf{{
"device", Value(
"multipleanalogsensorsclient")},
1251 {
"remote", Value(remoteInertialName)},
1252 {
"local", Value(localInertialName)},
1253 {
"timeout",Value(imuTimeout)}};
1255 if (!(mas_client.open(mas_conf)))
1257 yError(
"Unable to open the MAS client");
1262 if (!(mas_client.view(iGyro)) ||
1263 !(mas_client.view(iAccel))) {
1265 yError(
"View failed of the MAS interfaces");
1270 commData.
iGyro = iGyro;
1271 commData.
iAccel = iAccel;
1274 yWarning(
"IMU data will be not received/used");
1278 yDebug(
"Commands to robot will be also streamed out on debug port");
1282 ctrl=
new Controller(drvTorso,drvHead,&commData,neckTime,eyesTime,min_abs_vel,10);
1284 eyesRefGen=
new EyePinvRefGen(drvTorso,drvHead,&commData,ctrl,counterRotGain,20);
1285 slv=
new Solver(drvTorso,drvHead,&commData,eyesRefGen,loc,ctrl,20);
1291 eyesRefGen->start();
1309 bool respond(
const Bottle &command, Bottle &reply)
override
1311 int ack=Vocab32::encode(
"ack");
1312 int nack=Vocab32::encode(
"nack");
1314 if (command.size()>0)
1316 switch (command.get(0).asVocab32())
1319 case createVocab32(
'g',
'e',
't'):
1321 if (command.size()>1)
1323 int type=command.get(1).asVocab32();
1325 if (type==createVocab32(
'T',
'n',
'e',
'c'))
1327 reply.addVocab32(
ack);
1328 reply.addFloat64(ctrl->
getTneck());
1331 else if (type==createVocab32(
'T',
'e',
'y',
'e'))
1333 reply.addVocab32(
ack);
1334 reply.addFloat64(ctrl->
getTeyes());
1337 else if (type==createVocab32(
'v',
'o',
'r'))
1340 reply.addVocab32(
ack);
1341 reply.addFloat64(gain[0]);
1344 else if (type==createVocab32(
'o',
'c',
'r'))
1347 reply.addVocab32(
ack);
1348 reply.addFloat64(gain[1]);
1351 else if (type==createVocab32(
's',
'a',
'c',
'c'))
1353 reply.addVocab32(
ack);
1357 else if (type==createVocab32(
's',
'i',
'n',
'h'))
1359 reply.addVocab32(
ack);
1363 else if (type==createVocab32(
's',
'a',
'c',
't'))
1365 reply.addVocab32(
ack);
1369 else if (type==createVocab32(
't',
'r',
'a',
'c'))
1371 reply.addVocab32(
ack);
1375 else if (type==createVocab32(
's',
't',
'a',
'b'))
1377 reply.addVocab32(
ack);
1381 else if (type==createVocab32(
'd',
'o',
'n',
'e'))
1383 reply.addVocab32(
ack);
1387 else if (type==createVocab32(
's',
'd',
'o',
'n'))
1389 reply.addVocab32(
ack);
1393 else if (type==createVocab32(
'p',
'i',
't',
'c'))
1395 double min_deg,max_deg;
1398 reply.addVocab32(
ack);
1399 reply.addFloat64(min_deg);
1400 reply.addFloat64(max_deg);
1403 else if (type==createVocab32(
'r',
'o',
'l',
'l'))
1405 double min_deg,max_deg;
1408 reply.addVocab32(
ack);
1409 reply.addFloat64(min_deg);
1410 reply.addFloat64(max_deg);
1413 else if (type==createVocab32(
'y',
'a',
'w'))
1415 double min_deg,max_deg;
1418 reply.addVocab32(
ack);
1419 reply.addFloat64(min_deg);
1420 reply.addFloat64(max_deg);
1423 else if (type==createVocab32(
'e',
'y',
'e',
's'))
1425 reply.addVocab32(
ack);
1429 else if (type==createVocab32(
'n',
't',
'o',
'l'))
1433 reply.addVocab32(
ack);
1434 reply.addFloat64(angle);
1437 else if (type==createVocab32(
'd',
'e',
's'))
1442 reply.addVocab32(
ack);
1443 reply.addList().read(des);
1447 else if (type==createVocab32(
'v',
'e',
'l'))
1452 reply.addVocab32(
ack);
1453 reply.addList().read(vel);
1457 else if ((type==createVocab32(
'p',
'o',
's',
'e')) && (command.size()>2))
1459 string poseSel=command.get(2).asString();
1463 if (ctrl->
getPose(poseSel,
x,stamp))
1465 reply.addVocab32(
ack);
1466 reply.addList().read(
x);
1468 Bottle &bStamp=reply.addList();
1469 bStamp.addInt32(stamp.getCount());
1470 bStamp.addFloat64(stamp.getTime());
1475 else if ((type==createVocab32(
'2',
'D')) && (command.size()>2))
1477 if (Bottle *bOpt=command.get(2).asList())
1482 string eye=bOpt->get(0).asString();
1483 x[0]=bOpt->get(1).asFloat64();
1484 x[1]=bOpt->get(2).asFloat64();
1485 x[2]=bOpt->get(3).asFloat64();
1490 reply.addVocab32(
ack);
1491 reply.addList().read(px);
1497 else if ((type==createVocab32(
'3',
'D')) && (command.size()>3))
1499 int subType=command.get(2).asVocab32();
1500 if (subType==createVocab32(
'm',
'o',
'n',
'o'))
1502 if (Bottle *bOpt=command.get(3).asList())
1506 string eye=bOpt->get(0).asString();
1507 double u=bOpt->get(1).asFloat64();
1508 double v=bOpt->get(2).asFloat64();
1509 double z=bOpt->get(3).asFloat64();
1514 reply.addVocab32(
ack);
1515 reply.addList().read(
x);
1521 else if (subType==createVocab32(
's',
't',
'e',
'r'))
1523 if (Bottle *bOpt=command.get(3).asList())
1527 Vector pxl(2),pxr(2);
1528 pxl[0]=bOpt->get(0).asFloat64();
1529 pxl[1]=bOpt->get(1).asFloat64();
1530 pxr[0]=bOpt->get(2).asFloat64();
1531 pxr[1]=bOpt->get(3).asFloat64();
1536 reply.addVocab32(
ack);
1537 reply.addList().read(
x);
1543 else if (subType==createVocab32(
'p',
'r',
'o',
'j'))
1545 if (Bottle *bOpt=command.get(3).asList())
1550 string eye=bOpt->get(0).asString();
1551 double u=bOpt->get(1).asFloat64();
1552 double v=bOpt->get(2).asFloat64();
1553 plane[0]=bOpt->get(3).asFloat64();
1554 plane[1]=bOpt->get(4).asFloat64();
1555 plane[2]=bOpt->get(5).asFloat64();
1556 plane[3]=bOpt->get(6).asFloat64();
1561 reply.addVocab32(
ack);
1562 reply.addList().read(
x);
1568 else if (subType==createVocab32(
'a',
'n',
'g'))
1570 if (Bottle *bOpt=command.get(3).asList())
1575 string type=bOpt->get(0).asString();
1581 reply.addVocab32(
ack);
1582 reply.addList().read(
x);
1588 else if ((type==createVocab32(
'a',
'n',
'g')) && (command.size()>2))
1590 if (Bottle *bOpt=command.get(2).asList())
1595 x[0]=bOpt->get(0).asFloat64();
1596 x[1]=bOpt->get(1).asFloat64();
1597 x[2]=bOpt->get(2).asFloat64();
1600 reply.addVocab32(
ack);
1601 reply.addList().read(ang);
1606 else if (type==createVocab32(
'p',
'i',
'd'))
1611 reply.addVocab32(
ack);
1612 reply.addList()=options;
1615 else if (type==createVocab32(
'i',
'n',
'f',
'o'))
1620 reply.addVocab32(
ack);
1621 reply.addList()=
info;
1625 else if (type==createVocab32(
't',
'w',
'e',
'a'))
1628 if (tweakGet(options))
1630 reply.addVocab32(
ack);
1631 reply.addList()=options;
1641 case createVocab32(
's',
'e',
't'):
1643 if (command.size()>2)
1645 int type=command.get(1).asVocab32();
1646 if (type==createVocab32(
'T',
'n',
'e',
'c'))
1648 double execTime=command.get(2).asFloat64();
1650 reply.addVocab32(
ack);
1653 else if (type==createVocab32(
'T',
'e',
'y',
'e'))
1655 double execTime=command.get(2).asFloat64();
1657 reply.addVocab32(
ack);
1660 else if (type==createVocab32(
'v',
'o',
'r'))
1663 gain[0]=command.get(2).asFloat64();
1665 reply.addVocab32(
ack);
1668 else if (type==createVocab32(
'o',
'c',
'r'))
1671 gain[1]=command.get(2).asFloat64();
1673 reply.addVocab32(
ack);
1676 else if (type==createVocab32(
's',
'a',
'c',
'c'))
1678 commData.
saccadesOn=(command.get(2).asInt32()>0);
1679 reply.addVocab32(
ack);
1682 else if (type==createVocab32(
's',
'i',
'n',
'h'))
1684 double period=command.get(2).asFloat64();
1686 reply.addVocab32(
ack);
1689 else if (type==createVocab32(
's',
'a',
'c',
't'))
1691 double angle=command.get(2).asFloat64();
1693 reply.addVocab32(
ack);
1696 else if (type==createVocab32(
'n',
't',
'o',
'l'))
1698 double angle=command.get(2).asFloat64();
1700 reply.addVocab32(
ack);
1703 else if (type==createVocab32(
't',
'r',
'a',
'c'))
1705 bool mode=(command.get(2).asInt32()>0);
1707 reply.addVocab32(
ack);
1710 else if (type==createVocab32(
's',
't',
'a',
'b'))
1712 bool mode=(command.get(2).asInt32()>0);
1716 else if (type==createVocab32(
'p',
'i',
'd'))
1718 if (Bottle *bOpt=command.get(2).asList())
1721 reply.addVocab32(
ack);
1725 else if (type==createVocab32(
't',
'w',
'e',
'a'))
1727 if (Bottle *bOpt=command.get(2).asList())
1729 reply.addVocab32(tweakSet(*bOpt)?
ack:
nack);
1739 case createVocab32(
'l',
'o',
'o',
'k'):
1741 if (command.size()>2)
1743 int type=command.get(1).asVocab32();
1744 if (type==createVocab32(
'3',
'D'))
1746 if (Bottle *bOpt=command.get(2).asList())
1751 x[0]=bOpt->get(0).asFloat64();
1752 x[1]=bOpt->get(1).asFloat64();
1753 x[2]=bOpt->get(2).asFloat64();
1757 reply.addVocab32(
ack);
1763 else if (type==createVocab32(
'm',
'o',
'n',
'o'))
1765 if (Bottle *bOpt=command.get(2).asList())
1769 string eye=bOpt->get(0).asString();
1770 double u=bOpt->get(1).asFloat64();
1771 double v=bOpt->get(2).asFloat64();
1775 if (bOpt->get(3).isFloat64())
1777 z=bOpt->get(3).asFloat64();
1780 else if ((bOpt->get(3).asString()==
"ver") && (bOpt->size()>4))
1782 double ver=bOpt->get(4).asFloat64();
1794 reply.addVocab32(
ack);
1802 else if (type==createVocab32(
's',
't',
'e',
'r'))
1804 if (Bottle *bOpt=command.get(2).asList())
1808 Vector pxl(2),pxr(2);
1809 pxl[0]=bOpt->get(0).asFloat64();
1810 pxl[1]=bOpt->get(1).asFloat64();
1811 pxr[0]=bOpt->get(2).asFloat64();
1812 pxr[1]=bOpt->get(3).asFloat64();
1819 reply.addVocab32(
ack);
1826 else if (type==createVocab32(
'a',
'n',
'g'))
1828 if (Bottle *bOpt=command.get(2).asList())
1833 string type=bOpt->get(0).asString();
1841 reply.addVocab32(
ack);
1853 case createVocab32(
's',
't',
'o',
'p'):
1856 reply.addVocab32(
ack);
1861 case createVocab32(
's',
't',
'o',
'r'):
1865 reply.addVocab32(
ack);
1871 case createVocab32(
'r',
'e',
's',
't'):
1873 if (command.size()>1)
1875 int id=command.get(1).asInt32();
1876 if (restoreContext(
id))
1878 reply.addVocab32(
ack);
1887 case createVocab32(
'd',
'e',
'l'):
1889 if (command.size()>1)
1891 Bottle *ids=command.get(1).asList();
1892 if (deleteContexts(ids))
1894 reply.addVocab32(
ack);
1903 case createVocab32(
'b',
'i',
'n',
'd'):
1905 if (command.size()>2)
1907 int joint=command.get(1).asVocab32();
1908 if (joint==createVocab32(
'p',
'i',
't',
'c'))
1910 double min=command.get(2).asFloat64();
1911 double max=command.get(3).asFloat64();
1913 reply.addVocab32(
ack);
1916 else if (joint==createVocab32(
'r',
'o',
'l',
'l'))
1918 double min=command.get(2).asFloat64();
1919 double max=command.get(3).asFloat64();
1921 reply.addVocab32(
ack);
1924 else if (joint==createVocab32(
'y',
'a',
'w'))
1926 double min=command.get(2).asFloat64();
1927 double max=command.get(3).asFloat64();
1929 reply.addVocab32(
ack);
1932 else if (joint==createVocab32(
'e',
'y',
'e',
's'))
1934 double ver=command.get(2).asFloat64();
1936 reply.addVocab32(
ack);
1945 case createVocab32(
'c',
'l',
'e',
'a'):
1947 if (command.size()>1)
1949 int joint=command.get(1).asVocab32();
1950 if (joint==createVocab32(
'p',
'i',
't',
'c'))
1953 reply.addVocab32(
ack);
1956 else if (joint==createVocab32(
'r',
'o',
'l',
'l'))
1959 reply.addVocab32(
ack);
1962 else if (joint==createVocab32(
'y',
'a',
'w'))
1965 reply.addVocab32(
ack);
1968 else if (joint==createVocab32(
'e',
'y',
'e',
's'))
1971 reply.addVocab32(
ack);
1980 case createVocab32(
'r',
'e',
'g',
'i'):
1982 if (command.size()>1)
1984 int type=command.get(1).asVocab32();
1985 if (type==createVocab32(
'o',
'n',
'g',
'o'))
1987 if (command.size()>2)
1989 double checkPoint=command.get(2).asFloat64();
1992 reply.addVocab32(
ack);
2003 case createVocab32(
'u',
'n',
'r',
'e'):
2005 if (command.size()>1)
2007 int type=command.get(1).asVocab32();
2008 if (type==createVocab32(
'o',
'n',
'g',
'o'))
2010 if (command.size()>2)
2012 double checkPoint=command.get(2).asFloat64();
2015 reply.addVocab32(
ack);
2026 case createVocab32(
'l',
'i',
's',
't'):
2028 if (command.size()>1)
2030 int type=command.get(1).asVocab32();
2031 if (type==createVocab32(
'o',
'n',
'g',
'o'))
2033 reply.addVocab32(
ack);
2043 case createVocab32(
's',
'u',
's',
'p'):
2048 reply.addVocab32(
ack);
2053 case createVocab32(
'r',
'u',
'n'):
2058 reply.addVocab32(
ack);
2063 case createVocab32(
's',
't',
'a',
't'):
2065 reply.addVocab32(
ack);
2066 if (ctrl->isSuspended())
2067 reply.addString(
"suspended");
2069 reply.addString(
"running");
2075 return RFModule::respond(command,reply);
2079 reply.addVocab32(
nack);
2089 if (eyesRefGen!=
nullptr)
2098 if (drvTorso!=
nullptr)
2101 if (drvHead!=
nullptr)
2104 if (mas_client.isValid())
2107 if (commData.
port_xd!=
nullptr)
2108 if (!commData.
port_xd->isClosed())
2110 if (rpcPort.asPort().isOpen())
2130 if (commData.
port_xd!=
nullptr)
2131 commData.
port_xd->interrupt();
2132 rpcPort.interrupt();
2153 if (doSaveTweakFile)
2155 lock_guard<mutex> lg(mutexTweak);
2157 doSaveTweakFile=
false;
2169 rf.setDefaultContext(
"iKinGazeCtrl");
2170 rf.setDefaultConfigFile(
"config.ini");
2171 rf.configure(
argc,argv);
2174 if (!
yarp.checkNetwork())
2176 yError(
"YARP server not available!");
2181 return mod.runModule(rf);
bool unregisterMotionOngoingEvent(const double checkPoint)
bool setGazeStabilization(const bool f)
bool getPose(const string &poseSel, Vector &x, Stamp &stamp)
void setTeyes(const double execTime)
bool look(const Vector &x)
void minAllowedVergenceChanged() override
void setTneck(const double execTime)
bool getVelocity(Vector &vel)
bool getDesired(Vector &des)
void findMinimumAllowedVergence()
void setTrackingMode(const bool f)
bool getGazeStabilization() const
bool getTrackingMode() const
Bottle listMotionOngoingEvents()
bool registerMotionOngoingEvent(const double checkPoint)
ResourceFinder rf_cameras
double saccadesActivationAngle
IThreeAxisLinearAccelerometers * iAccel
double gyro_noise_threshold
double saccadesInhibitionPeriod
double minAllowedVergence
IThreeAxisGyroscopes * iGyro
iKinLimbVersion head_version
void minAllowedVergenceChanged() override
Vector getCounterRotGain()
void manageBindEyes(const double ver)
bool bindEyes(const double ver)
void setCounterRotGain(const Vector &gain)
virtual bool getExtrinsicsMatrix(const string &type, Matrix &M)
virtual bool setExtrinsicsMatrix(const string &type, const Matrix &M)
bool restoreContext(const int id)
void storeContext(int *id)
bool configure(ResourceFinder &rf)
bool tweakSet(const Bottle &options)
EyePinvRefGen * eyesRefGen
bool getInfo(Bottle &info)
bool respond(const Bottle &command, Bottle &reply) override
bool deleteContexts(Bottle *contextIdList)
bool updateModule() override
IThreeAxisLinearAccelerometers * iAccel
std::map< int, Context > contextMap
iKinLimbVersion constrainHeadVersion(const iKinLimbVersion &ver_in)
PolyDriver * waitPart(const Property &partOpt, const double ping_robot_tmo)
double getPeriod() override
bool interruptModule() override
IThreeAxisGyroscopes * iGyro
bool tweakGet(Bottle &options)
void getPidOptions(Bottle &options)
bool setIntrinsicsMatrix(const string &type, const Matrix &M, const int w, const int h)
double getDistFromVergence(const double ver)
Vector get3DPoint(const string &type, const Vector &ang)
void setPidOptions(const Bottle &options)
bool getIntrinsicsMatrix(const string &type, Matrix &M, int &w, int &h)
bool triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
bool projectPoint(const string &type, const Vector &x, Vector &px)
Vector getAbsAngles(const Vector &x)
void bindNeckPitch(const double min_deg, const double max_deg)
void getCurNeckYawRange(double &min_deg, double &max_deg)
void getCurNeckPitchRange(double &min_deg, double &max_deg)
double getNeckAngleUserTolerance() const
void bindNeckYaw(const double min_deg, const double max_deg)
void setNeckAngleUserTolerance(const double angle)
void getCurNeckRollRange(double &min_deg, double &max_deg)
void bindNeckRoll(const double min_deg, const double max_deg)
A class for defining the versions of the iCub limbs.
std::string get_version() const
Return the version string.
int main(int argc, char *argv[])
#define GAZECTRL_SERVER_VER
Copyright (C) 2008 RobotCub Consortium.
double saccadesInhibitionPeriod
double saccadesActivationAngle
double neckAngleUserTolerance