d4c
d4c_client.cpp
1 /*
2  * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia
3  * Authors: Ilaria Gori, Ugo Pattacini
4  * email: ilaria.gori@iit.it, ugo.pattacini@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #include <cstdio>
19 #include <cstdarg>
20 
21 #include <iCub/d4c/d4c_client.h>
22 #include <iCub/d4c/private/d4c_helpers.h>
23 
24 using namespace std;
25 using namespace yarp::os;
26 using namespace yarp::sig;
27 using namespace iCub::d4c;
28 
29 
30 /************************************************************************/
31 D4CClient::D4CClient() : isOpen(false), verbosity(0)
32 {
33  remote="";
34  local="";
35 
36  field.resize(7,0.0);
37  xdot.resize(7,0.0);
38  x.resize(7,0.0);
39  xhat.resize(7,0.0);
40  qhat.resize(10,0.0);
41 }
42 
43 
44 /************************************************************************/
45 D4CClient::~D4CClient()
46 {
47  close();
48 }
49 
50 
51 /************************************************************************/
52 void D4CClient::printMessage(const int logtype, const int level,
53  const char *format, ...) const
54 {
55  if (verbosity>=level)
56  {
57  string str;
58  str="*** "+local+": ";
59 
60  va_list arg;
61  char buf[512];
62  va_start(arg,format);
63  vsnprintf(buf,sizeof(buf),format,arg);
64  va_end(arg);
65 
66  str+=buf;
67  switch (logtype)
68  {
69  case log::error:
70  yError(str.c_str());
71  break;
72  case log::warning:
73  yWarning(str.c_str());
74  break;
75  case log::info:
76  yInfo(str.c_str());
77  break;
78  default:
79  printf("%s\n",str.c_str());
80  }
81  }
82 }
83 
84 
85 /************************************************************************/
86 bool D4CClient::open(const Property &options)
87 {
88  if (options.check("remote"))
89  remote=options.find("remote").asString().c_str();
90  else
91  {
92  printMessage(log::error,1,"\"remote\" option is mandatory to open the client!");
93  return false;
94  }
95 
96  if (options.check("local"))
97  local=options.find("local").asString().c_str();
98  else
99  {
100  printMessage(log::error,1,"\"local\" option is mandatory to open the client!");
101  return false;
102  }
103 
104  carrier=options.check("carrier",Value("udp")).asString().c_str();
105  verbosity=options.check("verbosity",Value(0)).asInt();
106 
107  data.open((local+"/data:i").c_str());
108  rpc.open((local+"/rpc").c_str());
109 
110  bool ok=true;
111 
112  ok&=Network::connect((remote+"/data:o").c_str(),data.getName().c_str(),carrier.c_str());
113  ok&=Network::connect(rpc.getName().c_str(),(remote+"/rpc").c_str());
114 
115  if (ok)
116  {
117  Bottle cmd,reply;
118  cmd.addVocab(D4C_VOCAB_CMD_PING);
119 
120  if (rpc.write(cmd,reply))
121  {
122  if (reply.size()>0)
123  {
124  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
125  {
126  printMessage(log::info,1,"successfully connected with the server %s!",remote.c_str());
127  return isOpen=true;
128  }
129  }
130  }
131 
132  printMessage(log::error,1,"unable to get correct reply from the server %s!",remote.c_str());
133  close();
134 
135  return false;
136  }
137  else
138  {
139  printMessage(log::error,1,"unable to connect to the server %s!",remote.c_str());
140  close();
141 
142  return false;
143  }
144 }
145 
146 
147 /************************************************************************/
148 void D4CClient::close()
149 {
150  if (isOpen)
151  {
152  data.interrupt();
153  rpc.interrupt();
154 
155  data.close();
156  rpc.close();
157 
158  isOpen=false;
159 
160  printMessage(log::info,1,"client closed");
161  }
162  else
163  printMessage(log::warning,3,"client is already closed");
164 }
165 
166 
167 /************************************************************************/
168 bool D4CClient::addItem(const Property &options, int &item)
169 {
170  if (isOpen)
171  {
172  string options_str=options.toString().c_str();
173  printMessage(log::no_info,2,"request for adding new item: %s",options_str.c_str());
174 
175  Value val;
176  val.fromString(("("+options_str+")").c_str());
177 
178  Bottle cmd,reply;
179  cmd.addVocab(D4C_VOCAB_CMD_ADD);
180  cmd.add(val);
181 
182  if (rpc.write(cmd,reply))
183  {
184  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
185  {
186  item=reply.get(1).asInt();
187  printMessage(log::no_info,1,"item %d successfully added",item);
188  return true;
189  }
190  else
191  {
192  printMessage(log::error,1,"something went wrong: request rejected");
193  return false;
194  }
195  }
196  else
197  {
198  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
199  return false;
200  }
201  }
202  else
203  {
204  printMessage(log::warning,1,"client is not open");
205  return false;
206  }
207 }
208 
209 
210 /************************************************************************/
211 bool D4CClient::eraseItem(const int item)
212 {
213  if (isOpen)
214  {
215  printMessage(log::no_info,2,"request for erasing item %d",item);
216 
217  Bottle cmd,reply;
218  cmd.addVocab(D4C_VOCAB_CMD_DEL);
219  cmd.add(item);
220 
221  if (rpc.write(cmd,reply))
222  {
223  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
224  {
225  printMessage(log::no_info,1,"item %d successfully erased",item);
226  return true;
227  }
228  else
229  {
230  printMessage(log::error,1,"something went wrong: request rejected");
231  return false;
232  }
233  }
234  else
235  {
236  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
237  return false;
238  }
239  }
240  else
241  {
242  printMessage(log::warning,1,"client is not open");
243  return false;
244  }
245 }
246 
247 
248 /************************************************************************/
249 bool D4CClient::clearItems()
250 {
251  if (isOpen)
252  {
253  printMessage(log::no_info,2,"request for clearing item table");
254 
255  Bottle cmd,reply;
256  cmd.addVocab(D4C_VOCAB_CMD_CLEAR);
257 
258  if (rpc.write(cmd,reply))
259  {
260  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
261  {
262  printMessage(log::no_info,1,"all items have been successfully erased");
263  return true;
264  }
265  else
266  {
267  printMessage(log::error,1,"something went wrong: request rejected");
268  return false;
269  }
270  }
271  else
272  {
273  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
274  return false;
275  }
276  }
277  else
278  {
279  printMessage(log::warning,1,"client is not open");
280  return false;
281  }
282 }
283 
284 
285 /************************************************************************/
286 bool D4CClient::getItems(Bottle &items)
287 {
288  if (isOpen)
289  {
290  printMessage(log::no_info,2,"request for retrieving items ids list");
291 
292  Bottle cmd,reply;
293  cmd.addVocab(D4C_VOCAB_CMD_LIST);
294 
295  if (rpc.write(cmd,reply))
296  {
297  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
298  {
299  items=*reply.get(1).asList();
300  printMessage(log::no_info,1,"items ids successfully retrieved: %s",
301  items.toString().c_str());
302  return true;
303  }
304  else
305  {
306  printMessage(log::error,1,"something went wrong: request rejected");
307  return false;
308  }
309  }
310  else
311  {
312  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
313  return false;
314  }
315  }
316  else
317  {
318  printMessage(log::warning,1,"client is not open");
319  return false;
320  }
321 }
322 
323 
324 /************************************************************************/
325 bool D4CClient::setProperty(const int item, const Property &options)
326 {
327  if (isOpen)
328  {
329  string options_str=options.toString().c_str();
330  printMessage(log::no_info,2,"request for setting item %d property: %s",
331  item,options_str.c_str());
332 
333  Value val;
334  val.fromString(("("+options_str+")").c_str());
335 
336  Bottle cmd,reply;
337  cmd.addVocab(D4C_VOCAB_CMD_SET);
338  cmd.addInt(item);
339  cmd.add(val);
340 
341  if (rpc.write(cmd,reply))
342  {
343  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
344  {
345  printMessage(log::no_info,1,"item %d property successfully updated: %s",
346  item,options_str.c_str());
347  return true;
348  }
349  else
350  {
351  printMessage(log::error,1,"something went wrong: request rejected");
352  return false;
353  }
354  }
355  else
356  {
357  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
358  return false;
359  }
360  }
361  else
362  {
363  printMessage(log::warning,1,"client is not open");
364  return false;
365  }
366 }
367 
368 
369 /************************************************************************/
370 bool D4CClient::getProperty(const int item, Property &options)
371 {
372  if (isOpen)
373  {
374  printMessage(log::no_info,2,"request for retrieving item %d property",item);
375 
376  Bottle cmd,reply;
377  cmd.addVocab(D4C_VOCAB_CMD_GET);
378  cmd.addInt(item);
379 
380  if (rpc.write(cmd,reply))
381  {
382  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
383  {
384  options=extractProperty(reply.get(1));
385  printMessage(log::no_info,1,"item %d property successfully retrieved: %s",
386  item,options.toString().c_str());
387  return true;
388  }
389  else
390  {
391  printMessage(log::error,1,"something went wrong: request rejected");
392  return false;
393  }
394  }
395  else
396  {
397  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
398  return false;
399  }
400  }
401  else
402  {
403  printMessage(log::warning,1,"client is not open");
404  return false;
405  }
406 }
407 
408 
409 /************************************************************************/
410 bool D4CClient::enableField()
411 {
412  if (isOpen)
413  {
414  printMessage(log::no_info,2,"request for enabling field");
415 
416  Bottle cmd,reply;
417  cmd.addVocab(D4C_VOCAB_CMD_ENFIELD);
418 
419  if (rpc.write(cmd,reply))
420  {
421  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
422  {
423  printMessage(log::no_info,1,"field enabled");
424  return true;
425  }
426  else
427  {
428  printMessage(log::error,1,"something went wrong: request rejected");
429  return false;
430  }
431  }
432  else
433  {
434  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
435  return false;
436  }
437  }
438  else
439  {
440  printMessage(log::warning,1,"client is not open");
441  return false;
442  }
443 }
444 
445 
446 /************************************************************************/
447 bool D4CClient::disableField()
448 {
449  if (isOpen)
450  {
451  printMessage(log::no_info,2,"request for disabling field");
452 
453  Bottle cmd,reply;
454  cmd.addVocab(D4C_VOCAB_CMD_DISFIELD);
455 
456  if (rpc.write(cmd,reply))
457  {
458  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
459  {
460  printMessage(log::no_info,1,"field disabled");
461  return true;
462  }
463  else
464  {
465  printMessage(log::error,1,"something went wrong: request rejected");
466  return false;
467  }
468  }
469  else
470  {
471  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
472  return false;
473  }
474  }
475  else
476  {
477  printMessage(log::warning,1,"client is not open");
478  return false;
479  }
480 }
481 
482 
483 /************************************************************************/
484 bool D4CClient::getFieldStatus(bool &status)
485 {
486  if (isOpen)
487  {
488  printMessage(log::no_info,2,"request for retrieving field status");
489 
490  Bottle cmd,reply;
491  cmd.addVocab(D4C_VOCAB_CMD_STATFIELD);
492 
493  if (rpc.write(cmd,reply))
494  {
495  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
496  {
497  string sw=reply.get(1).asString().c_str();
498  status=sw=="on";
499  printMessage(log::no_info,1,"field status = %s",sw.c_str());
500  return true;
501  }
502  else
503  {
504  printMessage(log::error,1,"something went wrong: request rejected");
505  return false;
506  }
507  }
508  else
509  {
510  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
511  return false;
512  }
513  }
514  else
515  {
516  printMessage(log::warning,1,"client is not open");
517  return false;
518  }
519 }
520 
521 
522 /************************************************************************/
523 bool D4CClient::enableControl()
524 {
525  if (isOpen)
526  {
527  printMessage(log::no_info,2,"request for enabling control");
528 
529  Bottle cmd,reply;
530  cmd.addVocab(D4C_VOCAB_CMD_ENCTRL);
531 
532  if (rpc.write(cmd,reply))
533  {
534  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
535  {
536  printMessage(log::no_info,1,"control enabled");
537  return true;
538  }
539  else
540  {
541  printMessage(log::error,1,"something went wrong: request rejected");
542  return false;
543  }
544  }
545  else
546  {
547  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
548  return false;
549  }
550  }
551  else
552  {
553  printMessage(log::warning,1,"client is not open");
554  return false;
555  }
556 }
557 
558 
559 /************************************************************************/
560 bool D4CClient::disableControl()
561 {
562  if (isOpen)
563  {
564  printMessage(log::no_info,2,"request for disabling control");
565 
566  Bottle cmd,reply;
567  cmd.addVocab(D4C_VOCAB_CMD_DISCTRL);
568 
569  if (rpc.write(cmd,reply))
570  {
571  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
572  {
573  printMessage(log::no_info,1,"control disabled");
574  return true;
575  }
576  else
577  {
578  printMessage(log::error,1,"something went wrong: request rejected");
579  return false;
580  }
581  }
582  else
583  {
584  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
585  return false;
586  }
587  }
588  else
589  {
590  printMessage(log::warning,1,"client is not open");
591  return false;
592  }
593 }
594 
595 
596 /************************************************************************/
597 bool D4CClient::getControlStatus(bool &status)
598 {
599  if (isOpen)
600  {
601  printMessage(log::no_info,2,"request for retrieving control status");
602 
603  Bottle cmd,reply;
604  cmd.addVocab(D4C_VOCAB_CMD_STATCTRL);
605 
606  if (rpc.write(cmd,reply))
607  {
608  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
609  {
610  string sw=reply.get(1).asString().c_str();
611  status=sw=="on";
612  printMessage(log::no_info,1,"control status = %s",sw.c_str());
613  return true;
614  }
615  else
616  {
617  printMessage(log::error,1,"something went wrong: request rejected");
618  return false;
619  }
620  }
621  else
622  {
623  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
624  return false;
625  }
626  }
627  else
628  {
629  printMessage(log::warning,1,"client is not open");
630  return false;
631  }
632 }
633 
634 
635 /************************************************************************/
636 bool D4CClient::enableSimulation()
637 {
638  if (isOpen)
639  {
640  printMessage(log::no_info,2,"request for enabling simulation");
641 
642  Bottle cmd,reply;
643  cmd.addVocab(D4C_VOCAB_CMD_ENSIM);
644 
645  if (rpc.write(cmd,reply))
646  {
647  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
648  {
649  printMessage(log::no_info,1,"simulation enabled");
650  return true;
651  }
652  else
653  {
654  printMessage(log::error,1,"something went wrong: request rejected");
655  return false;
656  }
657  }
658  else
659  {
660  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
661  return false;
662  }
663  }
664  else
665  {
666  printMessage(log::warning,1,"client is not open");
667  return false;
668  }
669 }
670 
671 
672 /************************************************************************/
673 bool D4CClient::disableSimulation()
674 {
675  if (isOpen)
676  {
677  printMessage(log::no_info,2,"request for disabling simulation");
678 
679  Bottle cmd,reply;
680  cmd.addVocab(D4C_VOCAB_CMD_DISSIM);
681 
682  if (rpc.write(cmd,reply))
683  {
684  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
685  {
686  printMessage(log::no_info,1,"simulation disabled");
687  return true;
688  }
689  else
690  {
691  printMessage(log::error,1,"something went wrong: request rejected");
692  return false;
693  }
694  }
695  else
696  {
697  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
698  return false;
699  }
700  }
701  else
702  {
703  printMessage(log::warning,1,"client is not open");
704  return false;
705  }
706 }
707 
708 
709 /************************************************************************/
710 bool D4CClient::getSimulationStatus(bool &status)
711 {
712  if (isOpen)
713  {
714  printMessage(log::no_info,2,"request for retrieving simulation status");
715 
716  Bottle cmd,reply;
717  cmd.addVocab(D4C_VOCAB_CMD_STATSIM);
718 
719  if (rpc.write(cmd,reply))
720  {
721  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
722  {
723  string sw=reply.get(1).asString().c_str();
724  status=sw=="on";
725  printMessage(log::no_info,1,"simulation status = %s",sw.c_str());
726  return true;
727  }
728  else
729  {
730  printMessage(log::error,1,"something went wrong: request rejected");
731  return false;
732  }
733  }
734  else
735  {
736  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
737  return false;
738  }
739  }
740  else
741  {
742  printMessage(log::warning,1,"client is not open");
743  return false;
744  }
745 }
746 
747 
748 /************************************************************************/
749 bool D4CClient::setPeriod(const int period)
750 {
751  if (isOpen)
752  {
753  printMessage(log::no_info,2,"request for setting period to %s [ms]",period);
754 
755  Bottle cmd,reply;
756  cmd.addVocab(D4C_VOCAB_CMD_SETPER);
757  cmd.addInt(period);
758 
759  if (rpc.write(cmd,reply))
760  {
761  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
762  {
763  printMessage(log::no_info,1,"period successfully updated");
764  return true;
765  }
766  else
767  {
768  printMessage(log::error,1,"something went wrong: request rejected");
769  return false;
770  }
771  }
772  else
773  {
774  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
775  return false;
776  }
777  }
778  else
779  {
780  printMessage(log::warning,1,"client is not open");
781  return false;
782  }
783 }
784 
785 
786 /************************************************************************/
787 bool D4CClient::getPeriod(int &period)
788 {
789  if (isOpen)
790  {
791  printMessage(log::no_info,2,"request for retrieving period");
792 
793  Bottle cmd,reply;
794  cmd.addVocab(D4C_VOCAB_CMD_GETPER);
795 
796  if (rpc.write(cmd,reply))
797  {
798  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
799  {
800  period=reply.get(1).asInt();
801  printMessage(log::no_info,1,"period successfully retrieved: %d [ms]",period);
802  return true;
803  }
804  else
805  {
806  printMessage(log::error,1,"something went wrong: request rejected");
807  return false;
808  }
809  }
810  else
811  {
812  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
813  return false;
814  }
815  }
816  else
817  {
818  printMessage(log::warning,1,"client is not open");
819  return false;
820  }
821 }
822 
823 
824 /************************************************************************/
825 bool D4CClient::setPointStateToTool()
826 {
827  if (isOpen)
828  {
829  printMessage(log::no_info,2,"request for setting state equal to tool state");
830 
831  Bottle cmd,reply;
832  cmd.addVocab(D4C_VOCAB_CMD_SETSTATETOTOOL);
833 
834  if (rpc.write(cmd,reply))
835  {
836  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
837  {
838  printMessage(log::no_info,1,"state successfully updated");
839  return true;
840  }
841  else
842  {
843  printMessage(log::error,1,"something went wrong: request rejected");
844  return false;
845  }
846  }
847  else
848  {
849  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
850  return false;
851  }
852  }
853  else
854  {
855  printMessage(log::warning,1,"client is not open");
856  return false;
857  }
858 }
859 
860 
861 /************************************************************************/
862 bool D4CClient::attachToolFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
863 {
864  if (isOpen)
865  {
866  if ((x.length()<3) || (o.length()<4))
867  {
868  printMessage(log::error,1,"problem with vector lengths");
869  return false;
870  }
871 
872  Value val_x; val_x.fromString(("("+string(x.toString().c_str())+")").c_str());
873  Value val_o; val_o.fromString(("("+string(o.toString().c_str())+")").c_str());
874 
875  Property options;
876  options.put("x",val_x);
877  options.put("o",val_o);
878 
879  string options_str=options.toString().c_str();
880  printMessage(log::no_info,2,"request for attaching tool frame: %s",options_str.c_str());
881 
882  Value val;
883  val.fromString(("("+options_str+")").c_str());
884 
885  Bottle cmd,reply;
886  cmd.addVocab(D4C_VOCAB_CMD_ATTACHTOOLFRAME);
887  cmd.add(val);
888 
889  if (rpc.write(cmd,reply))
890  {
891  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
892  {
893  printMessage(log::no_info,1,"tool frame successfully attached");
894  return true;
895  }
896  else
897  {
898  printMessage(log::error,1,"something went wrong: request rejected");
899  return false;
900  }
901  }
902  else
903  {
904  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
905  return false;
906  }
907  }
908  else
909  {
910  printMessage(log::warning,1,"client is not open");
911  return false;
912  }
913 }
914 
915 
916 /************************************************************************/
917 bool D4CClient::getToolFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
918 {
919  if (isOpen)
920  {
921  printMessage(log::no_info,2,"request for retrieving tool frame");
922 
923  Bottle cmd,reply;
924  cmd.addVocab(D4C_VOCAB_CMD_GETTOOLFRAME);
925 
926  if (rpc.write(cmd,reply))
927  {
928  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
929  {
930  Property options=extractProperty(reply.get(1));
931  extractVector(options,"x",x);
932  extractVector(options,"o",o);
933 
934  printMessage(log::no_info,1,"tool frame successfully retrieved %s",
935  options.toString().c_str());
936  return true;
937  }
938  else
939  {
940  printMessage(log::error,1,"something went wrong: request rejected");
941  return false;
942  }
943  }
944  else
945  {
946  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
947  return false;
948  }
949  }
950  else
951  {
952  printMessage(log::warning,1,"client is not open");
953  return false;
954  }
955 }
956 
957 
958 /************************************************************************/
959 bool D4CClient::removeToolFrame()
960 {
961  if (isOpen)
962  {
963  printMessage(log::no_info,2,"request for removing tool frame");
964 
965  Bottle cmd,reply;
966  cmd.addVocab(D4C_VOCAB_CMD_REMOVETOOLFRAME);
967 
968  if (rpc.write(cmd,reply))
969  {
970  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
971  {
972  printMessage(log::no_info,1,"tool successfully removed");
973  return true;
974  }
975  else
976  {
977  printMessage(log::error,1,"something went wrong: request rejected");
978  return false;
979  }
980  }
981  else
982  {
983  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
984  return false;
985  }
986  }
987  else
988  {
989  printMessage(log::warning,1,"client is not open");
990  return false;
991  }
992 }
993 
994 
995 /************************************************************************/
996 bool D4CClient::getTool(yarp::sig::Vector &x, yarp::sig::Vector &o)
997 {
998  if (isOpen)
999  {
1000  printMessage(log::no_info,2,"request for retrieving tool");
1001 
1002  Bottle cmd,reply;
1003  cmd.addVocab(D4C_VOCAB_CMD_GETTOOL);
1004 
1005  if (rpc.write(cmd,reply))
1006  {
1007  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1008  {
1009  Property options=extractProperty(reply.get(1));
1010  extractVector(options,"x",x);
1011  extractVector(options,"o",o);
1012 
1013  printMessage(log::no_info,1,"tool successfully retrieved %s",
1014  options.toString().c_str());
1015  return true;
1016  }
1017  else
1018  {
1019  printMessage(log::error,1,"something went wrong: request rejected");
1020  return false;
1021  }
1022  }
1023  else
1024  {
1025  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1026  return false;
1027  }
1028  }
1029  else
1030  {
1031  printMessage(log::warning,1,"client is not open");
1032  return false;
1033  }
1034 }
1035 
1036 
1037 /************************************************************************/
1038 bool D4CClient::setPointState(const Vector &x, const Vector &o,
1039  const Vector &xdot, const Vector &odot)
1040 {
1041  if (isOpen)
1042  {
1043  Vector pos(7),vel(7);
1044  copyVectorData(x,pos);
1045  copyVectorData(o,pos);
1046  copyVectorData(xdot,vel);
1047  copyVectorData(odot,vel);
1048 
1049  Value val_x; val_x.fromString(("("+string(pos.toString().c_str())+")").c_str());
1050  Value val_xdot; val_xdot.fromString(("("+string(vel.toString().c_str())+")").c_str());
1051 
1052  Property options;
1053  options.put("x",val_x);
1054  options.put("xdot",val_xdot);
1055 
1056  string options_str=options.toString().c_str();
1057  printMessage(log::no_info,2,"request for setting state: %s",options_str.c_str());
1058 
1059  Value val;
1060  val.fromString(("("+options_str+")").c_str());
1061 
1062  Bottle cmd,reply;
1063  cmd.addVocab(D4C_VOCAB_CMD_SETSTATE);
1064  cmd.add(val);
1065 
1066  if (rpc.write(cmd,reply))
1067  {
1068  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1069  {
1070  printMessage(log::no_info,1,"state successfully updated");
1071  return true;
1072  }
1073  else
1074  {
1075  printMessage(log::error,1,"something went wrong: request rejected");
1076  return false;
1077  }
1078  }
1079  else
1080  {
1081  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1082  return false;
1083  }
1084  }
1085  else
1086  {
1087  printMessage(log::warning,1,"client is not open");
1088  return false;
1089  }
1090 }
1091 
1092 
1093 /************************************************************************/
1094 bool D4CClient::setPointOrientation(const Vector &o, const Vector &odot)
1095 {
1096  if (isOpen)
1097  {
1098  Value val_o; val_o.fromString(("("+string(o.toString().c_str())+")").c_str());
1099  Value val_odot; val_odot.fromString(("("+string(odot.toString().c_str())+")").c_str());
1100 
1101  Property options;
1102  options.put("o",val_o);
1103  options.put("odot",val_odot);
1104 
1105  string options_str=options.toString().c_str();
1106  printMessage(log::no_info,2,"request for setting orientation: %s",options_str.c_str());
1107 
1108  Value val;
1109  val.fromString(("("+options_str+")").c_str());
1110 
1111  Bottle cmd,reply;
1112  cmd.addVocab(D4C_VOCAB_CMD_SETORIEN);
1113  cmd.add(val);
1114 
1115  if (rpc.write(cmd,reply))
1116  {
1117  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1118  {
1119  printMessage(log::no_info,1,"orientation successfully updated");
1120  return true;
1121  }
1122  else
1123  {
1124  printMessage(log::error,1,"something went wrong: request rejected");
1125  return false;
1126  }
1127  }
1128  else
1129  {
1130  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1131  return false;
1132  }
1133  }
1134  else
1135  {
1136  printMessage(log::warning,1,"client is not open");
1137  return false;
1138  }
1139 }
1140 
1141 
1142 /************************************************************************/
1143 bool D4CClient::getPointState(Vector &x, Vector &o, Vector &xdot, Vector &odot)
1144 {
1145  if (isOpen)
1146  {
1147  if (Property *prop=data.read(false))
1148  {
1149  extractVector(*prop,"x",this->x);
1150  extractVector(*prop,"xdot",this->xdot);
1151  }
1152 
1153  x=getVectorPos(this->x);
1154  o=getVectorOrien(this->x);
1155  xdot=getVectorPos(this->xdot);
1156  odot=getVectorOrien(this->xdot);
1157 
1158  printMessage(log::no_info,1,"got state: x = %s xdot = %s",
1159  this->x.toString().c_str(),this->xdot.toString().c_str());
1160  return true;
1161  }
1162  else
1163  {
1164  printMessage(log::warning,1,"client is not open");
1165  return false;
1166  }
1167 }
1168 
1169 
1170 /************************************************************************/
1171 bool D4CClient::getField(Vector &field)
1172 {
1173  if (isOpen)
1174  {
1175  if (Property *prop=data.read(false))
1176  extractVector(*prop,"field",this->field);
1177 
1178  field=this->field;
1179 
1180  printMessage(log::no_info,1,"got field: field=%s",
1181  this->field.toString().c_str());
1182  return true;
1183  }
1184  else
1185  {
1186  printMessage(log::warning,1,"client is not open");
1187  return false;
1188  }
1189 }
1190 
1191 
1192 /************************************************************************/
1193 bool D4CClient::getSimulation(Vector &xhat, Vector &ohat, Vector &qhat)
1194 {
1195  if (isOpen)
1196  {
1197  if (Property *prop=data.read(false))
1198  {
1199  extractVector(*prop,"xhat",this->xhat);
1200  extractVector(*prop,"qhat",this->qhat);
1201  }
1202 
1203  xhat=getVectorPos(this->xhat);
1204  ohat=getVectorOrien(this->xhat);
1205  qhat=this->qhat;
1206 
1207  printMessage(log::no_info,1,"got simulated end-effector pose: xhat = %s; got part configuration: qhat = %s",
1208  this->xhat.toString().c_str(),this->qhat.toString().c_str());
1209  return true;
1210  }
1211  else
1212  {
1213  printMessage(log::warning,1,"client is not open");
1214  return false;
1215  }
1216 }
1217 
1218 
1219 /************************************************************************/
1220 bool D4CClient::getActiveIF(string &activeIF)
1221 {
1222  if (isOpen)
1223  {
1224  printMessage(log::no_info,2,"request to know the active interface");
1225 
1226  Bottle cmd,reply;
1227  cmd.addVocab(D4C_VOCAB_CMD_GETACTIF);
1228 
1229  if (rpc.write(cmd,reply))
1230  {
1231  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1232  {
1233  activeIF=reply.get(1).asString().c_str();
1234  printMessage(log::no_info,1,"active interface: %s",activeIF.c_str());
1235  return true;
1236  }
1237  else
1238  {
1239  printMessage(log::error,1,"something went wrong: request rejected");
1240  return false;
1241  }
1242  }
1243  else
1244  {
1245  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1246  return false;
1247  }
1248  }
1249  else
1250  {
1251  printMessage(log::warning,1,"client is not open");
1252  return false;
1253  }
1254 }
1255 
1256 
1257 /************************************************************************/
1258 bool D4CClient::setActiveIF(const string &activeIF)
1259 {
1260  if (isOpen)
1261  {
1262  printMessage(log::no_info,2,"request to set the active interface");
1263 
1264  Bottle cmd,reply;
1265  cmd.addVocab(D4C_VOCAB_CMD_SETACTIF);
1266  cmd.addString(activeIF.c_str());
1267 
1268  if (rpc.write(cmd,reply))
1269  {
1270  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1271  {
1272  printMessage(log::no_info,1,"active interface successfully set to %s",activeIF.c_str());
1273  return true;
1274  }
1275  else
1276  {
1277  printMessage(log::error,1,"something went wrong: request rejected");
1278  return false;
1279  }
1280  }
1281  else
1282  {
1283  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1284  return false;
1285  }
1286  }
1287  else
1288  {
1289  printMessage(log::warning,1,"client is not open");
1290  return false;
1291  }
1292 }
1293 
1294 
1295 /************************************************************************/
1296 bool D4CClient::getTrajectory(deque<Vector> &trajPos, deque<Vector> &trajOrien,
1297  const unsigned int maxIterations, const double Ts)
1298 {
1299  if (isOpen)
1300  {
1301  printMessage(log::no_info,2,"request to retrieve the whole trajectory");
1302 
1303  Bottle cmd,reply;
1304  cmd.addVocab(D4C_VOCAB_CMD_GETTRAJ);
1305 
1306  Property options;
1307  options.put("maxIterations",(int)maxIterations);
1308  options.put("Ts",Ts);
1309 
1310  string options_str=options.toString().c_str();
1311 
1312  Value val;
1313  val.fromString(("("+options_str+")").c_str());
1314 
1315  cmd.add(val);
1316  if (rpc.write(cmd,reply))
1317  {
1318  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1319  {
1320  bool okPos=false;
1321  if (Bottle *BtrajPos=reply.get(1).asList())
1322  {
1323  if (BtrajPos->get(0).asString()=="trajPos")
1324  {
1325  trajPos.clear();
1326  for (int i=1; i<BtrajPos->size(); i++)
1327  {
1328  if (Bottle *point=BtrajPos->get(i).asList())
1329  {
1330  Vector pos(point->size());
1331  for (int j=0; j<point->size(); j++)
1332  pos[j]=point->get(j).asDouble();
1333 
1334  trajPos.push_back(pos);
1335  }
1336  }
1337 
1338  okPos=true;
1339  printMessage(log::no_info,1,"trajectory in position has been computed");
1340  }
1341  }
1342 
1343  bool okOrien=false;
1344  if (Bottle *BtrajOrien=reply.get(2).asList())
1345  {
1346  if (BtrajOrien->get(0).asString()=="trajOrien")
1347  {
1348  trajOrien.clear();
1349  for (int i=1; i<BtrajOrien->size(); i++)
1350  {
1351  if (Bottle *point=BtrajOrien->get(i).asList())
1352  {
1353  Vector orien(point->size());
1354  for (int j=0; j<point->size(); j++)
1355  orien[j]=point->get(j).asDouble();
1356 
1357  trajOrien.push_back(orien);
1358  }
1359  }
1360 
1361  okOrien=true;
1362  printMessage(log::no_info,1,"trajectory in orientation has been computed");
1363  }
1364  }
1365 
1366  if (okPos && okOrien)
1367  return true;
1368  }
1369 
1370  printMessage(log::error,1,"something went wrong: request rejected");
1371  return false;
1372  }
1373  else
1374  {
1375  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1376  return false;
1377  }
1378  }
1379  else
1380  {
1381  printMessage(log::warning,1,"client is not open");
1382  return false;
1383  }
1384 }
1385 
1386 
1387 /************************************************************************/
1388 bool D4CClient::executeTrajectory(const deque<Vector> &trajPos, const deque<Vector> &trajOrien,
1389  const double trajTime)
1390 {
1391  if (isOpen)
1392  {
1393  printMessage(log::no_info,2,"request to execute user trajectory");
1394 
1395  if (trajPos.size()!=trajOrien.size())
1396  {
1397  printMessage(log::error,1,"position and orientation data have different size!");
1398  return false;
1399  }
1400  else if (trajTime<0.0)
1401  {
1402  printMessage(log::error,1,"negative trajectory duration provided!");
1403  return false;
1404  }
1405 
1406  Bottle cmd,reply;
1407  cmd.addVocab(D4C_VOCAB_CMD_EXECTRAJ);
1408  Bottle &cmdOptions=cmd.addList();
1409 
1410  Bottle &bTrajTime=cmdOptions.addList();
1411  bTrajTime.addString("trajTime");
1412  bTrajTime.addDouble(trajTime);
1413 
1414  Bottle &bPos=cmdOptions.addList();
1415  bPos.addString("trajPos");
1416  for (unsigned int i=0; i<trajPos.size(); i++)
1417  {
1418  Bottle &point=bPos.addList();
1419  for (size_t j=0; j<trajPos[i].length(); j++)
1420  point.addDouble(trajPos[i][j]);
1421  }
1422 
1423  Bottle &bOrien=cmdOptions.addList();
1424  bOrien.addString("trajOrien");
1425  for (unsigned int i=0; i<trajOrien.size(); i++)
1426  {
1427  Bottle &point=bOrien.addList();
1428  for (size_t j=0; j<trajOrien[i].length(); j++)
1429  point.addDouble(trajOrien[i][j]);
1430  }
1431 
1432  if (rpc.write(cmd,reply))
1433  {
1434  if (reply.get(0).asVocab()==D4C_VOCAB_CMD_ACK)
1435  {
1436  printMessage(log::no_info,1,"trajectory executed");
1437  return true;
1438  }
1439 
1440  printMessage(log::error,1,"something went wrong: request rejected");
1441  return false;
1442  }
1443  else
1444  {
1445  printMessage(log::error,1,"unable to get reply from the server %s!",remote.c_str());
1446  return false;
1447  }
1448  }
1449  else
1450  {
1451  printMessage(log::warning,1,"client is not open");
1452  return false;
1453  }
1454 }
1455 
1456 
iCub::d4c
Definition: d4c.h:46
Generated on Sun May 31 2015 15:17:45 for d4c by   doxygen 1.8.9