karma
•All Modules
module.cpp
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Vadim Tikhanoff Ugo Pattacini
4  * email: vadim.tikhanoff@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 <cmath>
19 #include <sstream>
20 #include <cstdio>
21 #include <limits>
22 
23 #include <yarp/math/Rand.h>
24 #include <yarp/math/Math.h>
25 
26 #include "iCub/module.h"
27 
28 using namespace std;
29 using namespace yarp::os;
30 using namespace yarp::sig;
31 using namespace yarp::math;
32 
33 #define RET_INVALID -1
34 
35 #define CMD_TRAIN yarp::os::createVocab('t','r','a','i')
36 #define CMD_EXECUTE yarp::os::createVocab('e','x','e','c')
37 #define CMD_TOOLEXTEND yarp::os::createVocab('e','x','t','d')
38 
39 /**********************************************************/
40 bool Manager::configure(ResourceFinder &rf)
41 {
42  name=rf.find("name").asString();
43  camera=rf.find("camera").asString();
44  if ((camera!="left") && (camera!="right"))
45  camera="left";
46 
47  hand=rf.find("hand").asString();
48  if ((hand!="left") && (hand!="right"))
49  hand="left";
50 
51  //incoming
52  particleFilter.open("/"+name+"/particle:i");
53  pointedLoc.open("/"+name+"/point:i");
54  blobExtractor.open("/"+name+"/blobs:i");
55 
56  //outgoing
57  segmentPoint.open("/"+name+"/segmentTarget:o"); //port to send off target Points to segmentator
58  iolStateMachine.open("/"+name+"/iolState:o");
59 
60  //rpc
61  rpcMIL.open("/"+name+"/mil:rpc"); //rpc client to query mil classifications
62  rpcHuman.open("/"+name+"/human:rpc"); //rpc server to interact with the italkManager
63  rpcMotorAre.open("/"+name+"/are:rpc"); //rpc server to query ARE
64  rpcMotorKarma.open("/"+name+"/karma:rpc"); //rpc server to query Karma
65  rpcKarmaLearn.open("/"+name+"/learn:rpc");
66  rpcReconstruct.open("/"+name+"/reconstruct:rpc");
67  rpcGraspEstimate.open("/"+name+"/graspestimate:rpc");
68  rpcOPC.open("/"+name+"/opc:rpc");
69 
70  Rand::init();
71 
72  randActions[0] = 0.0;
73  randActions[1] = 30.0;
74  randActions[2] = 150.0;
75  randActions[3] = 180.0;
76  randActions[4] = 225.0;
77  //randActions[5] = 270.0;
78  randActions[5] = 315.0;
79 
80  pointGood=false;
81  init=false;
82 
83  idleTmo = 60.0;
84  toolSmall.resize(3);
85  toolBig.resize(3);
86 
87  toolSmall[0] = 0.18;
88  toolSmall[1] = -0.17;
89  toolSmall[2] = -0.04;
90 
91  toolBig[0] = 0.29;
92  toolBig[1] = -0.26;
93  toolBig[2] = -0.05;
94 
95  //attach(rpcHuman);
96  return true;
97 }
98 
99 /**********************************************************/
100 bool Manager::interruptModule()
101 {
102  segmentPoint.interrupt();
103  blobExtractor.interrupt();
104  rpcHuman.interrupt();
105  rpcMotorAre.interrupt();
106  rpcMotorKarma.interrupt();
107  particleFilter.interrupt();
108  pointedLoc.interrupt();
109  iolStateMachine.interrupt();
110  rpcMIL.interrupt();
111  rpcKarmaLearn.interrupt();
112  rpcReconstruct.interrupt();
113  rpcGraspEstimate.interrupt();
114  rpcOPC.interrupt();
115 
116  return true;
117 }
118 /**********************************************************/
119 bool Manager::close()
120 {
121  segmentPoint.close();
122  blobExtractor.close();
123  rpcHuman.close();
124  rpcMotorAre.close();
125  rpcMotorKarma.close();
126  particleFilter.close();
127  pointedLoc.close();
128  iolStateMachine.close();
129  rpcMIL.close();
130  rpcKarmaLearn.close();
131  rpcReconstruct.close();
132  rpcGraspEstimate.close();
133  rpcOPC.close();
134 
135  return true;
136 }
137 /**********************************************************/
138 int Manager::processHumanCmd(const Bottle &cmd, Bottle &b)
139 {
140  int ret=Vocab::encode(cmd.get(0).asString());
141  b.clear();
142  if (cmd.size()>1)
143  {
144  if (cmd.get(1).isList())
145  b=*cmd.get(1).asList();
146  else
147  b=cmd.tail();
148  }
149  return ret;
150 }
151 /**********************************************************/
152 bool Manager::updateModule()
153 {
154  if (isStopping())
155  return false;
156 
157  //init = true;//bypass for now
158 
159  while (!init)
160  {
161  Time::delay(0.5);
162  fprintf(stdout, "waiting for connection from iolStateMachineHandler\n");
163  if (iolStateMachine.getOutputCount() > 0)
164  {
165  fprintf(stdout, "sending home \n");
166  Bottle cmdIol, replyIol;
167  cmdIol.addString("home");
168  iolStateMachine.write(cmdIol,replyIol);
169  fprintf(stdout,"%s\n", replyIol.toString().c_str());
170  fprintf(stdout,"stopping attention...\n");
171  cmdIol.clear();
172  replyIol.clear();
173  cmdIol.addString("attention");
174  cmdIol.addString("stop");
175  iolStateMachine.write(cmdIol,replyIol);
176  fprintf(stdout,"%s\n", replyIol.toString().c_str());
177  init = true;
178  fprintf(stdout, "have successfully initialize it all\n");
179 
180  executeSpeech ("ok I am ready");
181  }
182  }
183 
184  Bottle cmd, val, reply;
185  rpcHuman.read(cmd, true);
186 
187  int rxCmd=processHumanCmd(cmd,val);
188  if (rxCmd==Vocab::encode("train"))
189  {
190  obj=cmd.get(1).asString();
191 
192  if (cmd.size() > 2)
193  {
194  fprintf(stdout,"with user data\n");
195  userTheta=cmd.get(2).asDouble();
196  }
197  else
198  {
199  fprintf(stdout,"no user data\n");
200  userTheta = -1.0;
201  }
202  //pointGood = pointedLoc.getLoc(pointLocation);
203  //Time::delay(1.5);
204  executeOnLoc(true);
205  reply.addString("ack");
206  rpcHuman.reply(reply);
207  pointGood = false;
208  }
209  if (rxCmd==Vocab::encode("test"))
210  {
211  obj=cmd.get(1).asString();
212 
213  //pointGood = pointedLoc.getLoc(pointLocation);
214  //Time::delay(1.5);
215  executeOnLoc(false);
216  reply.addString("ack");
217  rpcHuman.reply(reply);
218  pointGood = false;
219  }
220  if (rxCmd==Vocab::encode("handle"))
221  {
222  takeMotionARE();
223  reply.addString("ack");
224  rpcHuman.reply(reply);
225  }
226 
227  if (rxCmd==Vocab::encode("extend"))
228  {
229  if (cmd.size() > 2)
230  {
231  fprintf(stdout, "Will now use user selected arm and camera \n");
232  hand = cmd.get(1).asString();
233  camera = cmd.get(2).asString();
234  }
235  else
236  {
237  fprintf(stdout, "Will now use default arm and cam \n");
238  }
239 
240  lastTool.clear();
241  lastTool = executeToolLearning();
242  reply.addString("ack");
243  rpcHuman.reply(reply);
244 
245  fprintf(stdout, "GOT TOOL at %s, \n",lastTool.toString().c_str());
246  goHomeArmsHead();
247  }
248  if (rxCmd==Vocab::encode("tooltip"))
249  {
250  Bottle toSend;
251  toSend.clear();
252  toSend.addDouble(lastTool.get(0).asDouble());
253  toSend.addDouble(lastTool.get(1).asDouble());
254  toSend.addDouble(lastTool.get(2).asDouble());
255  //reply.addString("ack");
256  rpcHuman.reply(toSend);
257 
258  fprintf(stdout, "Replied with %s, \n",toSend.toString().c_str());
259  //goHomeArmsHead();
260  }
261  if (rxCmd==Vocab::encode("grasp"))
262  {
263  obj=cmd.get(1).asString();
264  if ( executePCLGrasp(obj) )
265  reply.addString("ack");
266  else
267  reply.addString("nack");
268  }
269  if (rxCmd==Vocab::encode("blobLoc"))
270  {
271  blobLoc.clear();
272  blobLoc = findBlobLoc();
273  if (blobLoc.size()<1)
274  {
275  reply.addString("nack");
276  rpcHuman.reply(reply);
277  }
278  else
279  rpcHuman.reply(blobLoc);
280  }
281 
282  if (rxCmd==Vocab::encode("objRecog"))
283  {
284  blobList.clear();
285  obj=cmd.get(1).asString();
286  blobLoc = executeBlobRecog(obj);
287 
288  if (blobLoc.size()<1)
289  {
290  reply.addString("nack");
291  rpcHuman.reply(reply);
292  }
293  else
294  rpcHuman.reply(blobLoc);
295  }
296  if (rxCmd==Vocab::encode("close"))
297  {
298  int arm=cmd.get(1).asInt();
299  executeCloseHand(arm);
300  reply.addString("ack");
301  rpcHuman.reply(reply);
302  }
303  if (rxCmd==Vocab::encode("give"))
304  {
305  int arm=cmd.get(1).asInt();
306  executeGiveAction(arm);
307  reply.addString("ack");
308  rpcHuman.reply(reply);
309  }
310  if (rxCmd==Vocab::encode("class"))
311  {
312  reply = classifyThem();
313  rpcHuman.reply(reply);
314  }
315 
316  Bottle result;
317  result.clear();
318 
319  return true;
320 }
321 
322 /**********************************************************/
323 Bottle Manager::classifyThem()
324 {
325  Bottle cmdIol;
326  Bottle replyIol;
327  cmdIol.clear(), replyIol.clear();
328  cmdIol.addString("class");
329  iolStateMachine.write(cmdIol,replyIol);
330  fprintf(stdout,"%s\n", replyIol.toString().c_str());
331 
332  return replyIol;
333 }
334 /**********************************************************/
335 void Manager::segmentAndTrack( int x, int y )
336 {
337  Bottle toSegment;
338  toSegment.clear();
339 
340  toSegment.addInt(x); //(closestBlob.get(0).asInt());
341  toSegment.addInt(y); //(closestBlob.get(1).asInt());
342  toSegment.addInt(80);
343  toSegment.addInt(80);
344  fprintf(stdout, "segmenting cmd is %s\n",toSegment.toString().c_str());
345  segmentPoint.write(toSegment);
346 
347  Bottle cmdAre, replyAre;
348  cmdAre.addString("track");
349  cmdAre.addString("track");
350  cmdAre.addString("no_sacc");
351  rpcMotorAre.write(cmdAre,replyAre);
352  fprintf(stdout,"tracking started%s:\n",replyAre.toString().c_str());
353 }
354 /**********************************************************/
355 void Manager::goHomeArmsHead()
356 {
357  Bottle cmdAre, replyAre;
358  cmdAre.clear();
359  replyAre.clear();
360  cmdAre.addString("home");
361  cmdAre.addString("arms");
362  cmdAre.addString("head");
363  rpcMotorAre.write(cmdAre,replyAre);
364  fprintf(stdout,"gone home %s:\n",replyAre.toString().c_str());
365 }
366 /**********************************************************/
367 void Manager::goHome()
368 {
369  Bottle cmdAre, replyAre;
370  cmdAre.clear();
371  replyAre.clear();
372  cmdAre.addString("home");
373  cmdAre.addString("all");
374  rpcMotorAre.write(cmdAre,replyAre);
375  fprintf(stdout,"gone home %s:\n",replyAre.toString().c_str());
376 }
377 /**********************************************************/
378 void Manager::takeMotionARE()
379 {
380  //to fill with data for karmaMotor
381  fprintf(stdout,"Will now start take motion proceedure:\n");
382  Bottle cmdAre,replyAre;
383  cmdAre.clear();
384  replyAre.clear();
385  cmdAre.addString("take");
386  cmdAre.addString("motion");
387  fprintf(stdout,"%s\n",cmdAre.toString().c_str());
388  rpcMotorAre.write(cmdAre, replyAre);
389  fprintf(stdout,"action is %s:\n",replyAre.toString().c_str());
390 }
391 
392 /**********************************************************/
393 Bottle Manager::executeToolLearning()
394 {
395  //to fill with data for karmaMotor
396  fprintf(stdout,"Will now start the tool learn proceedure:\n");
397  Bottle karmaMotor,KarmaReply;
398  karmaMotor.clear();
399  KarmaReply.clear();
400  karmaMotor.addString("find");
401  karmaMotor.addString(hand);
402  karmaMotor.addString(camera);
403  fprintf(stdout,"%s\n",karmaMotor.toString().c_str());
404  rpcMotorKarma.write(karmaMotor, KarmaReply);
405  fprintf(stdout,"action is %s:\n",KarmaReply.toString().c_str());
406 
407  Bottle toReturn;
408  toReturn.clear();
409  toReturn.addDouble( KarmaReply.get(1).asDouble() );
410  toReturn.addDouble( KarmaReply.get(2).asDouble() );
411  toReturn.addDouble( KarmaReply.get(3).asDouble() );
412  return toReturn;
413 }
414 /**********************************************************/
415 Bottle Manager::executeBlobRecog(const string &objName)
416 {
417  Bottle loc;
418  loc.clear();
419  fprintf(stdout, "\n\n\nexecuteBlobRecog****************************************************************************\n\n" );
420 
421  Bottle blobs;
422  blobs.clear();
423  bool invalid = false;
424 
425  // grab the blobs
426  blobs=getBlobs();
427  // failure handling
428  if (blobs.size()==0)
429  {
430  fprintf (stdout,"INVALID BLOB SIZE\n");
431  invalid = true;
432  }
433  latchTimer = 0.0;
434 
435  if ( !invalid )
436  {
437  Bottle classResults = classifyThem();
438 
439  for (int x=0; x < blobs.size(); x++)
440  {
441  pointLocation = getBlobCOG(blobs,x);
442  fprintf (stdout,"point is %d %d \n", pointLocation.x, pointLocation.y);
443  Bottle closestBlob;
444  mutexResources.lock();
445  closestBlob=findClosestBlob(blobs,pointLocation);
446  mutexResources.unlock();
447 
448  cv::Point cog;
449  cog.x = closestBlob.get(0).asInt();
450  cog.y = closestBlob.get(1).asInt();
451 
452  int index = 0;
453  index = closestBlob.get(2).asInt();
454 
455  fprintf(stdout, "closest blob is x:%d y:%d with index %d\n\n",cog.x, cog.y, index);
456 
457  //classify the blob
458  //Bottle mil;
459  //fprintf(stdout,"ask to classify\n");
460  //mil=classify(blobs, index);
461  //fprintf(stdout,"done classifying\n");
462  //get the type of the blob
463  //Bottle type;
464  //fprintf(stdout,"ask to get type\n");
465  //type=getType(&mil, index);
466 
467  fprintf(stdout, "The type is: %s and object name requested is: %s\n\n", classResults.get(x).asString().c_str(), objName.c_str() );
468 
469  if (classResults.get(x).asString() == objName)
470  {
471 
472  pointLocation = getBlobCOG(blobs,index);
473  fprintf (stdout,"point is %d %d \n", pointLocation.x, pointLocation.y);
474  fprintf(stdout,"I have found the requested object %s, at: %d %d\n",objName.c_str(), pointLocation.x, pointLocation.y );
475  loc.addInt (pointLocation.x);
476  loc.addInt (pointLocation.y);
477  }
478  }
479  }
480 
481  return loc;
482 }
483 
484 /**********************************************************/
485 int Manager::executeToolOnLoc()
486 {
487  fprintf(stdout, "\n\n\n****************************************************************************\n\n" );
488  Bottle blobs;
489  blobs.clear();
490  // grab the blobs
491  blobs=getBlobs();
492  // failure handling
493  if (blobs.size()==0)
494  return RET_INVALID;
495 
496  if (blobs.size()<2)
497  {
498  pointLocation = getBlobCOG(blobs,0);
499  fprintf (stdout,"point is %d %d \n", pointLocation.x, pointLocation.y);
500  pointGood = true;
501  }
502  else
503  {
504  fprintf (stdout,"I see more than two blobs\n");
505  pointGood = false;
506  }
507  if (pointGood)
508  {
509  Bottle closestBlob;
510  mutexResources.lock();
511  closestBlob=findClosestBlob(blobs,pointLocation);
512  mutexResources.unlock();
513 
514  cv::Point cog;
515  cog.x = closestBlob.get(0).asInt();
516  cog.y = closestBlob.get(1).asInt();
517 
518  /*
519  * segment the object
520  */
521  segmentAndTrack(cog.x, cog.y);
522 
523  int index = 0;
524  index = closestBlob.get(2).asInt();
525 
526 
527  fprintf(stdout, "closest blob is x:%d y:%d with index %d\n\n", cog.x, cog.y, index);
528  }
529  return 0;
530 }
531 /**********************************************************/
532 Bottle Manager::findBlobLoc()
533 {
534  Bottle loc;
535  loc.clear();
536  fprintf(stdout, "\n\n\nfindBlobLoc****************************************************************************\n\n" );
537  Bottle blobs;
538  blobs.clear();
539  bool invalid = false;
540  // grab the blobs
541  blobs=getBlobs();
542  // failure handling
543  if (blobs.size()==0)
544  {
545  fprintf (stdout,"INVALID BLOB SIZE\n");
546  invalid = true;
547  }
548 
549  latchTimer = 0.0;
550 
551  if (blobs.size()<2 && !invalid)
552  {
553  pointLocation = getBlobCOG(blobs,0);
554  fprintf (stdout,"point is %d %d \n", pointLocation.x, pointLocation.y);
555  pointGood = true;
556  }
557  else
558  {
559  fprintf (stdout,"I either see more than two blobs or nothing at all\n");
560  pointGood = false;
561  }
562  if (pointGood)
563  {
564  Bottle closestBlob;
565  mutexResources.lock();
566  closestBlob=findClosestBlob(blobs,pointLocation);
567  mutexResources.unlock();
568 
569  cv::Point cog;
570  cog.x = closestBlob.get(0).asInt();
571  cog.y = closestBlob.get(1).asInt();
572 
573  /*
574  * segment the object
575  */
576  //segmentAndTrack(cog.x, cog.y);
577 
578  int index = 0;
579  index = closestBlob.get(2).asInt();
580 
581  fprintf(stdout, "closest blob is x:%d y:%d with index %d\n\n", cog.x, cog.y, index);
582 
583  Vector initPos;
584  if (get3DPosition(cog,initPos))
585  {
586  loc.addDouble(initPos[0]);
587  loc.addDouble(initPos[1]);
588  loc.addDouble(initPos[2]);
589  fprintf(stdout,"Got a 3D point at %lf %lf %lf \n", initPos[0], initPos[1],initPos[2]);
590  }
591  }
592  return loc;
593 }
594 
595 /**********************************************************/
596 bool Manager::executePCLGrasp( const string &objName )
597 {
598  fprintf(stdout, "\n\n\nexecutePCLGrasp****************************************************************************\n\n" );
599  executeSpeech ("ok, will now try to grasp the "+ objName);
600  Bottle blobs;
601  blobs.clear();
602  bool invalid = false;
603  bool isGrasped = false;
604  cv::Point locObj;
605  Bottle result;
606  result.clear();
607 
608  fprintf(stdout, "releasing head\n");
609  //here put head in bind 5.0 and in idle
610  Bottle cmdAre, replyAre;
611  cmdAre.clear();
612  replyAre.clear();
613  cmdAre.addString("release");
614  rpcMotorAre.write(cmdAre,replyAre);
615  fprintf(stdout, "the reply is: %s \n",replyAre.toString().c_str());
616 
617  Time::delay(3.0);
618  // grab the blobs
619  blobs=getBlobs();
620  // failure handling
621  if (blobs.size()==0)
622  {
623  invalid = true;
624  return false;
625  }
626  latchTimer = 0.0;
627 
628  if ( !invalid )
629  {
630  result = executeBlobRecog(objName);
631  locObj.x = result.get(0).asInt();
632  locObj.y = result.get(1).asInt();
633 
634  fprintf (stdout,"point is %d %d \n", locObj.x, locObj.y);
635  pointGood = true;
636  }
637  else
638  {
639  fprintf (stdout,"I have an issue spotting the %s\n",objName.c_str());
640  executeSpeech ("I can't seem to find the "+ objName);
641  pointGood = false;
642  }
643  if (pointGood)
644  {
645  objectPos.clear();
646  if (get3DPosition(locObj,objectPos))
647  {
648  fprintf(stdout,"Got a 3D point at %lf %lf %lf \n", objectPos[0], objectPos[1],objectPos[2]);
649  //check 3D point
650 
651  if (objectPos[0] < -0.47 )
652  {
653  executeSpeech ("I cannot safely reach for the "+ objName + ", will try to find a solution");
654  fprintf(stdout, "I cannot reach for this object, will find a solution\n");
655  Bottle cmdHome, cmdReply;
656  cmdHome.clear();
657  cmdReply.clear();
658  cmdHome.addString("home");
659  cmdHome.addString("all");
660  rpcMotorAre.write(cmdHome,cmdReply);
661  Time::delay(2.0);
662  executeToolSearchOnLoc( objName );
663  }
664  else
665  {
666  //should figure out if the 3D position is close enough to grasp or need tool to reach it
667  fprintf(stdout,"Will now send the blob to graphsegmentation:\n");
668  Bottle segCmd, segReply;
669  segCmd.clear();
670  segReply.clear();
671  segCmd.addInt(locObj.x);
672  segCmd.addInt(locObj.y);
673  fprintf(stdout, "the cmd is: %s \n",segCmd.toString().c_str());
674  rpcReconstruct.write(segCmd, segReply);
675  fprintf(stdout, "the reply is: %s \n",segReply.toString().c_str());
676 
677  fprintf(stdout,"Will now ask for 3Dreconstruction:\n");
678  segCmd.clear();
679  segReply.clear();
680  segCmd.addString("3Drec");
681  segCmd.addString(objName);
682 
683  fprintf(stdout, "the cmd is: %s \n", segCmd.toString().c_str());
684  rpcReconstruct.write(segCmd, segReply);
685  fprintf(stdout, "the reply is: %s \n",segReply.toString().c_str());
686 
687  //should now ask if the grasp action has been accomplished
688  Bottle cmd, reply;
689  latchTimer=Time::now();
690 
691  while (!isGrasped)
692  {
693  cmd.clear();
694  reply.clear();
695  cmd.addString("isGrasped");
696  fprintf(stdout, "the cmd is: %s \n", cmd.toString().c_str());
697  rpcGraspEstimate.write(cmd,reply);
698  string rep=reply.toString().c_str();
699  rep.erase (rep.begin());
700  rep.erase (rep.begin()+4);
701 
702  if (rep == "true")
703  isGrasped = true;
704 
705  Time::delay(0.5);
706  if ((Time::now()-latchTimer)>idleTmo)
707  {
708  fprintf(stdout,"--- Timeout elapsed ---\n");
709  break;
710  }
711  }
712 
713  if (isGrasped)
714  {
715  fprintf(stdout, "Grasped finished\n");
716  Time::delay(3.0);
717  fprintf(stdout, "Now Releasing...\n");
718 
719  cmd.clear();
720  reply.clear();
721  cmd.addString("release");
722  fprintf(stdout, "the cmd is: %s \n", cmd.toString().c_str());
723  rpcGraspEstimate.write(cmd,reply);
724  fprintf(stdout, "the reply is: %s \n",reply.toString().c_str());
725  Time::delay(3.0);
726  }
727  else
728  {
729  Bottle cmd, rep;
730  cmd.clear();
731  rep.clear();
732  cmd.addString("home");
733  cmd.addString("all");
734  rpcMotorAre.write(cmd,rep);
735 
736  Bottle cmdAre, replyAre;
737  cmdAre.clear();
738  replyAre.clear();
739  cmdAre.addString("release");
740  rpcMotorAre.write(cmdAre,replyAre);
741  fprintf(stdout, "the reply is: %s \n",replyAre.toString().c_str());
742 
743  executeSpeech ("sorry I could not figure out how to do this");
744  fprintf(stdout, "did not seem to be able to grasp correctly or has been aborted\n");
745  }
746  }
747  }
748  fprintf(stdout, "Finished the grasping sequence \n");
749 
750  Bottle cmd, rep;
751  cmd.clear();
752  rep.clear();
753  cmd.addString("home");
754  cmd.addString("all");
755  rpcMotorAre.write(cmd,rep);
756 
757  }
758  return isGrasped;
759 }
760 
761 /**********************************************************/
762 
763 int Manager::executeToolSearchOnLoc( const string &objName )
764 {
765  fprintf(stdout, "\n\n\nexecuteToolSearchOnLoc****************************************************************************\n\n" );
766  Bottle blobs;
767  blobs.clear();
768  // grab the blobs
769  blobs=getBlobs();
770  // failure handling
771  Bottle result;
772  cv::Point objLoc;
773 
774  int objIndex = 0;
775 
776  if (blobs.size()==0)
777  return RET_INVALID;
778 
779  result = executeBlobRecog(objName);
780  objLoc.x = result.get(0).asInt();
781  objLoc.y = result.get(1).asInt();
782 
783  blobsDetails = new blobsData[blobs.size()];
784 
785  Bottle memCmd, memRep;
786  memCmd.clear(); memRep.clear();
787  memCmd.addVocab(Vocab::encode("ask"));
788  Bottle &tmp=memCmd.addList();
789  tmp.addString ("all");
790  rpcOPC.write(memCmd,memRep);
791 
792  fprintf(stdout, "THE REPLY IS %s \n",memRep.toString().c_str());
793 
794  int sizeInMem =0;
795 
796  if (memRep.get(0).asVocab()==Vocab::encode("ack"))
797  {
798  if (Bottle *idField = memRep.get(1).asList())
799  {
800  if (Bottle *idVals = idField->get(1).asList())
801  {
802  sizeInMem = idVals->size();
803  sizeInMem = sizeInMem - 1; //remove the table
804  fprintf (stdout,"******************************************size is %d \n", sizeInMem);
805  }
806  }
807  }
808 
809 
810 
811  bool valid = false;
812  if (blobs.size() == sizeInMem)
813  valid = true;
814  else
815  executeSpeech ("sorry Something is wrong with the objects");
816 
817 
818  Bottle classResults = classifyThem();
819 
820  if (valid)
821  {
822  //figure out how many blobs are available
823  for (int x=0; x < blobs.size(); x++)
824  {
825  pointLocation = getBlobCOG(blobs,x);
826  fprintf (stdout,"point is %d %d \n", pointLocation.x, pointLocation.y);
827  fprintf (stdout,"object is %d %d \n", objLoc.x, objLoc.y);
828 
829  Bottle closestBlob;
830  mutexResources.lock();
831  closestBlob=findClosestBlob(blobs,pointLocation);
832  mutexResources.unlock();
833 
834  fprintf(stdout, "checkin if objDiff x %d objDiff y %d \n",abs( objLoc.x - pointLocation.x), abs( objLoc.y - pointLocation.y));
835 
836  if ( abs( objLoc.x - pointLocation.x) < 5 && abs( objLoc.y - pointLocation.y) < 5)
837  {
838  objIndex = x;
839  fprintf(stdout, "I have found the object of interest and not considering it.\n");
840  }
841  else
842  {
843  fprintf(stdout, "\n\n\n\nI AM IN SETTING UP BLOBS with blob size = %d and x= %d\n\n\n\n",blobs.size(), x);
844  Bottle blobs;
845  blobs.clear();
846  // grab the blobs
847  blobs=getBlobs();
848 
849  blobsDetails[x].posistion.x = (int) closestBlob.get(0).asDouble();
850  blobsDetails[x].posistion.y = (int) closestBlob.get(1).asDouble();
851  blobsDetails[x].index = closestBlob.get(2).asInt();
852  blobsDetails[x].lenght = 0.0;
853  blobsDetails[x].lenght = getBlobLenght(blobs, x);
854  blobsDetails[x].vdrawError = 0.0;
855 
856  //Bottle mil;
857  //mil.clear();
858  //fprintf(stdout,"ask to classify \n ");
859  //mil=classify(blobs, x);
860  //fprintf(stdout,"done classifying\n");
861  //Bottle type;
862  //type.clear();
863  //fprintf(stdout,"ask to get type\n");
864  //type=getType(&mil, x);
865  blobsDetails[x].name = classResults.get(x).asString();
866 
867  fprintf(stdout, "SO: name is: %s x is %d u is %d index is %d\n",blobsDetails[x].name.c_str(),blobsDetails[x].posistion.x, blobsDetails[x].posistion.y, blobsDetails[x].index);
868 
869  //should look at x y
870  /*Bottle cmdAre, replyAre;
871  cmdAre.clear();
872  replyAre.clear();
873  cmdAre.addString("look");
874  Bottle &tmp=cmdAre.addList();
875  tmp.addInt (blobsDetails[x].posistion.x);
876  tmp.addInt (blobsDetails[x].posistion.y);
877  rpcMotorAre.write(cmdAre,replyAre);
878  fprintf(stdout,"looking started %s:\n",replyAre.toString().c_str());*/
879  }
880  }
881  fprintf(stdout, "\n\n\n\nI AM OUT SETTING UP BLOBS \n\n\n\n");
882  Bottle homeAfterLookCmd, homeAfterLookReply;
883  homeAfterLookCmd.clear();
884  homeAfterLookReply.clear();
885  homeAfterLookCmd.addString("home");
886  homeAfterLookCmd.addString("head");
887  rpcMotorAre.write(homeAfterLookCmd,homeAfterLookReply);
888 
889  int toolLenght = 1000;
890  int smallIndex = -1;
891  int bigIndex = -1;
892  fprintf(stdout,"\n\n");
893  for (int x=0; x < blobs.size(); x++)
894  {
895  if ( x!=objIndex)
896  {
897  fprintf(stdout,"The lenght is %lf, with index %d\n",blobsDetails[x].lenght, x);
898  // figure out if using small or big tool
899  if (blobsDetails[x].lenght < toolLenght )
900  {
901  toolLenght=(int)blobsDetails[x].lenght;
902  smallIndex = x;
903  }
904  }
905  }
906  for (int x=0; x < blobs.size(); x++)
907  {
908  if ( x!=objIndex && x!=smallIndex)
909  bigIndex = x;
910  }
911 
912  //this needs to be changed and filled in by milClassifier
913 
914  fprintf(stdout,"The small tool is the blob index %d \n",smallIndex);
915  blobsDetails[smallIndex].name = "small";
916 
917  fprintf(stdout,"The big tool is the blob index %d \n", bigIndex);
918  blobsDetails[bigIndex].name = "big";
919 
920  fprintf(stdout,"\n\n");
921 
922  Bottle small, big;
923 
924  small.clear();
925  small = executeKarmaOptimize(toolSmall, blobsDetails[smallIndex].name);
926  blobsDetails[smallIndex].bestAngle = small.get(1).asDouble();
927  blobsDetails[smallIndex].bestDistance = small.get(2).asDouble();
928 
929  big.clear();
930  big = executeKarmaOptimize(toolBig, blobsDetails[bigIndex].name);
931  blobsDetails[bigIndex].bestAngle = big.get(1).asDouble();
932  blobsDetails[bigIndex].bestDistance = big.get(2).asDouble();
933 
934  fprintf(stdout,"\n\n");
935 
936  //Attach the tool
937  executeToolAttach(toolSmall);
938  double virtualtmp = 0.01;
939 
940  //setup all parameters to get the best possible configuration
941  while (virtualtmp > 0.0 && virtualtmp < 0.08 )
942  {
943  virtualtmp = executeVirtualDraw(blobsDetails[smallIndex]);
944  if (virtualtmp > 0.1)
945  blobsDetails[smallIndex].bestDistance -= 0.005;
946  else if (virtualtmp > 1.0)
947  blobsDetails[smallIndex].bestDistance -= 0.1;
948  else
949  blobsDetails[smallIndex].bestDistance += 0.005;
950 
951  fprintf(stdout, "the reply is: %lf \n",virtualtmp);
952  }
953 
954  //do it one last time to get the correct confidence
955  blobsDetails[smallIndex].vdrawError = executeVirtualDraw(blobsDetails[smallIndex]);
956  fprintf (stdout, "\n\nTHE BEST ANGLE IS %lf WITH DISTANCE %lf with confidence %lf\n\n",blobsDetails[smallIndex].bestAngle, blobsDetails[smallIndex].bestDistance, blobsDetails[smallIndex].vdrawError );
957 
958  //Attach the tool
959  executeToolAttach(toolBig);
960  virtualtmp = 0.01;
961  //setup all parameters to get the best possible configuration
962  while (virtualtmp > 0.0 && virtualtmp < 0.08 )
963  {
964  virtualtmp = executeVirtualDraw(blobsDetails[bigIndex]);
965  if (virtualtmp > 0.1)
966  blobsDetails[bigIndex].bestDistance -= 0.005;
967  else if (virtualtmp > 1.0)
968  blobsDetails[bigIndex].bestDistance -= 0.1;
969  else
970  blobsDetails[bigIndex].bestDistance += 0.005;
971 
972  fprintf(stdout, "the reply is: %lf \n",virtualtmp);
973  }
974 
975  //do it one last time to get the correct confidence
976  blobsDetails[bigIndex].vdrawError = executeVirtualDraw(blobsDetails[bigIndex]);
977  fprintf (stdout, "\n\nTHE BEST ANGLE IS %lf WITH DISTANCE %lf and confidence %lf\n\n",blobsDetails[bigIndex].bestAngle, blobsDetails[bigIndex].bestDistance, blobsDetails[bigIndex].vdrawError );
978 
979  int whichArm = 0;
980  Bottle cmdHome, cmdReply;
981  cmdHome.clear();
982  cmdReply.clear();
983  cmdHome.addString("home");
984  cmdHome.addString("head");
985  rpcMotorAre.write(cmdHome,cmdReply);
986 
987  //once blobs and vdraw has been determined compare them
988  double bestChoice = 1000.0;
989  int bestIndex = -1;
990  double bestDistance = -1;
991  for (int x=0; x < blobs.size(); x++)
992  {
993  if (blobsDetails[x].vdrawError < bestChoice && blobsDetails[x].bestDistance > bestDistance && x != objIndex)
994  //if ( blobsDetails[x].bestDistance > bestDistance && x != objIndex)
995  {
996  bestDistance = blobsDetails[x].bestDistance;
997  //bestChoice = blobsDetails[x].vdrawError;
998  bestIndex = x;
999  }
1000  }
1001 
1002  //HERE DO MAXIMUM DISTANCE CHECK... -0.40 -0.33
1003  string tmpObjName = blobsDetails[bestIndex].name;
1004  executeSpeech ("can you give me the " + tmpObjName + "please?");
1005 
1006  //objectPos[0] has the oject position -0.43 is the maximum distance
1007 
1008  //blobsDetails[bestIndex].bestDistance = blobsDetails[bestIndex].bestDistance - 0.05;
1009  //figure out best distance fro subsequent grasp
1010 
1011  double max = -0.35;
1012  blobsDetails[bestIndex].bestDistance = fabs(objectPos[0]) - fabs( max );
1013 
1014  fprintf(stdout, "*********************************************************************************\n");
1015  fprintf(stdout, "THE MAX DISTANCE I WILL DO IS %lf\n,",blobsDetails[bestIndex].bestDistance);
1016  fprintf(stdout, "*********************************************************************************\n");
1017 
1018  //blobsDetails[bestIndex].bestDistance = blobsDetails[bestIndex].bestDistance - 0.01;
1019 
1020  Bottle cmdAre, replyAre;
1021  cmdAre.clear();
1022  replyAre.clear();
1023  cmdAre.addString("point");
1024  Bottle &tmp=cmdAre.addList();
1025  tmp.addInt (blobsDetails[bestIndex].posistion.x);
1026  tmp.addInt (blobsDetails[bestIndex].posistion.y);
1027 
1028  bool sendAction = true;
1029  if (blobsDetails[bestIndex].posistion.x > 0 && blobsDetails[bestIndex].posistion.x < 160 )
1030  {
1031  whichArm = 0;
1032  cmdAre.addString("left");
1033  }
1034  else if (blobsDetails[bestIndex].posistion.x > 160 && blobsDetails[bestIndex].posistion.x < 320 )
1035  {
1036  whichArm = 1;
1037  cmdAre.addString("right");
1038  }
1039  else
1040  {
1041  executeSpeech ("oh my...I seemed to got confused...sorry");
1042  fprintf(stdout, "something is wrong with the action:\n");
1043  sendAction = false;
1044  }
1045 
1046  fprintf(stdout, "the cmd is: %s \n",cmdAre.toString().c_str());
1047  if (sendAction)
1048  {
1049  rpcMotorAre.write(cmdAre,replyAre);
1050  fprintf(stdout, "the reply is: %s \n",replyAre.toString().c_str());
1051  }
1052 
1053 
1054  if (sendAction)
1055  {
1056  executeGiveAction(whichArm);
1057  Time::delay(5.0);
1058  }
1059  if (sendAction)
1060  {
1061  executeCloseHand(whichArm);
1062  Time::delay(5.0);
1063  }
1064 
1065  Bottle homeCmd, homeReply;
1066  homeCmd.clear();
1067  homeReply.clear();
1068  homeCmd.addString("home");
1069  homeCmd.addString("arms");
1070  homeCmd.addString("head");
1071  rpcMotorAre.write(homeCmd,homeReply);
1072 
1073  if (tmpObjName == "small")
1074  executeToolAttach(toolSmall);
1075  else
1076  executeToolAttach(toolBig);
1077 
1078  if (sendAction)
1079  executeToolDrawNear(blobsDetails[bestIndex]);
1080 
1081  Bottle homeToolcmd, homeToolrep;
1082  homeToolcmd.clear();
1083  homeToolrep.clear();
1084  homeToolcmd.addString("home");
1085  homeToolcmd.addString("arms");
1086  homeToolcmd.addString("head");
1087  rpcMotorAre.write(homeToolcmd,homeToolrep);
1088 
1089  executeSpeech ("Ok, thank you");
1090 
1091  if (sendAction)
1092  executeDropAway(whichArm);
1093 
1094  if (sendAction)
1095  executePCLGrasp( objName );
1096  }
1097 
1098  return 0;
1099 }
1100 /**********************************************************/
1101 bool Manager::executeDropAway(int ARM)
1102 {
1103  Bottle cmdAre, replyAre;
1104  cmdAre.clear();
1105  replyAre.clear();
1106  cmdAre.addString("drop");
1107  cmdAre.addString("away");
1108  if (ARM == LEFTARM)
1109  cmdAre.addString("left");
1110  else
1111  cmdAre.addString("right");
1112 
1113  rpcMotorAre.write(cmdAre,replyAre);
1114 
1115  printf("done deploying\n");
1116  return true;
1117 }
1118 /**********************************************************/
1119 int Manager::executeToolAttach(const Vector &tool)
1120 {
1121  fprintf(stdout,"Will now send to karmaMotor:\n");
1122  Bottle karmaMotor,KarmaReply;
1123  karmaMotor.addString("tool");
1124  karmaMotor.addString("remove");
1125  rpcMotorKarma.write(karmaMotor, KarmaReply);
1126 
1127  karmaMotor.clear();KarmaReply.clear();
1128  karmaMotor.addString("tool");
1129  karmaMotor.addString("attach");
1130  karmaMotor.addString("right");
1131  karmaMotor.addDouble(tool[0]);
1132  karmaMotor.addDouble(tool[1]);
1133  karmaMotor.addDouble(tool[2]);
1134  fprintf(stdout,"%s\n",karmaMotor.toString().c_str());
1135  rpcMotorKarma.write(karmaMotor, KarmaReply);
1136  fprintf(stdout,"reply is %s:\n",KarmaReply.toString().c_str());
1137  return 1;
1138 }
1139 /**********************************************************/
1140 Bottle Manager::executeKarmaOptimize(const Vector &tool, const string &name)
1141 {
1142  Bottle cmdLearn, cmdReply;
1143  cmdLearn.clear();
1144  cmdReply.clear();
1145  cmdLearn.addString("optimize");
1146  cmdLearn.addString(name);
1147  fprintf(stdout, "the cmd is: %s \n",cmdLearn.toString().c_str());
1148  rpcKarmaLearn.write(cmdLearn, cmdReply);
1149  fprintf(stdout, "the reply is: %s \n",cmdReply.toString().c_str());
1150 
1151  double optAng = cmdReply.get(1).asDouble();
1152  double optDisp = cmdReply.get(2).asDouble();
1153 
1154  fprintf(stdout, "the optimum angle is %lf with optimum disp %lf\n ",optAng , optDisp);
1155  return cmdReply;
1156 }
1157 /**********************************************************/
1158 double Manager::executeToolDrawNear(blobsData &blobsDetails)
1159 {
1160  double result = 0.0;
1161  //for (int tools = 0; tools < blobs.size(); tools++)
1162  fprintf(stdout,"Will now send to karmaMotor using %s as a tool:\n", blobsDetails.name.c_str());
1163  Bottle karmaMotor,KarmaReply;
1164  karmaMotor.addString("draw");
1165  karmaMotor.addDouble(objectPos[0]);
1166  karmaMotor.addDouble(objectPos[1]);
1167  karmaMotor.addDouble(objectPos[2]);
1168  karmaMotor.addDouble(blobsDetails.bestAngle);
1169  karmaMotor.addDouble(0.12); //10 cm
1170  karmaMotor.addDouble(blobsDetails.bestDistance);
1171  fprintf(stdout,"%s\n",karmaMotor.toString().c_str());
1172  rpcMotorKarma.write(karmaMotor, KarmaReply);
1173  fprintf(stdout,"vdraw is %s:\n",KarmaReply.toString().c_str());
1174  result = KarmaReply.get(1).asDouble();
1175  return result;
1176 }
1177 /**********************************************************/
1178 double Manager::executeVirtualDraw(blobsData &blobsDetails)
1179 {
1180  double result = 0.0;
1181  //for (int tools = 0; tools < blobs.size(); tools++)
1182  fprintf(stdout,"Will now send to karmaMotor:\n");
1183  Bottle karmaMotor,KarmaReply;
1184  karmaMotor.addString("vdraw");
1185  karmaMotor.addDouble(objectPos[0]);
1186  karmaMotor.addDouble(objectPos[1]);
1187  karmaMotor.addDouble(objectPos[2]);
1188  karmaMotor.addDouble(blobsDetails.bestAngle);
1189  karmaMotor.addDouble(0.1); //10 cm
1190  karmaMotor.addDouble(blobsDetails.bestDistance);
1191 
1192 
1193  fprintf(stdout,"%s\n",karmaMotor.toString().c_str());
1194  rpcMotorKarma.write(karmaMotor, KarmaReply);
1195  fprintf(stdout,"vdraw is %s:\n",KarmaReply.toString().c_str());
1196  result = KarmaReply.get(1).asDouble();
1197  return result;
1198 }
1199 /**********************************************************/
1200 bool Manager::executeGiveAction(int ARM)
1201 {
1202 
1203  Bottle cmdAre, replyAre;
1204  cmdAre.clear();
1205  replyAre.clear();
1206  cmdAre.addString("tato");
1207  if (ARM == LEFTARM)
1208  cmdAre.addString("left");
1209  else
1210  cmdAre.addString("right");
1211 
1212  rpcMotorAre.write(cmdAre,replyAre);
1213 
1214  return true;
1215 }
1216 /**********************************************************/
1217 bool Manager::executeCloseHand (int ARM)
1218 {
1219 
1220  Bottle cmdAre, replyAre;
1221  cmdAre.clear();
1222  replyAre.clear();
1223  cmdAre.addString("close");
1224  if (ARM == LEFTARM)
1225  cmdAre.addString("left");
1226  else
1227  cmdAre.addString("right");
1228 
1229  rpcMotorAre.write(cmdAre,replyAre);
1230 
1231  printf("done closing\n");
1232  return true;
1233 }
1234 
1235 /**********************************************************/
1236 bool Manager::executeSpeech (const string &speech)
1237 {
1238  Bottle cmdIol;
1239  Bottle replyIol;
1240  cmdIol.clear(), replyIol.clear();
1241  cmdIol.addString("say");
1242  cmdIol.addString(speech);
1243  iolStateMachine.write(cmdIol,replyIol);
1244  fprintf(stdout,"%s\n", replyIol.toString().c_str());
1245  return true;
1246 }
1247 
1248 /**********************************************************/
1249 int Manager::executeOnLoc(bool shouldTrain)
1250 {
1251  fprintf(stdout, "\n\n\n****************************************************************************\n\n" );
1252  Bottle blobs;
1253  blobs.clear();
1254  // grab the blobs
1255  blobs=getBlobs();
1256  // failure handling
1257 
1258  if (blobs.size()==0)
1259  return RET_INVALID;
1260 
1261  if (blobs.size()<2)
1262  {
1263  pointLocation = getBlobCOG(blobs,0);
1264  fprintf (stdout,"point is %d %d \n", pointLocation.x, pointLocation.y);
1265  pointGood = true;
1266  }
1267  else
1268  {
1269  fprintf (stdout,"I see more than two blobs\n");
1270  pointGood = false;
1271  }
1272  if (pointGood)
1273  {
1274  Bottle closestBlob;
1275  mutexResources.lock();
1276  closestBlob=findClosestBlob(blobs,pointLocation);
1277  mutexResources.unlock();
1278 
1279  cv::Point cog;
1280  cog.x = closestBlob.get(0).asInt();
1281  cog.y = closestBlob.get(1).asInt();
1282 
1283  /*
1284  * segment the object
1285  */
1286  segmentAndTrack(cog.x, cog.y);
1287 
1288  int index = 0;
1289  index = closestBlob.get(2).asInt();
1290 
1291  fprintf(stdout, "closest blob is x:%d y:%d with index %d\n\n",cog.x, cog.y, index);
1292 
1293  //classify the blob
1294  Bottle mil;
1295  mil=classify(blobs, index);
1296  //get the type of the blob
1297  Bottle type;
1298  type=getType(&mil, index);
1299 
1300  double actionOrient = 0.0;
1301  int guessAction=(int)Rand::scalar(0,randActions.size());
1302  fprintf(stdout, "the guess action is %d chosen from size %d\n", guessAction, (int)randActions.size());
1303 
1304  double orient = 0.0;
1305  orient = closestBlob.get(3).asDouble();
1306 
1307  if (!shouldTrain)
1308  {
1309  fprintf(stdout,"SHOULD BE IN TEST MODE!\n");
1310  Bottle cmdLearn, cmdReply;
1311  cmdLearn.clear();
1312  cmdReply.clear();
1313  cmdLearn.addString("optimize");
1314  cmdLearn.addString(obj);
1315  /*Bottle &angles = cmdLearn.addList();
1316  for (int i = 0; i< randActions.size(); i++)
1317  {
1318  angles.addDouble(randActions[i]);
1319  }*/
1320  fprintf(stdout, "the cmd is: %s \n",cmdLearn.toString().c_str());
1321  rpcKarmaLearn.write(cmdLearn, cmdReply);
1322  fprintf(stdout, "the reply is: %s \n",cmdReply.toString().c_str());
1323 
1324  double optAng = cmdReply.get(1).asDouble();
1325  double optDisp = cmdReply.get(2).asDouble();
1326 
1327  fprintf(stdout, "the optimum angle is %lf with optimum disp %lf\n ",optAng , optDisp);
1328 
1329  Vector initPos;
1330  actionOrient = 0.0;
1331  actionOrient = optAng + orient;
1332 
1333  if (actionOrient > 360.0)
1334  actionOrient -= 360.0;
1335 
1336  if (actionOrient > 45.0 && actionOrient < 135.0)
1337  actionOrient += 180.0;
1338 
1339  fprintf(stdout, "\n\nthe FINAL angle is %lf \n\n",actionOrient);
1340  }
1341  else
1342  {
1343  fprintf(stdout,"SHOULD BE IN TRAIN MODE!\n");
1344  if (userTheta >= 0.0)
1345  actionOrient = userTheta;
1346  else
1347  actionOrient = Rand::scalar(-210.0,30.0);//randActions[guessAction];
1348  }
1349 
1350  Vector initPos;
1351  double finalOrient;
1352  if (get3DPosition(cog,initPos))
1353  {
1354  Bottle results;
1355  double offset = 0.0;
1356  results = getOffset(closestBlob, actionOrient, initPos);
1357  offset = results.get(0).asDouble();
1358  finalOrient = results.get(1).asDouble();
1359 
1360  fprintf(stdout,"Will now send to karmaMotor:\n");
1361  Bottle karmaMotor,KarmaReply;
1362  karmaMotor.addString("push");
1363  karmaMotor.addDouble(initPos[0]);
1364  karmaMotor.addDouble(initPos[1]);
1365  karmaMotor.addDouble(initPos[2] + 0.05);
1366  karmaMotor.addDouble(actionOrient);
1367  karmaMotor.addDouble( offset );// + 0.06 );
1368 
1369  fprintf(stdout,"%s\n",karmaMotor.toString().c_str());
1370  rpcMotorKarma.write(karmaMotor, KarmaReply);
1371  fprintf(stdout,"action is %s:\n",KarmaReply.toString().c_str());
1372 
1373  cv::Point finalPoint;
1374  if (particleFilter.getTraker(finalPoint))
1375  {
1376  while(finalPoint.x <1 && finalPoint.x >320 && finalPoint.y <1 && finalPoint.y >240)
1377  {
1378  fprintf(stdout, "\n\n\ndid not get a correct final point from particle filter..retrying...\n\n\n");
1379  particleFilter.getTraker(finalPoint);
1380  }
1381  Vector finalPos;
1382  if (get3DPosition(finalPoint,finalPos))
1383  {
1384  fprintf(stdout,"The Final 3Dpos is %lf %lf %lf \n",finalPos[0], finalPos[1], finalPos[2]);
1385 
1386  //original orientation
1387  //send it all
1388  double disp = 0.0;
1389  disp = norm(finalPos - initPos);
1390 
1391  if (shouldTrain)
1392  {
1393  Bottle cmdLearn, replyLearn;
1394  cmdLearn.clear();
1395  replyLearn.clear();
1396  cmdLearn.addString("train");
1397  cmdLearn.addString(obj);
1398 
1399  double wrappedAng = 0.0;
1400  wrappedAng = wrapAng ( finalOrient );
1401 
1402  cmdLearn.addDouble( wrappedAng );
1403  cmdLearn.addDouble( disp );
1404 
1405  fprintf(stdout, "the cmd is: %s \n",cmdLearn.toString().c_str());
1406  rpcKarmaLearn.write(cmdLearn, replyLearn);
1407  fprintf(stdout, "the reply is: %s \n",replyLearn.toString().c_str());
1408  }
1409  }
1410  }
1411  goHome();
1412  }
1413  }
1414  return 0;
1415 }
1416 /**********************************************************/
1417 double Manager::getBlobLenght(const Bottle &blobs, const int i)
1418 {
1419 
1420  double dist = 0;
1421  if ((i>=0) && (i<blobs.size()))
1422  {
1423  cv::Point tl,br;
1424  Bottle *item=blobs.get(i).asList();
1425  if (item==NULL)
1426  return dist;
1427 
1428  tl.x=(int)item->get(0).asDouble();
1429  tl.y=(int)item->get(1).asDouble();
1430  br.x=(int)item->get(2).asDouble();
1431  br.y=(int)item->get(3).asDouble();
1432 
1433 
1434  int dx = abs(tl.x - br.x);
1435  int dy = abs(tl.y - br.y);
1436 
1437  fprintf(stdout, "%d %d \n", dx, dy);
1438 
1439  dist = sqrt ( (double)((dx*dx) + (dy*dy)) );
1440 
1441  }
1442  return dist;
1443 }
1444 
1445 /**********************************************************/
1446 double Manager::wrapAng ( const double ang )
1447 {
1448  if ((ang<0.0) || (ang>=360.0))
1449  {
1450  double theta=(M_PI/180.0)*ang;
1451  theta=(180.0/M_PI)*atan2(sin(theta),cos(theta));
1452  if (theta<0.0)
1453  theta+=360.0;
1454  return theta;
1455  }
1456  else
1457  return ang;
1458 }
1459 /**********************************************************/
1460 Bottle Manager::getOffset( Bottle &closestBlob, double actionOrient, Vector &initPos )
1461 {
1462  double orient = 0.0;
1463  orient = closestBlob.get(3).asDouble();
1464 
1465  int axe1 = 0;
1466  int axe2 = 0;
1467  cv::Point cog;
1468  cog.x = closestBlob.get(0).asInt();
1469  cog.y = closestBlob.get(1).asInt();
1470 
1471  axe1 = closestBlob.get(4).asInt();
1472  axe2 = closestBlob.get(5).asInt();
1473  double finalOrient=actionOrient;
1474  fprintf(stdout ,"INITIAL orientation is: %lf \n", orient);
1475  if (abs(axe1-axe2) < 5)
1476  {
1477  fprintf(stdout,"seem a round object to me...sending theta2\n");
1478  }
1479  else
1480  {
1481  fprintf(stdout,"sending theta2 - theta1 <-> axis diff is %d\n", abs(axe1-axe2));
1482  finalOrient -= orient;
1483  }
1484 
1485  cv::Point pxls;
1486  Vector offPos;
1487  fprintf(stdout,"The 3Dpos is %lf %lf %lf \n",initPos[0], initPos[1], initPos[2]);
1488 
1489  double alpha = finalOrient * M_PI/180; //radians
1490  pxls.x = int(cog.x + (axe1 * 1.2 ) * cos(alpha));
1491  pxls.y = int(cog.y - (axe2 * 1.2 ) * sin(alpha));
1492 
1493  get3DPosition(pxls,offPos);
1494  fprintf(stdout,"The 3Dpos off point is %lf %lf %lf \n",offPos[0], offPos[1], offPos[2]);
1495  double offset= norm (initPos-offPos);
1496  fprintf(stdout,"The offset is %lf \n",offset);
1497 
1498  Bottle ret;
1499  ret.addDouble(offset);
1500  ret.addDouble(finalOrient);
1501  return ret;
1502 }
1503 
1504 /**********************************************************/
1505 Bottle Manager::classify(const Bottle &blobs, int index)
1506 {
1507  //grab resources
1508  mutexResources.lock();
1509  Bottle mils;
1510  mils.clear();
1511 
1512  Bottle gotMils;
1513  gotMils.clear();
1514 
1515  Bottle cmd,reply;
1516  cmd.addVocab(Vocab::encode("classify"));
1517 
1518  Bottle &options=cmd.addList();
1519  ostringstream tag;
1520  tag<<"blob_"<<index;
1521  Bottle &item=options.addList();
1522  item.addString(tag.str());
1523  item.addList()=*blobs.get(index).asList();
1524 
1525  printf("Sending classification request: %s\n",cmd.toString().c_str());
1526  rpcMIL.write(cmd,reply);
1527  printf("Received reply: %s\n",reply.toString().c_str());
1528  mutexResources.unlock();
1529  //Bottle &toReturn = gotMils.addList();
1530 
1531  cv::Point cog;
1532  cog=getBlobCOG(blobs,index);
1533  Bottle &tmpLine = gotMils.addList();
1534  Bottle &tmpMils = tmpLine.addList();
1535  tmpMils.addDouble(cog.x);
1536  tmpMils.addDouble(cog.y);
1537  tmpLine.addList()=*reply.get(0).asList()->get(1).asList();
1538 
1539  mils.clear();
1540  mils.addList()=gotMils;
1541  //release resources
1542  return mils;
1543 }
1544 /**********************************************************/
1545 Bottle Manager::getType(const yarp::os::Bottle *scores, int index)
1546 {
1547  ostringstream tag;
1548  tag<<"blob_"<<index;
1549  if (scores!=NULL)
1550  {
1551  double max_score=0.0;
1552  string max_label="unknown";
1553  if (scores->get(0).asList()->size() > 0)
1554  {
1555  Bottle *tmp_scores=scores->get(0).asList()->get(0).asList()->get(1).asList();
1556 
1557  fprintf(stdout,"bottle is: %s \n",tmp_scores->toString().c_str());
1558 
1559  for(int s=0; s<tmp_scores->size(); s++)
1560  {
1561  if(tmp_scores->get(s).asList()->get(1).asDouble()>max_score)
1562  {
1563  max_score=tmp_scores->get(s).asList()->get(1).asDouble();
1564  max_label=tmp_scores->get(s).asList()->get(0).asString();
1565  }
1566  }
1567  }
1568  //fprintf(stdout, "the prob is %lf ", scores->get(0).asList()->get(j).asList()->get(1).asList()->get(1).asDouble());
1569  //fprintf(stdout,"\n");
1570  tag.str("");
1571  tag.clear();
1572  tag<<max_label;
1573  Bottle type;
1574  type.clear();
1575  type.addString(max_label);
1576  return type;
1577  }
1578  Bottle type;
1579  type.clear();
1580  type.addString("error");
1581  return type;
1582 }
1583 /**********************************************************/
1584 double Manager::getPeriod()
1585 {
1586  return 0.1;
1587 }
1588 
1589 /**********************************************************/
1590 bool Manager::get3DPosition(const cv::Point &point, Vector &x)
1591 {
1592  Bottle cmdMotor,replyMotor;
1593  cmdMotor.addVocab(Vocab::encode("get"));
1594  cmdMotor.addVocab(Vocab::encode("s2c"));
1595  Bottle &options=cmdMotor.addList();
1596  options.addString(camera);
1597  options.addInt(point.x);
1598  options.addInt(point.y);
1599  printf("Sending motor query: %s\n",cmdMotor.toString().c_str());
1600  rpcMotorAre.write(cmdMotor,replyMotor);
1601  printf("Received blob cartesian coordinates: %s\n",replyMotor.toString().c_str());
1602 
1603  if (replyMotor.size()>=3)
1604  {
1605  x.resize(3);
1606  x[0]=replyMotor.get(0).asDouble();
1607  x[1]=replyMotor.get(1).asDouble();
1608  x[2]=replyMotor.get(2).asDouble();
1609  return true;
1610  }
1611  else
1612  return false;
1613 }
1614 /**********************************************************/
1615 Bottle Manager::getBlobs()
1616 {
1617  // grab resources
1618  mutexResources.lock();
1619 
1620  if (Bottle *pBlobs=blobExtractor.read(false))
1621  {
1622  lastBlobs=*pBlobs;
1623  printf("Received blobs list: %s\n",lastBlobs.toString().c_str());
1624 
1625  if (lastBlobs.size()==1)
1626  {
1627  if (lastBlobs.get(0).asVocab()==Vocab::encode("empty"))
1628  lastBlobs.clear();
1629  }
1630  }
1631  // release resources
1632  mutexResources.unlock();
1633 
1634  return lastBlobs;
1635 }
1636 /**********************************************************/
1637 cv::Point Manager::getBlobCOG(const Bottle &blobs, const int i)
1638 {
1639  cv::Point cog=cv::Point(RET_INVALID,RET_INVALID);
1640  if ((i>=0) && (i<blobs.size()))
1641  {
1642  cv::Point tl,br;
1643  Bottle *item=blobs.get(i).asList();
1644  if (item==NULL)
1645  return cog;
1646 
1647  tl.x=(int)item->get(0).asDouble();
1648  tl.y=(int)item->get(1).asDouble();
1649  br.x=(int)item->get(2).asDouble();
1650  br.y=(int)item->get(3).asDouble();
1651 
1652  cog.x=(tl.x+br.x)>>1;
1653  cog.y=(tl.y+br.y)>>1;
1654  }
1655  return cog;
1656 }
1657 /**********************************************************/
1658 Bottle Manager::findClosestBlob(const Bottle &blobs, const cv::Point &loc)
1659 {
1660  int ret=RET_INVALID;
1661  double min_d2=std::numeric_limits<double>::max();
1662  Bottle pointReturn;
1663  pointReturn.clear();
1664  for (int i=0; i<blobs.size(); i++)
1665  {
1666  cv::Point cog=getBlobCOG(blobs,i);
1667  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
1668  continue;
1669 
1670  double dx=loc.x-cog.x;
1671  double dy=loc.y-cog.y;
1672  double d2=dx*dx+dy*dy;
1673 
1674  if (d2<min_d2)
1675  {
1676  min_d2=d2;
1677  ret=i;
1678  }
1679  }
1680  cv::Point cog=getBlobCOG( blobs, ret );
1681  pointReturn.addDouble(cog.x); //cog x
1682  pointReturn.addDouble(cog.y); //cog y
1683  pointReturn.addInt(ret); //index of blob
1684  Bottle *item=blobs.get(ret).asList();
1685  double orient = (double)item->get(4).asDouble();
1686  int axe1 = item->get(5).asInt();
1687  int axe2 = item->get(6).asInt();
1688  pointReturn.addDouble(orient);
1689  pointReturn.addInt(axe1);
1690  pointReturn.addInt(axe2);
1691  return pointReturn;
1692 }
1693 /**********************************************************/
Generated by doxygen 1.9.1