iCub-main
ClientGazeController.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
4  * email: ugo.pattacini@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 
19 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
20 // Developed by Ugo Pattacini
21 
22 #include <algorithm>
23 #include <sstream>
24 
25 #include <yarp/math/Math.h>
26 #include "ClientGazeController.h"
27 
28 #define GAZECTRL_CLIENT_VER "2.0"
29 #define GAZECTRL_DEFAULT_TMO 0.1 // [s]
30 #define GAZECTRL_ACK Vocab32::encode("ack")
31 #define GAZECTRL_NACK Vocab32::encode("nack")
32 
33 using namespace std;
34 using namespace yarp::os;
35 using namespace yarp::dev;
36 using namespace yarp::sig;
37 using namespace yarp::math;
38 
39 
40 /************************************************************************/
41 void GazeEventHandler::onRead(Bottle &event)
42 {
43  if (interface!=NULL)
44  interface->eventHandling(event);
45 }
46 
47 
48 /************************************************************************/
50 {
51  this->interface=interface;
52  setStrict();
53  useCallback();
54 }
55 
56 
57 /************************************************************************/
59 {
60  init();
61 }
62 
63 
64 /************************************************************************/
66 {
67  init();
68  open(config);
69 }
70 
71 
72 /************************************************************************/
74 {
75  connected=false;
76  closed=true;
77 
78  timeout=GAZECTRL_DEFAULT_TMO;
79  lastFpMsgArrivalTime=0.0;
80  lastAngMsgArrivalTime=0.0;
81 
82  fixationPoint.resize(3,0.0);
83  angles.resize(3,0.0);
84 
85  portEvents.setInterface(this);
86 }
87 
88 
89 /************************************************************************/
90 bool ClientGazeController::open(Searchable &config)
91 {
92  string remote, local, carrier;
93 
94  if (config.check("remote"))
95  remote=config.find("remote").asString();
96  else
97  return false;
98 
99  if (config.check("local"))
100  local=config.find("local").asString();
101  else
102  return false;
103 
104  closed=false;
105  carrier=config.check("carrier",Value("udp")).asString();
106 
107  if (config.check("timeout"))
108  timeout=config.find("timeout").asFloat64();
109 
110  portCmdFp.open(local+"/xd:o");
111  portCmdAng.open(local+"/angles:o");
112  portCmdMono.open(local+"/mono:o");
113  portCmdStereo.open(local+"/stereo:o");
114  portStateFp.open(local+"/x:i");
115  portStateAng.open(local+"/angles:i");
116  portStateHead.open(local+"/q:i");
117  portEvents.open(local+"/events:i");
118  portRpc.open(local+"/rpc");
119 
120  bool ok=true;
121  ok&=Network::connect(portRpc.getName(),remote+"/rpc");
122  if (ok)
123  {
124  Bottle info;
125  getInfoHelper(info);
126  if (info.check("server_version"))
127  {
128  string server_version=info.find("server_version").asString();
129  if (server_version!=GAZECTRL_CLIENT_VER)
130  {
131  yError("version mismatch => server(%s) != client(%s); please update accordingly",
132  server_version.c_str(),GAZECTRL_CLIENT_VER);
133  close();
134  return false;
135  }
136  }
137  else
138  yWarning("unable to retrieve server version; please update the server");
139  }
140  else
141  {
142  yError("unable to connect to the server rpc port!");
143  close();
144  return false;
145  }
146 
147  ok&=Network::connect(portCmdFp.getName(),remote+"/xd:i",carrier);
148  ok&=Network::connect(portCmdAng.getName(),remote+"/angles:i",carrier);
149  ok&=Network::connect(portCmdMono.getName(),remote+"/mono:i",carrier);
150  ok&=Network::connect(portCmdStereo.getName(),remote+"/stereo:i",carrier);
151  ok&=Network::connect(remote+"/x:o",portStateFp.getName(),carrier);
152  ok&=Network::connect(remote+"/angles:o",portStateAng.getName(),carrier);
153  ok&=Network::connect(remote+"/q:o",portStateHead.getName(),carrier);
154  ok&=Network::connect(remote+"/events:o",portEvents.getName(),carrier);
155 
156  return connected=ok;
157 }
158 
159 
160 /************************************************************************/
162 {
163  if (closed)
164  return true;
165 
166  deleteContexts();
167 
168  while (eventsMap.size()>0)
169  unregisterEvent(*eventsMap.begin()->second);
170 
171  portCmdFp.interrupt();
172  portCmdAng.interrupt();
173  portCmdMono.interrupt();
174  portCmdStereo.interrupt();
175  portStateFp.interrupt();
176  portStateAng.interrupt();
177  portStateHead.interrupt();
178  portEvents.interrupt();
179  portRpc.interrupt();
180 
181  portCmdFp.close();
182  portCmdAng.close();
183  portCmdMono.close();
184  portCmdStereo.close();
185  portStateFp.close();
186  portStateAng.close();
187  portStateHead.close();
188  portEvents.close();
189  portRpc.close();
190 
191  connected=false;
192  return closed=true;
193 }
194 
195 
196 /************************************************************************/
198 {
199  if (!connected)
200  return false;
201 
202  Bottle command, reply;
203  command.addString("set");
204  command.addString("track");
205  command.addInt32((int)f);
206 
207  if (!portRpc.write(command,reply))
208  {
209  yError("unable to get reply from server!");
210  return false;
211  }
212 
213  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
214 }
215 
216 
217 /************************************************************************/
219 {
220  if (!connected || (f==NULL))
221  return false;
222 
223  Bottle command, reply;
224  command.addString("get");
225  command.addString("track");
226 
227  if (!portRpc.write(command,reply))
228  {
229  yError("unable to get reply from server!");
230  return false;
231  }
232 
233  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
234  {
235  *f=(reply.get(1).asInt32()>0);
236  return true;
237  }
238 
239  return false;
240 }
241 
242 
243 /************************************************************************/
245 {
246  if (!connected)
247  return false;
248 
249  Bottle command, reply;
250  command.addString("set");
251  command.addString("stab");
252  command.addInt32((int)f);
253 
254  if (!portRpc.write(command,reply))
255  {
256  yError("unable to get reply from server!");
257  return false;
258  }
259 
260  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
261 }
262 
263 
264 /************************************************************************/
266 {
267  if (!connected || (f==NULL))
268  return false;
269 
270  Bottle command, reply;
271  command.addString("get");
272  command.addString("stab");
273 
274  if (!portRpc.write(command,reply))
275  {
276  yError("unable to get reply from server!");
277  return false;
278  }
279 
280  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
281  {
282  *f=(reply.get(1).asInt32()>0);
283  return true;
284  }
285 
286  return false;
287 }
288 
289 
290 /************************************************************************/
291 bool ClientGazeController::getFixationPoint(Vector &fp, Stamp *stamp)
292 {
293  if (!connected)
294  return false;
295 
296  double now=Time::now();
297  if (Vector *v=portStateFp.read(false))
298  {
299  fixationPoint=*v;
300  portStateFp.getEnvelope(fpStamp);
301  lastFpMsgArrivalTime=now;
302  }
303 
304  fp=fixationPoint;
305  if (stamp!=NULL)
306  *stamp=fpStamp;
307 
308  return (now-lastFpMsgArrivalTime<timeout);
309 }
310 
311 
312 /************************************************************************/
313 bool ClientGazeController::getAngles(Vector &ang, Stamp *stamp)
314 {
315  if (!connected)
316  return false;
317 
318  double now=Time::now();
319  if (Vector *v=portStateAng.read(false))
320  {
321  angles=*v;
322  portStateAng.getEnvelope(anglesStamp);
323  lastAngMsgArrivalTime=now;
324  }
325 
326  ang=angles;
327  if (stamp!=NULL)
328  *stamp=anglesStamp;
329 
330  return (now-lastAngMsgArrivalTime<timeout);
331 }
332 
333 
334 /************************************************************************/
336 {
337  if (!connected || (fp.length()<3))
338  return false;
339 
340  Bottle &cmd=portCmdFp.prepare();
341  cmd.clear();
342 
343  cmd.addFloat64(fp[0]);
344  cmd.addFloat64(fp[1]);
345  cmd.addFloat64(fp[2]);
346 
347  portCmdFp.writeStrict();
348  return true;
349 }
350 
351 
352 /************************************************************************/
354 {
355  if (!connected || (ang.length()<3))
356  return false;
357 
358  Bottle &cmd=portCmdAng.prepare();
359  cmd.clear();
360 
361  cmd.addString("abs");
362  cmd.addFloat64(ang[0]);
363  cmd.addFloat64(ang[1]);
364  cmd.addFloat64(ang[2]);
365 
366  portCmdAng.writeStrict();
367  return true;
368 }
369 
370 
371 /************************************************************************/
373 {
374  if (!connected || (ang.length()<3))
375  return false;
376 
377  Bottle &cmd=portCmdAng.prepare();
378  cmd.clear();
379 
380  cmd.addString("rel");
381  cmd.addFloat64(ang[0]);
382  cmd.addFloat64(ang[1]);
383  cmd.addFloat64(ang[2]);
384 
385  portCmdAng.writeStrict();
386  return true;
387 }
388 
389 
390 /************************************************************************/
391 bool ClientGazeController::lookAtMonoPixel(const int camSel, const Vector &px,
392  const double z)
393 {
394  if (!connected || (px.length()<2))
395  return false;
396 
397  Bottle &cmd=portCmdMono.prepare();
398  cmd.clear();
399 
400  cmd.addString((camSel==0)?"left":"right");
401  cmd.addFloat64(px[0]);
402  cmd.addFloat64(px[1]);
403  cmd.addFloat64(z);
404 
405  portCmdMono.writeStrict();
406  return true;
407 }
408 
409 
410 /************************************************************************/
412  const Vector &px,
413  const double ver)
414 {
415  if (!connected || (px.length()<2))
416  return false;
417 
418  Bottle &cmd=portCmdMono.prepare();
419  cmd.clear();
420 
421  cmd.addString((camSel==0)?"left":"right");
422  cmd.addFloat64(px[0]);
423  cmd.addFloat64(px[1]);
424  cmd.addString("ver");
425  cmd.addFloat64(ver);
426 
427  portCmdMono.writeStrict();
428  return true;
429 }
430 
431 
432 /************************************************************************/
433 bool ClientGazeController::lookAtStereoPixels(const Vector &pxl, const Vector &pxr)
434 {
435  if (!connected || (pxl.length()<2) || (pxr.length()<2))
436  return false;
437 
438  Bottle &cmd=portCmdStereo.prepare();
439  cmd.clear();
440 
441  cmd.addFloat64(pxl[0]);
442  cmd.addFloat64(pxl[1]);
443  cmd.addFloat64(pxr[0]);
444  cmd.addFloat64(pxr[1]);
445 
446  portCmdStereo.writeStrict();
447  return true;
448 }
449 
450 
451 /************************************************************************/
453 {
454  if (!connected || (fp.length()<3))
455  return false;
456 
457  Bottle command, reply;
458  command.addString("look");
459  command.addString("3D");
460  Bottle &payLoad=command.addList();
461  payLoad.addFloat64(fp[0]);
462  payLoad.addFloat64(fp[1]);
463  payLoad.addFloat64(fp[2]);
464 
465  if (!portRpc.write(command,reply))
466  {
467  yError("unable to get reply from server!");
468  return false;
469  }
470 
471  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
472 }
473 
474 
475 /************************************************************************/
477 {
478  if (!connected || (ang.length()<3))
479  return false;
480 
481  Bottle command, reply;
482  command.addString("look");
483  command.addString("ang");
484  Bottle &payLoad=command.addList();
485  payLoad.addString("abs");
486  for (size_t i=0; i<ang.length(); i++)
487  payLoad.addFloat64(ang[i]);
488 
489  if (!portRpc.write(command,reply))
490  {
491  yError("unable to get reply from server!");
492  return false;
493  }
494 
495  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
496 }
497 
498 
499 /************************************************************************/
501 {
502  if (!connected || (ang.length()<3))
503  return false;
504 
505  Bottle command, reply;
506  command.addString("look");
507  command.addString("ang");
508  Bottle &payLoad=command.addList();
509  payLoad.addString("rel");
510  for (size_t i=0; i<ang.length(); i++)
511  payLoad.addFloat64(ang[i]);
512 
513  if (!portRpc.write(command,reply))
514  {
515  yError("unable to get reply from server!");
516  return false;
517  }
518 
519  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
520 }
521 
522 
523 /************************************************************************/
525  const Vector &px,
526  const double z)
527 {
528  if (!connected || (px.length()<2))
529  return false;
530 
531  Bottle command, reply;
532  command.addString("look");
533  command.addString("mono");
534  Bottle &payLoad=command.addList();
535  payLoad.addString((camSel==0)?"left":"right");
536  payLoad.addFloat64(px[0]);
537  payLoad.addFloat64(px[1]);
538  payLoad.addFloat64(z);
539 
540  if (!portRpc.write(command,reply))
541  {
542  yError("unable to get reply from server!");
543  return false;
544  }
545 
546  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
547 }
548 
549 
550 /************************************************************************/
552  const Vector &px,
553  const double ver)
554 {
555  if (!connected || (px.length()<2))
556  return false;
557 
558  Bottle command, reply;
559  command.addString("look");
560  command.addString("mono");
561  Bottle &payLoad=command.addList();
562  payLoad.addString((camSel==0)?"left":"right");
563  payLoad.addFloat64(px[0]);
564  payLoad.addFloat64(px[1]);
565  payLoad.addString("ver");
566  payLoad.addFloat64(ver);
567 
568  if (!portRpc.write(command,reply))
569  {
570  yError("unable to get reply from server!");
571  return false;
572  }
573 
574  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
575 }
576 
577 
578 /************************************************************************/
580  const Vector &pxr)
581 {
582  if (!connected || (pxl.length()<2) || (pxr.length()<2))
583  return false;
584 
585  Bottle command, reply;
586  command.addString("look");
587  command.addString("stereo");
588  Bottle &payLoad=command.addList();
589  payLoad.addFloat64(pxl[0]);
590  payLoad.addFloat64(pxl[1]);
591  payLoad.addFloat64(pxr[0]);
592  payLoad.addFloat64(pxr[1]);
593 
594  if (!portRpc.write(command,reply))
595  {
596  yError("unable to get reply from server!");
597  return false;
598  }
599 
600  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
601 }
602 
603 
604 /************************************************************************/
606 {
607  if (!connected || (t==NULL))
608  return false;
609 
610  Bottle command, reply;
611  command.addString("get");
612  command.addString("Tneck");
613 
614  if (!portRpc.write(command,reply))
615  {
616  yError("unable to get reply from server!");
617  return false;
618  }
619 
620  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
621  {
622  *t=reply.get(1).asFloat64();
623  return true;
624  }
625 
626  return false;
627 }
628 
629 
630 /************************************************************************/
632 {
633  if (!connected || (t==NULL))
634  return false;
635 
636  Bottle command, reply;
637  command.addString("get");
638  command.addString("Teyes");
639 
640  if (!portRpc.write(command,reply))
641  {
642  yError("unable to get reply from server!");
643  return false;
644  }
645 
646  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
647  {
648  *t=reply.get(1).asFloat64();
649  return true;
650  }
651 
652  return false;
653 }
654 
655 
656 /************************************************************************/
658 {
659  if (!connected || (gain==NULL))
660  return false;
661 
662  Bottle command, reply;
663  command.addString("get");
664  command.addString("vor");
665 
666  if (!portRpc.write(command,reply))
667  {
668  yError("unable to get reply from server!");
669  return false;
670  }
671 
672  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
673  {
674  *gain=reply.get(1).asFloat64();
675  return true;
676  }
677 
678  return false;
679 }
680 
681 
682 /************************************************************************/
684 {
685  if (!connected || (gain==NULL))
686  return false;
687 
688  Bottle command, reply;
689  command.addString("get");
690  command.addString("ocr");
691 
692  if (!portRpc.write(command,reply))
693  {
694  yError("unable to get reply from server!");
695  return false;
696  }
697 
698  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
699  {
700  *gain=reply.get(1).asFloat64();
701  return true;
702  }
703 
704  return false;
705 }
706 
707 
708 /************************************************************************/
710 {
711  if (!connected || (f==NULL))
712  return false;
713 
714  Bottle command, reply;
715  command.addString("get");
716  command.addString("sacc");
717 
718  if (!portRpc.write(command,reply))
719  {
720  yError("unable to get reply from server!");
721  return false;
722  }
723 
724  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
725  {
726  *f=(reply.get(1).asInt32()>0);
727  return true;
728  }
729 
730  return false;
731 }
732 
733 
734 /************************************************************************/
736 {
737  if (!connected || (period==NULL))
738  return false;
739 
740  Bottle command, reply;
741  command.addString("get");
742  command.addString("sinh");
743 
744  if (!portRpc.write(command,reply))
745  {
746  yError("unable to get reply from server!");
747  return false;
748  }
749 
750  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
751  {
752  *period=reply.get(1).asFloat64();
753  return true;
754  }
755 
756  return false;
757 }
758 
759 
760 /************************************************************************/
762 {
763  if (!connected || (angle==NULL))
764  return false;
765 
766  Bottle command, reply;
767  command.addString("get");
768  command.addString("sact");
769 
770  if (!portRpc.write(command,reply))
771  {
772  yError("unable to get reply from server!");
773  return false;
774  }
775 
776  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
777  {
778  *angle=reply.get(1).asFloat64();
779  return true;
780  }
781 
782  return false;
783 }
784 
785 
786 /************************************************************************/
787 bool ClientGazeController::getPose(const string &poseSel, Vector &x, Vector &o,
788  Stamp *stamp)
789 {
790  if (!connected)
791  return false;
792 
793  Bottle command, reply;
794  command.addString("get");
795  command.addString("pose");
796  command.addString(poseSel);
797 
798  if (!portRpc.write(command,reply))
799  {
800  yError("unable to get reply from server!");
801  return false;
802  }
803 
804  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
805  {
806  if (Bottle *bPose=reply.get(1).asList())
807  {
808  if (bPose->size()>=7)
809  {
810  x.resize(3);
811  o.resize(bPose->size()-x.length());
812 
813  for (size_t i=0; i<x.length(); i++)
814  x[i]=bPose->get(i).asFloat64();
815 
816  for (size_t i=0; i<o.length(); i++)
817  o[i]=bPose->get(x.length()+i).asFloat64();
818 
819  if ((reply.size()>2) && (stamp!=NULL))
820  {
821  if (Bottle *bStamp=reply.get(2).asList())
822  {
823  Stamp tmpStamp(bStamp->get(0).asInt32(),
824  bStamp->get(1).asFloat64());
825 
826  *stamp=tmpStamp;
827  }
828  }
829 
830  return true;
831  }
832  }
833  }
834 
835  return false;
836 }
837 
838 
839 /************************************************************************/
840 bool ClientGazeController::getLeftEyePose(Vector &x, Vector &o, Stamp *stamp)
841 {
842  return getPose("left",x,o,stamp);
843 }
844 
845 
846 /************************************************************************/
847 bool ClientGazeController::getRightEyePose(Vector &x, Vector &o, Stamp *stamp)
848 {
849  return getPose("right",x,o,stamp);
850 }
851 
852 
853 /************************************************************************/
854 bool ClientGazeController::getHeadPose(Vector &x, Vector &o, Stamp *stamp)
855 {
856  return getPose("head",x,o,stamp);
857 }
858 
859 
860 /************************************************************************/
861 bool ClientGazeController::get2DPixel(const int camSel, const Vector &x,
862  Vector &px)
863 {
864  if (!connected || (x.length()<3))
865  return false;
866 
867  Bottle command, reply;
868  command.addString("get");
869  command.addString("2D");
870  Bottle &bOpt=command.addList();
871  bOpt.addString((camSel==0)?"left":"right");
872  bOpt.addFloat64(x[0]);
873  bOpt.addFloat64(x[1]);
874  bOpt.addFloat64(x[2]);
875 
876  if (!portRpc.write(command,reply))
877  {
878  yError("unable to get reply from server!");
879  return false;
880  }
881 
882  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
883  {
884  if (Bottle *bPixel=reply.get(1).asList())
885  {
886  px.resize(bPixel->size());
887  for (size_t i=0; i<px.length(); i++)
888  px[i]=bPixel->get(i).asFloat64();
889 
890  return true;
891  }
892  }
893 
894  return false;
895 }
896 
897 
898 /************************************************************************/
899 bool ClientGazeController::get3DPoint(const int camSel, const Vector &px,
900  const double z, Vector &x)
901 {
902  if (!connected || (px.length()<2))
903  return false;
904 
905  Bottle command, reply;
906  command.addString("get");
907  command.addString("3D");
908  command.addString("mono");
909  Bottle &bOpt=command.addList();
910  bOpt.addString((camSel==0)?"left":"right");
911  bOpt.addFloat64(px[0]);
912  bOpt.addFloat64(px[1]);
913  bOpt.addFloat64(z);
914 
915  if (!portRpc.write(command,reply))
916  {
917  yError("unable to get reply from server!");
918  return false;
919  }
920 
921  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
922  {
923  if (Bottle *bPoint=reply.get(1).asList())
924  {
925  x.resize(bPoint->size());
926  for (size_t i=0; i<x.length(); i++)
927  x[i]=bPoint->get(i).asFloat64();
928 
929  return true;
930  }
931  }
932 
933  return false;
934 }
935 
936 
937 /************************************************************************/
938 bool ClientGazeController::get3DPointOnPlane(const int camSel, const Vector &px,
939  const Vector &plane, Vector &x)
940 {
941  if (!connected || (px.length()<2) || (plane.length()<4))
942  return false;
943 
944  Bottle command, reply;
945  command.addString("get");
946  command.addString("3D");
947  command.addString("proj");
948  Bottle &bOpt=command.addList();
949  bOpt.addString((camSel==0)?"left":"right");
950  bOpt.addFloat64(px[0]);
951  bOpt.addFloat64(px[1]);
952  bOpt.addFloat64(plane[0]);
953  bOpt.addFloat64(plane[1]);
954  bOpt.addFloat64(plane[2]);
955  bOpt.addFloat64(plane[3]);
956 
957  if (!portRpc.write(command,reply))
958  {
959  yError("unable to get reply from server!");
960  return false;
961  }
962 
963  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
964  {
965  if (Bottle *bPoint=reply.get(1).asList())
966  {
967  x.resize(bPoint->size());
968  for (size_t i=0; i<x.length(); i++)
969  x[i]=bPoint->get(i).asFloat64();
970 
971  return true;
972  }
973  }
974 
975  return false;
976 }
977 
978 
979 /************************************************************************/
980 bool ClientGazeController::get3DPointFromAngles(const int mode, const Vector &ang,
981  Vector &x)
982 {
983  if (!connected || (ang.length()<3))
984  return false;
985 
986  Bottle command, reply;
987  command.addString("get");
988  command.addString("3D");
989  command.addString("ang");
990  Bottle &bOpt=command.addList();
991  bOpt.addString((mode==0)?"abs":"rel");
992  bOpt.addFloat64(ang[0]);
993  bOpt.addFloat64(ang[1]);
994  bOpt.addFloat64(ang[2]);
995 
996  if (!portRpc.write(command,reply))
997  {
998  yError("unable to get reply from server!");
999  return false;
1000  }
1001 
1002  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1003  {
1004  if (Bottle *bPoint=reply.get(1).asList())
1005  {
1006  x.resize(bPoint->size());
1007  for (size_t i=0; i<x.length(); i++)
1008  x[i]=bPoint->get(i).asFloat64();
1009 
1010  return true;
1011  }
1012  }
1013 
1014  return false;
1015 }
1016 
1017 
1018 /************************************************************************/
1019 bool ClientGazeController::getAnglesFrom3DPoint(const Vector &x, Vector &ang)
1020 {
1021  if (!connected || (x.length()<3))
1022  return false;
1023 
1024  Bottle command, reply;
1025  command.addString("get");
1026  command.addString("ang");
1027  Bottle &bOpt=command.addList();
1028  bOpt.addFloat64(x[0]);
1029  bOpt.addFloat64(x[1]);
1030  bOpt.addFloat64(x[2]);
1031 
1032  if (!portRpc.write(command,reply))
1033  {
1034  yError("unable to get reply from server!");
1035  return false;
1036  }
1037 
1038  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1039  {
1040  if (Bottle *bAng=reply.get(1).asList())
1041  {
1042  ang.resize(bAng->size());
1043  for (size_t i=0; i<ang.length(); i++)
1044  ang[i]=bAng->get(i).asFloat64();
1045 
1046  return true;
1047  }
1048  }
1049 
1050  return false;
1051 }
1052 
1053 
1054 /************************************************************************/
1055 bool ClientGazeController::triangulate3DPoint(const Vector &pxl, const Vector &pxr,
1056  Vector &x)
1057 {
1058  if (!connected || ((pxl.length()<2) && (pxr.length()<2)))
1059  return false;
1060 
1061  Bottle command, reply;
1062  command.addString("get");
1063  command.addString("3D");
1064  command.addString("stereo");
1065  Bottle &bOpt=command.addList();
1066  bOpt.addFloat64(pxl[0]);
1067  bOpt.addFloat64(pxl[1]);
1068  bOpt.addFloat64(pxr[0]);
1069  bOpt.addFloat64(pxr[1]);
1070 
1071  if (!portRpc.write(command,reply))
1072  {
1073  yError("unable to get reply from server!");
1074  return false;
1075  }
1076 
1077  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1078  {
1079  if (Bottle *bPoint=reply.get(1).asList())
1080  {
1081  x.resize(bPoint->size());
1082  for (size_t i=0; i<x.length(); i++)
1083  x[i]=bPoint->get(i).asFloat64();
1084 
1085  return true;
1086  }
1087  }
1088 
1089  return false;
1090 }
1091 
1092 
1093 /************************************************************************/
1095 {
1096  if (!connected)
1097  return false;
1098 
1099  Bottle command, reply;
1100  command.addString("get");
1101  command.addString("des");
1102 
1103  if (!portRpc.write(command,reply))
1104  {
1105  yError("unable to get reply from server!");
1106  return false;
1107  }
1108 
1109  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1110  {
1111  if (Bottle *bDes=reply.get(1).asList())
1112  {
1113  qdes.resize(bDes->size());
1114  for (size_t i=0; i<qdes.length(); i++)
1115  qdes[i]=bDes->get(i).asFloat64();
1116 
1117  return true;
1118  }
1119  }
1120 
1121  return false;
1122 }
1123 
1124 
1125 /************************************************************************/
1127 {
1128  if (!connected)
1129  return false;
1130 
1131  Bottle command, reply;
1132  command.addString("get");
1133  command.addString("vel");
1134 
1135  if (!portRpc.write(command,reply))
1136  {
1137  yError("unable to get reply from server!");
1138  return false;
1139  }
1140 
1141  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1142  {
1143  if (Bottle *bVel=reply.get(1).asList())
1144  {
1145  qdot.resize(bVel->size());
1146  for (size_t i=0; i<qdot.length(); i++)
1147  qdot[i]=bVel->get(i).asFloat64();
1148 
1149  return true;
1150  }
1151  }
1152 
1153  return false;
1154 }
1155 
1156 
1157 /************************************************************************/
1159 {
1160  if (!connected)
1161  return false;
1162 
1163  Bottle command, reply;
1164  command.addString("get");
1165  command.addString("pid");
1166 
1167  if (!portRpc.write(command,reply))
1168  {
1169  yError("unable to get reply from server!");
1170  return false;
1171  }
1172 
1173  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1174  {
1175  if (Bottle *bOpt=reply.get(1).asList())
1176  {
1177  options=*bOpt;
1178  return true;
1179  }
1180  }
1181 
1182  return false;
1183 }
1184 
1185 
1186 /************************************************************************/
1188 {
1189  if (!connected)
1190  return false;
1191 
1192  Bottle command, reply;
1193  command.addString("set");
1194  command.addString("Tneck");
1195  command.addFloat64(t);
1196 
1197  if (!portRpc.write(command,reply))
1198  {
1199  yError("unable to get reply from server!");
1200  return false;
1201  }
1202 
1203  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1204 }
1205 
1206 
1207 /************************************************************************/
1209 {
1210  if (!connected)
1211  return false;
1212 
1213  Bottle command, reply;
1214  command.addString("set");
1215  command.addString("Teyes");
1216  command.addFloat64(t);
1217 
1218  if (!portRpc.write(command,reply))
1219  {
1220  yError("unable to get reply from server!");
1221  return false;
1222  }
1223 
1224  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1225 }
1226 
1227 
1228 /************************************************************************/
1229 bool ClientGazeController::setVORGain(const double gain)
1230 {
1231  if (!connected)
1232  return false;
1233 
1234  Bottle command, reply;
1235  command.addString("set");
1236  command.addString("vor");
1237  command.addFloat64(gain);
1238 
1239  if (!portRpc.write(command,reply))
1240  {
1241  yError("unable to get reply from server!");
1242  return false;
1243  }
1244 
1245  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1246 }
1247 
1248 
1249 /************************************************************************/
1250 bool ClientGazeController::setOCRGain(const double gain)
1251 {
1252  if (!connected)
1253  return false;
1254 
1255  Bottle command, reply;
1256  command.addString("set");
1257  command.addString("ocr");
1258  command.addFloat64(gain);
1259 
1260  if (!portRpc.write(command,reply))
1261  {
1262  yError("unable to get reply from server!");
1263  return false;
1264  }
1265 
1266  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1267 }
1268 
1269 
1270 /************************************************************************/
1272 {
1273  if (!connected)
1274  return false;
1275 
1276  Bottle command, reply;
1277  command.addString("set");
1278  command.addString("sacc");
1279  command.addInt32((int)f);
1280 
1281  if (!portRpc.write(command,reply))
1282  {
1283  yError("unable to get reply from server!");
1284  return false;
1285  }
1286 
1287  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1288 }
1289 
1290 
1291 /************************************************************************/
1293 {
1294  if (!connected)
1295  return false;
1296 
1297  Bottle command, reply;
1298  command.addString("set");
1299  command.addString("sinh");
1300  command.addFloat64(period);
1301 
1302  if (!portRpc.write(command,reply))
1303  {
1304  yError("unable to get reply from server!");
1305  return false;
1306  }
1307 
1308  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1309 }
1310 
1311 
1312 /************************************************************************/
1314 {
1315  if (!connected)
1316  return false;
1317 
1318  Bottle command, reply;
1319  command.addString("set");
1320  command.addString("sact");
1321  command.addFloat64(angle);
1322 
1323  if (!portRpc.write(command,reply))
1324  {
1325  yError("unable to get reply from server!");
1326  return false;
1327  }
1328 
1329  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1330 }
1331 
1332 
1333 /************************************************************************/
1334 bool ClientGazeController::setStereoOptions(const Bottle &options)
1335 {
1336  if (!connected)
1337  return false;
1338 
1339  Bottle command, reply;
1340  command.addString("set");
1341  command.addString("pid");
1342  command.addList()=options;
1343 
1344  if (!portRpc.write(command,reply))
1345  {
1346  yError("unable to get reply from server!");
1347  return false;
1348  }
1349 
1350  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1351 }
1352 
1353 
1354 /************************************************************************/
1355 bool ClientGazeController::blockNeckJoint(const string &joint, const double min,
1356  const double max)
1357 {
1358  if (!connected)
1359  return false;
1360 
1361  Bottle command, reply;
1362  command.addString("bind");
1363  command.addString(joint);
1364  command.addFloat64(min);
1365  command.addFloat64(max);
1366 
1367  if (!portRpc.write(command,reply))
1368  {
1369  yError("unable to get reply from server!");
1370  return false;
1371  }
1372 
1373  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1374 }
1375 
1376 
1377 /************************************************************************/
1378 bool ClientGazeController::blockNeckJoint(const string &joint, const int j)
1379 {
1380  if (!connected)
1381  return false;
1382 
1383  Vector *val=portStateHead.read(true);
1384  return blockNeckJoint(joint,(*val)[j],(*val)[j]);
1385 }
1386 
1387 
1388 /************************************************************************/
1389 bool ClientGazeController::getNeckJointRange(const string &joint, double *min,
1390  double *max)
1391 {
1392  if (!connected || (min==NULL) || (max==NULL))
1393  return false;
1394 
1395  Bottle command, reply;
1396  command.addString("get");
1397  command.addString(joint);
1398 
1399  if (!portRpc.write(command,reply))
1400  {
1401  yError("unable to get reply from server!");
1402  return false;
1403  }
1404 
1405  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>2))
1406  {
1407  *min=reply.get(1).asFloat64();
1408  *max=reply.get(2).asFloat64();
1409  return true;
1410  }
1411 
1412  return false;
1413 }
1414 
1415 
1416 /************************************************************************/
1417 bool ClientGazeController::clearJoint(const string &joint)
1418 {
1419  if (!connected)
1420  return false;
1421 
1422  Bottle command, reply;
1423  command.addString("clear");
1424  command.addString(joint);
1425 
1426  if (!portRpc.write(command,reply))
1427  {
1428  yError("unable to get reply from server!");
1429  return false;
1430  }
1431 
1432  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1433 }
1434 
1435 
1436 /************************************************************************/
1437 bool ClientGazeController::bindNeckPitch(const double min, const double max)
1438 {
1439  return blockNeckJoint("pitch",min,max);
1440 }
1441 
1442 
1443 /************************************************************************/
1445 {
1446  return blockNeckJoint("pitch",val,val);
1447 }
1448 
1449 
1450 /************************************************************************/
1452 {
1453  return blockNeckJoint("pitch",3);
1454 }
1455 
1456 
1457 /************************************************************************/
1458 bool ClientGazeController::bindNeckRoll(const double min, const double max)
1459 {
1460  return blockNeckJoint("roll",min,max);
1461 }
1462 
1463 
1464 /************************************************************************/
1466 {
1467  return blockNeckJoint("roll",val,val);
1468 }
1469 
1470 
1471 /************************************************************************/
1473 {
1474  return blockNeckJoint("roll",4);
1475 }
1476 
1477 
1478 /************************************************************************/
1479 bool ClientGazeController::bindNeckYaw(const double min, const double max)
1480 {
1481  return blockNeckJoint("yaw",min,max);
1482 }
1483 
1484 
1485 /************************************************************************/
1487 {
1488  return blockNeckJoint("yaw",val,val);
1489 }
1490 
1491 
1492 /************************************************************************/
1494 {
1495  return blockNeckJoint("yaw",5);
1496 }
1497 
1498 
1499 /************************************************************************/
1500 bool ClientGazeController::blockEyes(const double ver)
1501 {
1502  if (!connected)
1503  return false;
1504 
1505  Bottle command, reply;
1506  command.addString("bind");
1507  command.addString("eyes");
1508  command.addFloat64(ver);
1509 
1510  if (!portRpc.write(command,reply))
1511  {
1512  yError("unable to get reply from server!");
1513  return false;
1514  }
1515 
1516  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1517 }
1518 
1519 
1520 /************************************************************************/
1522 {
1523  if (!connected)
1524  return false;
1525 
1526  Vector *val=portStateHead.read(true);
1527  return blockEyes((*val)[5]);
1528 }
1529 
1530 
1531 /************************************************************************/
1533 {
1534  return getNeckJointRange("pitch",min,max);
1535 }
1536 
1537 
1538 /************************************************************************/
1540 {
1541  return getNeckJointRange("roll",min,max);
1542 }
1543 
1544 
1545 /************************************************************************/
1547 {
1548  return getNeckJointRange("yaw",min,max);
1549 }
1550 
1551 
1552 /************************************************************************/
1554 {
1555  if (!connected || (ver==NULL))
1556  return false;
1557 
1558  Bottle command, reply;
1559  command.addString("get");
1560  command.addString("eyes");
1561 
1562  if (!portRpc.write(command,reply))
1563  {
1564  yError("unable to get reply from server!");
1565  return false;
1566  }
1567 
1568  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>2))
1569  {
1570  *ver=reply.get(1).asFloat64();
1571  return true;
1572  }
1573 
1574  return false;
1575 }
1576 
1577 
1578 /************************************************************************/
1580 {
1581  return clearJoint("pitch");
1582 }
1583 
1584 
1585 /************************************************************************/
1587 {
1588  return clearJoint("roll");
1589 }
1590 
1591 
1592 /************************************************************************/
1594 {
1595  return clearJoint("yaw");
1596 }
1597 
1598 
1599 /************************************************************************/
1601 {
1602  return clearJoint("eyes");
1603 }
1604 
1605 
1606 /************************************************************************/
1608 {
1609  if (!connected || (angle==NULL))
1610  return false;
1611 
1612  Bottle command, reply;
1613  command.addString("get");
1614  command.addString("ntol");
1615 
1616  if (!portRpc.write(command,reply))
1617  {
1618  yError("unable to get reply from server!");
1619  return false;
1620  }
1621 
1622  if ((reply.get(0).asVocab32()==GAZECTRL_ACK) && (reply.size()>1))
1623  {
1624  *angle=reply.get(1).asFloat64();
1625  return true;
1626  }
1627 
1628  return false;
1629 }
1630 
1631 
1632 /************************************************************************/
1634 {
1635  if (!connected)
1636  return false;
1637 
1638  Bottle command, reply;
1639  command.addString("set");
1640  command.addString("ntol");
1641  command.addFloat64(angle);
1642 
1643  if (!portRpc.write(command,reply))
1644  {
1645  yError("unable to get reply from server!");
1646  return false;
1647  }
1648 
1649  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1650 }
1651 
1652 
1653 /************************************************************************/
1655 {
1656  if (!connected || (f==NULL))
1657  return false;
1658 
1659  Bottle command, reply;
1660  command.addString("get");
1661  command.addString("done");
1662 
1663  if (!portRpc.write(command,reply))
1664  {
1665  yError("unable to get reply from server!");
1666  return false;
1667  }
1668 
1669  if (reply.get(0).asVocab32()==GAZECTRL_ACK)
1670  {
1671  *f=(reply.get(1).asInt32()>0);
1672  return true;
1673  }
1674  else
1675  return false;
1676 }
1677 
1678 
1679 /************************************************************************/
1680 bool ClientGazeController::waitMotionDone(const double period, const double timeout)
1681 {
1682  bool done=false;
1683  double t0=Time::now();
1684 
1685  while (!done)
1686  {
1687  Time::delay(period);
1688 
1689  if (!checkMotionDone(&done) || ((timeout>0.0) && ((Time::now()-t0)>timeout)))
1690  return false;
1691  }
1692 
1693  return true;
1694 }
1695 
1696 
1697 /************************************************************************/
1699 {
1700  if (!connected || (f==NULL))
1701  return false;
1702 
1703  Bottle command, reply;
1704  command.addString("get");
1705  command.addString("sdon");
1706 
1707  if (!portRpc.write(command,reply))
1708  {
1709  yError("unable to get reply from server!");
1710  return false;
1711  }
1712 
1713  if (reply.get(0).asVocab32()==GAZECTRL_ACK)
1714  {
1715  *f=(reply.get(1).asInt32()>0);
1716  return true;
1717  }
1718  else
1719  return false;
1720 }
1721 
1722 
1723 /************************************************************************/
1724 bool ClientGazeController::waitSaccadeDone(const double period, const double timeout)
1725 {
1726  bool done=false;
1727  double t0=Time::now();
1728 
1729  while (!done)
1730  {
1731  Time::delay(period);
1732 
1733  if (!checkSaccadeDone(&done) || ((timeout>0.0) && ((Time::now()-t0)>timeout)))
1734  return false;
1735  }
1736 
1737  return true;
1738 }
1739 
1740 
1741 /************************************************************************/
1743 {
1744  if (!connected)
1745  return false;
1746 
1747  Bottle command, reply;
1748  command.addString("stop");
1749 
1750  if (!portRpc.write(command,reply))
1751  {
1752  yError("unable to get reply from server!");
1753  return false;
1754  }
1755 
1756  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1757 }
1758 
1759 
1760 /************************************************************************/
1762 {
1763  if (!connected || (id==NULL))
1764  return false;
1765 
1766  Bottle command, reply;
1767  command.addString("stor");
1768 
1769  if (!portRpc.write(command,reply))
1770  {
1771  yError("unable to get reply from server!");
1772  return false;
1773  }
1774 
1775  if (reply.get(0).asVocab32()==GAZECTRL_ACK)
1776  {
1777  contextIdList.insert(*id=reply.get(1).asInt32());
1778  return true;
1779  }
1780  else
1781  return false;
1782 }
1783 
1784 
1785 /************************************************************************/
1787 {
1788  if (!connected || ((contextIdList.find(id)==contextIdList.end()) && (id!=0)))
1789  return false;
1790 
1791  Bottle command, reply;
1792  command.addString("rest");
1793  command.addInt32(id);
1794 
1795  if (!portRpc.write(command,reply))
1796  {
1797  yError("unable to get reply from server!");
1798  return false;
1799  }
1800 
1801  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1802 }
1803 
1804 
1805 /************************************************************************/
1807 {
1808  if (!connected || ((contextIdList.find(id)==contextIdList.end()) && (id!=0)))
1809  return false;
1810 
1811  Bottle command, reply;
1812  command.addString("del");
1813  command.addList().addInt32(id);
1814 
1815  if (!portRpc.write(command,reply))
1816  {
1817  yError("unable to get reply from server!");
1818  return false;
1819  }
1820 
1821  if (reply.get(0).asVocab32()==GAZECTRL_ACK)
1822  {
1823  contextIdList.erase(id);
1824  return true;
1825  }
1826  else
1827  return false;
1828 }
1829 
1830 
1831 /************************************************************************/
1833 {
1834  if (!connected)
1835  return false;
1836 
1837  if (contextIdList.empty())
1838  return true;
1839 
1840  Bottle command, reply;
1841  command.addString("del");
1842  Bottle &ids=command.addList();
1843  for (set<int>::iterator itr=contextIdList.begin(); itr!=contextIdList.end(); itr++)
1844  ids.addInt32(*itr);
1845 
1846  if (!portRpc.write(command,reply))
1847  {
1848  yError("unable to get reply from server!");
1849  return false;
1850  }
1851 
1852  contextIdList.clear();
1853 
1854  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
1855 }
1856 
1857 
1858 /************************************************************************/
1860 {
1861  Bottle command, reply;
1862  command.addString("get");
1863  command.addString("info");
1864 
1865  if (!portRpc.write(command,reply))
1866  {
1867  yError("unable to get reply from server!");
1868  return false;
1869  }
1870 
1871  if (reply.get(0).asVocab32()==GAZECTRL_ACK)
1872  {
1873  if (reply.size()>1)
1874  {
1875  if (Bottle *infoPart=reply.get(1).asList())
1876  info=*infoPart;
1877 
1878  return true;
1879  }
1880  }
1881 
1882  return false;
1883 }
1884 
1885 
1886 /************************************************************************/
1888 {
1889  if (connected)
1890  return getInfoHelper(info);
1891  else
1892  return false;
1893 }
1894 
1895 
1896 /************************************************************************/
1898 {
1899  string type=event.get(0).asString();
1900  double time=event.get(1).asFloat64();
1901  double checkPoint=(type=="motion-ongoing")?event.get(2).asFloat64():-1.0;
1902  map<string,GazeEvent*>::iterator itr;
1903 
1904  // rise the all-events callback
1905  itr=eventsMap.find("*");
1906  if (itr!=eventsMap.end())
1907  {
1908  if (itr->second!=NULL)
1909  {
1910  GazeEvent &Event=*itr->second;
1911  Event.gazeEventVariables.type=type;
1912  Event.gazeEventVariables.time=time;
1913 
1914  if (checkPoint>=0.0)
1915  Event.gazeEventVariables.motionOngoingCheckPoint=checkPoint;
1916 
1917  Event.gazeEventCallback();
1918  }
1919  }
1920 
1921  string typeExtended=type;
1922  if (checkPoint>=0.0)
1923  {
1924  ostringstream ss;
1925  ss<<type<<"-"<<checkPoint;
1926  typeExtended=ss.str();
1927  }
1928 
1929  // rise the event specific callback
1930  itr=eventsMap.find(typeExtended);
1931  if (itr!=eventsMap.end())
1932  {
1933  if (itr->second!=NULL)
1934  {
1935  GazeEvent &Event=*itr->second;
1936  Event.gazeEventVariables.type=type;
1937  Event.gazeEventVariables.time=time;
1938 
1939  if (checkPoint>=0.0)
1940  Event.gazeEventVariables.motionOngoingCheckPoint=checkPoint;
1941 
1942  Event.gazeEventCallback();
1943  }
1944  }
1945 }
1946 
1947 
1948 /************************************************************************/
1950 {
1951  if (!connected)
1952  return false;
1953 
1954  string type=event.gazeEventParameters.type;
1955  if (type=="motion-ongoing")
1956  {
1957  double checkPoint=event.gazeEventParameters.motionOngoingCheckPoint;
1958 
1959  Bottle command, reply;
1960  command.addString("register");
1961  command.addString("ongoing");
1962  command.addFloat64(checkPoint);
1963 
1964  if (!portRpc.write(command,reply))
1965  {
1966  yError("unable to get reply from server!");
1967  return false;
1968  }
1969 
1970  if (reply.get(0).asVocab32()!=GAZECTRL_ACK)
1971  return false;
1972 
1973  ostringstream ss;
1974  ss<<type<<"-"<<checkPoint;
1975  type=ss.str();
1976  }
1977 
1978  eventsMap[type]=&event;
1979  return true;
1980 }
1981 
1982 
1983 /************************************************************************/
1985 {
1986  if (!connected)
1987  return false;
1988 
1989  string type=event.gazeEventParameters.type;
1990  if (type=="motion-ongoing")
1991  {
1992  double checkPoint=event.gazeEventParameters.motionOngoingCheckPoint;
1993 
1994  Bottle command, reply;
1995  command.addString("unregister");
1996  command.addString("ongoing");
1997  command.addFloat64(checkPoint);
1998 
1999  if (!portRpc.write(command,reply))
2000  {
2001  yError("unable to get reply from server!");
2002  return false;
2003  }
2004 
2005  if (reply.get(0).asVocab32()!=GAZECTRL_ACK)
2006  return false;
2007 
2008  ostringstream ss;
2009  ss<<type<<"-"<<checkPoint;
2010  type=ss.str();
2011  }
2012 
2013  eventsMap.erase(type);
2014  return true;
2015 }
2016 
2017 
2018 /************************************************************************/
2019 bool ClientGazeController::tweakSet(const Bottle &options)
2020 {
2021  if (!connected)
2022  return false;
2023 
2024  Bottle command, reply;
2025  command.addString("set");
2026  command.addString("tweak");
2027  command.addList()=options;
2028 
2029  if (!portRpc.write(command,reply))
2030  {
2031  yError("unable to get reply from server!");
2032  return false;
2033  }
2034 
2035  return (reply.get(0).asVocab32()==GAZECTRL_ACK);
2036 }
2037 
2038 
2039 /************************************************************************/
2040 bool ClientGazeController::tweakGet(Bottle &options)
2041 {
2042  if (!connected)
2043  return false;
2044 
2045  Bottle command, reply;
2046  command.addString("get");
2047  command.addString("tweak");
2048 
2049  if (!portRpc.write(command,reply))
2050  {
2051  yError("unable to get reply from server!");
2052  return false;
2053  }
2054 
2055  if (reply.get(0).asVocab32()==GAZECTRL_ACK)
2056  {
2057  if (reply.size()>1)
2058  {
2059  if (Bottle *optionsPart=reply.get(1).asList())
2060  options=*optionsPart;
2061 
2062  return true;
2063  }
2064  }
2065 
2066  return false;
2067 }
2068 
2069 
2070 /************************************************************************/
2072 {
2073  close();
2074 }
2075 
2076 
#define GAZECTRL_CLIENT_VER
#define GAZECTRL_ACK
#define GAZECTRL_DEFAULT_TMO
clientgazecontroller : implements the client part of the the Gaze Control Interface.
bool getPose(const std::string &poseSel, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool getFixationPoint(yarp::sig::Vector &fp, yarp::os::Stamp *stamp=NULL)
bool lookAtFixationPointSync(const yarp::sig::Vector &fp)
bool lookAtMonoPixel(const int camSel, const yarp::sig::Vector &px, const double z=1.0)
bool waitSaccadeDone(const double period=0.1, const double timeout=0.0)
bool lookAtMonoPixelWithVergenceSync(const int camSel, const yarp::sig::Vector &px, const double ver)
bool setTrackingMode(const bool f)
bool get3DPointOnPlane(const int camSel, const yarp::sig::Vector &px, const yarp::sig::Vector &plane, yarp::sig::Vector &x)
bool triangulate3DPoint(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr, yarp::sig::Vector &x)
bool bindNeckPitch(const double min, const double max)
bool setStabilizationMode(const bool f)
bool setVORGain(const double gain)
bool getJointsVelocities(yarp::sig::Vector &qdot)
bool getNeckPitchRange(double *min, double *max)
bool getInfo(yarp::os::Bottle &info)
bool getNeckJointRange(const std::string &joint, double *min, double *max)
bool lookAtMonoPixelWithVergence(const int camSel, const yarp::sig::Vector &px, const double ver)
bool setOCRGain(const double gain)
bool unregisterEvent(yarp::dev::GazeEvent &event)
bool get3DPoint(const int camSel, const yarp::sig::Vector &px, const double z, yarp::sig::Vector &x)
bool tweakGet(yarp::os::Bottle &options)
bool getSaccadesActivationAngle(double *angle)
bool getOCRGain(double *gain)
bool lookAtStereoPixelsSync(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr)
bool lookAtRelAngles(const yarp::sig::Vector &ang)
bool getLeftEyePose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool getVORGain(double *gain)
bool getNeckAngleUserTolerance(double *angle)
bool getBlockedVergence(double *ver)
bool getRightEyePose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool registerEvent(yarp::dev::GazeEvent &event)
bool setNeckAngleUserTolerance(const double angle)
bool getSaccadesInhibitionPeriod(double *period)
bool setSaccadesActivationAngle(const double angle)
bool bindNeckYaw(const double min, const double max)
bool setEyesTrajTime(const double t)
bool lookAtStereoPixels(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr)
bool getInfoHelper(yarp::os::Bottle &info)
bool setStereoOptions(const yarp::os::Bottle &options)
bool bindNeckRoll(const double min, const double max)
bool setSaccadesMode(const bool f)
bool blockNeckJoint(const std::string &joint, const double min, const double max)
bool setSaccadesInhibitionPeriod(const double period)
bool getAngles(yarp::sig::Vector &ang, yarp::os::Stamp *stamp=NULL)
bool tweakSet(const yarp::os::Bottle &options)
bool getAnglesFrom3DPoint(const yarp::sig::Vector &x, yarp::sig::Vector &ang)
bool getJointsDesired(yarp::sig::Vector &qdes)
bool clearJoint(const std::string &joint)
bool get2DPixel(const int camSel, const yarp::sig::Vector &x, yarp::sig::Vector &px)
bool lookAtMonoPixelSync(const int camSel, const yarp::sig::Vector &px, const double z=1.0)
bool lookAtRelAnglesSync(const yarp::sig::Vector &ang)
bool getStereoOptions(yarp::os::Bottle &options)
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
bool getHeadPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool setNeckTrajTime(const double t)
bool lookAtFixationPoint(const yarp::sig::Vector &fp)
bool getNeckYawRange(double *min, double *max)
bool lookAtAbsAngles(const yarp::sig::Vector &ang)
bool deleteContext(const int id)
bool getNeckRollRange(double *min, double *max)
bool lookAtAbsAnglesSync(const yarp::sig::Vector &ang)
bool restoreContext(const int id)
bool get3DPointFromAngles(const int mode, const yarp::sig::Vector &ang, yarp::sig::Vector &x)
bool open(yarp::os::Searchable &config)
void eventHandling(yarp::os::Bottle &event)
void setInterface(ClientGazeController *interface)
void onRead(yarp::os::Bottle &event)
cmd
Definition: dataTypes.h:30
static float angle
Definition: iCub_Sim.cpp:72
static int v
Definition: iCub_Sim.cpp:42
bool done
Definition: main.cpp:42
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
static struct bpf_program fp
GLenum mode
Definition: rendering.cpp:48
degrees time
Definition: sine.m:5