iol
module.cpp
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini
4  * email: 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 <sstream>
19 #include <limits>
20 #include <cmath>
21 #include <algorithm>
22 #include <set>
23 
24 #include <yarp/cv/Cv.h>
25 #include <yarp/math/Math.h>
26 #include <yarp/math/Rand.h>
27 
28 #include "module.h"
29 
30 using namespace yarp::cv;
31 using namespace yarp::math;
32 
33 
34 /**********************************************************/
35 class BusyGate
36 {
37  bool &gate;
38  bool owner;
39 public:
40  /**********************************************************/
41  BusyGate(bool &g) : gate(g)
42  {
43  gate=true;
44  owner=true;
45  }
46 
47  /**********************************************************/
48  void release()
49  {
50  owner=false;
51  }
52 
53  /**********************************************************/
54  ~BusyGate()
55  {
56  if (owner)
57  gate=false;
58  }
59 };
60 
61 
62 /**********************************************************/
63 Tracker::Tracker(const string &trackerType_, const double trackerTmo_) :
64  trackerType(trackerType_), trackerState(idle),
65  trackerTmo(trackerTmo_), trackerTimer(0.0)
66 {
67  trackerResult.x=trackerResult.y=0;
68  trackerResult.width=trackerResult.height=0;
69 }
70 
71 
72 /**********************************************************/
73 void Tracker::prepare()
74 {
75  if (trackerState==no_need)
76  trackerState=init;
77 }
78 
79 
80 /**********************************************************/
81 void Tracker::latchBBox(const cv::Rect &bbox)
82 {
83  trackerResult.x=bbox.x;
84  trackerResult.y=bbox.y;
85  trackerResult.width=bbox.width;
86  trackerResult.height=bbox.height;
87  trackerState=no_need;
88 }
89 
90 
91 /**********************************************************/
92 void Tracker::track(ImageOf<PixelBgr> &img)
93 {
94  cv::Mat frame=toCvMat(img);
95  if (trackerState==init)
96  {
97  if (trackerType=="MIL")
98  tracker=cv::TrackerMIL::create();
99  else if (trackerType=="TLD")
100  tracker=cv::TrackerTLD::create();
101  else if (trackerType=="KCF")
102  tracker=cv::TrackerKCF::create();
103  else
104  tracker=cv::TrackerBoosting::create();
105 
106  tracker->init(frame,trackerResult);
107  trackerTimer=Time::now();
108  trackerState=tracking;
109  }
110  else if (trackerState==tracking)
111  {
112  if (Time::now()-trackerTimer<trackerTmo)
113  {
114  tracker->update(frame,trackerResult);
115  cv::Point tl((int)trackerResult.x,(int)trackerResult.y);
116  cv::Point br(tl.x+(int)trackerResult.width,tl.y+(int)trackerResult.height);
117  if ((tl.x<5) || (br.x>frame.cols-5) ||
118  (tl.y<5) || (br.y>frame.rows-5))
119  trackerState=idle;
120  }
121  else
122  trackerState=idle;
123  }
124 }
125 
126 
127 /**********************************************************/
128 bool Tracker::is_tracking(cv::Rect &bbox) const
129 {
130  bbox=cv::Rect((int)trackerResult.x,(int)trackerResult.y,
131  (int)trackerResult.width,(int)trackerResult.height);
132  return (trackerState!=idle);
133 }
134 
135 
136 /**********************************************************/
137 int Manager::processHumanCmd(const Bottle &cmd, Bottle &b)
138 {
139  int ret=Vocab32::encode(cmd.get(0).asString());
140  b.clear();
141 
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 
150  return ret;
151 }
152 
153 
154 /**********************************************************/
155 Bottle Manager::skimBlobs(const Bottle &blobs)
156 {
157  Bottle skimmedBlobs;
158  for (int i=0; i<blobs.size(); i++)
159  {
160  cv::Point cog=getBlobCOG(blobs,i);
161  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
162  continue;
163 
164  // skim out blobs that are too far in the cartesian space
165  Vector x;
166  if (get3DPosition(cog,x))
167  {
168  if ((x[0]>skim_blobs_x_bounds[0])&&(x[0]<skim_blobs_x_bounds[1])&&
169  (x[1]>skim_blobs_y_bounds[0])&&(x[1]<skim_blobs_y_bounds[1]))
170  skimmedBlobs.add(blobs.get(i));
171  }
172  }
173 
174  return skimmedBlobs;
175 }
176 
177 
178 /**********************************************************/
179 bool Manager::thresBBox(cv::Rect &bbox, const Image &img)
180 {
181  cv::Point tl(bbox.x,bbox.y);
182  cv::Point br(tl.x+bbox.width,tl.y+bbox.height);
183  tl.x=std::min((int)img.width(),std::max(tl.x,0));
184  tl.y=std::min((int)img.height(),std::max(tl.y,0));
185  br.x=std::min((int)img.width(),std::max(br.x,0));
186  br.y=std::min((int)img.height(),std::max(br.y,0));
187 
188  bbox=cv::Rect(tl.x,tl.y,br.x-tl.x,br.y-tl.y);
189  if ((bbox.width>tracker_min_blob_size[0]) &&
190  (bbox.height>tracker_min_blob_size[1]))
191  return true;
192  else
193  return false;
194 }
195 
196 
197 /**********************************************************/
198 Bottle Manager::getBlobs()
199 {
200  // grab resources
201  mutexResources.lock();
202 
203  if (Bottle *pBlobs=blobExtractor.read(false))
204  {
205  lastBlobsArrivalTime=Time::now();
206  lastBlobs=skimBlobs(*pBlobs);
207  yInfo("Received blobs list: %s",lastBlobs.toString().c_str());
208 
209  if (lastBlobs.size()==1)
210  {
211  if (lastBlobs.get(0).asVocab32()==Vocab32::encode("empty"))
212  lastBlobs.clear();
213  }
214  }
215  else if (Time::now()-lastBlobsArrivalTime>blobs_detection_timeout)
216  lastBlobs.clear();
217 
218  // release resources
219  mutexResources.unlock();
220 
221  return lastBlobs;
222 }
223 
224 
225 /**********************************************************/
226 cv::Point Manager::getBlobCOG(const Bottle &blobs, const int i)
227 {
228  cv::Point cog(RET_INVALID,RET_INVALID);
229  if ((i>=0) && (i<blobs.size()))
230  {
231  cv::Point tl,br;
232  Bottle *item=blobs.get(i).asList();
233  if (item==NULL)
234  return cog;
235 
236  tl.x=(int)item->get(0).asFloat64();
237  tl.y=(int)item->get(1).asFloat64();
238  br.x=(int)item->get(2).asFloat64();
239  br.y=(int)item->get(3).asFloat64();
240 
241  cog.x=(tl.x+br.x)>>1;
242  cog.y=(tl.y+br.y)>>1;
243  }
244 
245  return cog;
246 }
247 
248 
249 /**********************************************************/
250 bool Manager::get3DPosition(const cv::Point &point, Vector &x)
251 {
252  x.resize(3,0.0);
253  if (rpcGet3D.getOutputCount()>0)
254  {
255  // thanks to SFM we are here
256  // safe against borders checking
257  // command format: Rect tlx tly w h step
258  Bottle cmd,reply;
259  cmd.addString("Rect");
260  cmd.addInt32(point.x-3);
261  cmd.addInt32(point.y-3);
262  cmd.addInt32(7);
263  cmd.addInt32(7);
264  cmd.addInt32(2);
265 
266  mutexGet3D.lock();
267  yInfo("Sending get3D query: %s",cmd.toString().c_str());
268  rpcGet3D.write(cmd,reply);
269  yInfo("Received blob cartesian coordinates: %s",reply.toString().c_str());
270  mutexGet3D.unlock();
271 
272  int sz=reply.size();
273  if ((sz>0) && ((sz%3)==0))
274  {
275  Vector tmp(3);
276  int cnt=0;
277 
278  for (int i=0; i<sz; i+=3)
279  {
280  tmp[0]=reply.get(i+0).asFloat64();
281  tmp[1]=reply.get(i+1).asFloat64();
282  tmp[2]=reply.get(i+2).asFloat64();
283 
284  if (norm(tmp)>0.0)
285  {
286  x+=tmp;
287  cnt++;
288  }
289  }
290 
291  if (cnt>0)
292  x/=cnt;
293  else
294  yWarning("get3DPosition failed");
295  }
296  else
297  yError("SFM replied with wrong size");
298  }
299 
300  return (norm(x)>0.0);
301 }
302 
303 
304 /**********************************************************/
305 void Manager::acquireImage(const bool rtlocalization)
306 {
307  // grab resources
308  mutexResources.lock();
309 
310  // wait for incoming image
311  if (ImageOf<PixelBgr> *tmp=imgIn.read())
312  {
313  if (rtlocalization)
314  imgRtLoc=*tmp;
315  else
316  img=*tmp;
317  }
318 
319  // release resources
320  mutexResources.unlock();
321 }
322 
323 
324 /**********************************************************/
325 void Manager::drawBlobs(const Bottle &blobs, const int i,
326  Bottle *scores)
327 {
328  // grab resources
329  mutexResources.lock();
330 
331  BufferedPort<ImageOf<PixelBgr> > *port=(scores==NULL)?&imgOut:&imgRtLocOut;
332  if (port->getOutputCount()>0)
333  {
334  cv::Scalar highlight(0,255,0);
335  cv::Scalar lowlight(150,125,125);
336 
337  // latch image
338  ImageOf<PixelBgr> img=(scores==NULL)?this->img:this->imgRtLoc;
339  cv::Mat imgMat=toCvMat(img);
340  for (int j=0; j<blobs.size(); j++)
341  {
342  cv::Point tl,br,txtLoc;
343  Bottle *item=blobs.get(j).asList();
344  tl.x=(int)item->get(0).asFloat64();
345  tl.y=(int)item->get(1).asFloat64();
346  br.x=(int)item->get(2).asFloat64();
347  br.y=(int)item->get(3).asFloat64();
348  txtLoc.x=tl.x;
349  txtLoc.y=tl.y-5;
350 
351  ostringstream tag;
352  tag<<"blob_"<<j;
353 
354  if (scores!=NULL)
355  {
356  // find the blob name (or unknown)
357  string object=db.findName(*scores,tag.str());
358  tag.str("");
359  tag.clear();
360  tag<<object;
361  }
362 
363  cv::rectangle(imgMat,tl,br,(j==i)?highlight:lowlight,2);
364  cv::putText(imgMat,tag.str(),txtLoc,cv::FONT_HERSHEY_SIMPLEX,0.5,(j==i)?highlight:lowlight,2);
365  }
366 
367  port->prepare()=img;
368  port->write();
369  }
370 
371  // release resources
372  mutexResources.unlock();
373 }
374 
375 
376 /**********************************************************/
377 void Manager::rotate(cv::Mat &src, const double angle,
378  cv::Mat &dst)
379 {
380  int len=std::max(src.cols,src.rows);
381  cv::Point2f pt(len/2.0f,len/2.0f);
382  cv::Mat r=cv::getRotationMatrix2D(pt,angle,1.0);
383  cv::warpAffine(src,dst,r,cv::Size(len,len));
384 }
385 
386 
387 /**********************************************************/
388 void Manager::drawScoresHistogram(const Bottle &blobs,
389  const Bottle &scores, const int i)
390 {
391  if (imgHistogram.getOutputCount()>0)
392  {
393  // grab resources
394  mutexResources.lock();
395 
396  // create image containing histogram
397  ImageOf<PixelBgr> imgConf;
398  imgConf.resize(600,600); imgConf.zero();
399 
400  // opencv wrappers
401  cv::Mat imgRtLocMat=toCvMat(imgRtLoc);
402  cv::Mat imgConfMat=toCvMat(imgConf);
403 
404  ostringstream tag;
405  tag<<"blob_"<<i;
406 
407  // process scores on the given blob
408  if (Bottle *blobScores=scores.find(tag.str()).asList())
409  {
410  // set up some variables and constraints
411  int maxHeight=(int)(imgConf.height()*0.8);
412  int minHeight=imgConf.height()-20;
413  int widthStep=(blobScores->size()>0)?(int)(imgConf.width()/blobScores->size()):0;
414  set<string> gcFilters;
415 
416  // cycle over classes
417  for (int j=0; j<blobScores->size(); j++)
418  {
419  Bottle *item=blobScores->get(j).asList();
420  if (item==NULL)
421  continue;
422 
423  string name=item->get(0).asString();
424  double score=std::max(std::min(item->get(1).asFloat64(),1.0),0.0);
425 
426  // smooth out quickly varying scores
427  auto it=histFiltersPool.find(name);
428 
429  // create filter if not available
430  if (it==histFiltersPool.end())
431  {
432  Vector num(histFilterLength,1.0);
433  Vector den(histFilterLength,0.0); den[0]=histFilterLength;
434  histFiltersPool[name]=new Filter(num,den,Vector(1,score));
435  }
436  else
437  {
438  Vector scoreFilt=it->second->filt(Vector(1,score));
439  score=scoreFilt[0];
440  }
441 
442  // put the class name in a convenient set for garbage collection
443  gcFilters.insert(name);
444 
445  int classHeight=std::min(minHeight,(int)imgConf.height()-(int)(maxHeight*score));
446 
447  cv::rectangle(imgConfMat,cv::Point(j*widthStep,classHeight),cv::Point((j+1)*widthStep,minHeight),
448  histColorsCode[j%(int)histColorsCode.size()],cv::FILLED);
449 
450  cv::Mat textImg=cv::Mat::zeros(imgConf.height(),imgConf.width(),CV_8UC3);
451  cv::putText(textImg,name,cv::Point(imgConf.width()-580,(j+1)*widthStep-10),cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(255,255,255),2);
452  rotate(textImg,90.0,textImg);
453 
454  cv::Mat orig=toCvMat(imgConf);
455  orig=orig+textImg;
456  }
457 
458  // draw the blob snapshot
459  cv::Point tl,br,sz;
460  Bottle *item=blobs.get(i).asList();
461  tl.x=(int)item->get(0).asFloat64();
462  tl.y=(int)item->get(1).asFloat64();
463  br.x=(int)item->get(2).asFloat64();
464  br.y=(int)item->get(3).asFloat64();
465  sz.x=br.x-tl.x;
466  sz.y=br.y-tl.y;
467 
468  // copy the blob
469  ImageOf<PixelBgr> imgTmp1;
470  imgTmp1.resize(sz.x,sz.y);
471  cv::Mat imgRtLocRoi(imgRtLocMat,cv::Rect(tl.x,tl.y,sz.x,sz.y));
472  cv::Mat imgTmp1Mat=toCvMat(imgTmp1);
473  imgRtLocRoi.copyTo(imgTmp1Mat);
474 
475  // resize the blob proportionally
476  double resizeFact=sqrt((imgConf.width()*imgConf.height())/(imgTmp1.width()*imgTmp1.height()))/4.0;
477  size_t imgTmp2_width=(size_t)(resizeFact*imgTmp1.width());
478  size_t imgTmp2_height=(size_t)(resizeFact*imgTmp1.height());
479  if (imgTmp2_width>imgTmp2_height)
480  {
481  if (imgTmp2_width>imgConf.width()/4)
482  {
483  double r=(double)imgTmp2_height/(double)imgTmp2_width;
484  imgTmp2_width=imgConf.width()/4;
485  imgTmp2_height=(size_t)(r*imgTmp2_width);
486  }
487  }
488  else if (imgTmp2_height>imgConf.height()/4)
489  {
490  double r=(double)imgTmp2_width/(double)imgTmp2_height;
491  imgTmp2_height=imgConf.height()/4;
492  imgTmp2_width=(size_t)(r*imgTmp2_height);
493  }
494 
495  ImageOf<PixelBgr> imgTmp2;
496  imgTmp2.resize(std::max((size_t)1,imgTmp2_width),std::max((size_t)1,imgTmp2_height));
497  cv::Mat imgTmp2Mat=toCvMat(imgTmp2);
498  cv::resize(imgTmp1Mat,imgTmp2Mat,imgTmp2Mat.size());
499 
500  // superimpose the blob on the histogram
501  cv::Mat imgConfRoi(imgConfMat,cv::Rect(0,0,imgTmp2.width(),imgTmp2.height()));
502  imgTmp2Mat.copyTo(imgConfRoi);
503  cv::rectangle(imgConfMat,cv::Point(0,0),cv::Point(imgTmp2.width(),imgTmp2.height()),
504  cv::Scalar(255,255,255),3);
505 
506  // give chance for disposing filters that are no longer used (one at time)
507  if ((int)histFiltersPool.size()>blobScores->size())
508  {
509  for (auto it=histFiltersPool.begin();
510  it!=histFiltersPool.end(); it++)
511  {
512  if (gcFilters.find(it->first)==gcFilters.end())
513  {
514  delete it->second;
515  histFiltersPool.erase(it);
516  break;
517  }
518  }
519  }
520  }
521 
522  imgHistogram.prepare()=imgConf;
523  imgHistogram.write();
524 
525  // release resources
526  mutexResources.unlock();
527  }
528 }
529 
530 
531 /**********************************************************/
532 int Manager::findClosestBlob(const Bottle &blobs, const cv::Point &loc)
533 {
534  int ret=RET_INVALID;
535  double min_d2=std::numeric_limits<double>::max();
536 
537  for (int i=0; i<blobs.size(); i++)
538  {
539  cv::Point cog=getBlobCOG(blobs,i);
540  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
541  continue;
542 
543  double dx=loc.x-cog.x;
544  double dy=loc.y-cog.y;
545  double d2=dx*dx+dy*dy;
546 
547  if (d2<min_d2)
548  {
549  min_d2=d2;
550  ret=i;
551  }
552  }
553 
554  return ret;
555 }
556 
557 
558 /**********************************************************/
559 int Manager::findClosestBlob(const Bottle &blobs, const Vector &loc)
560 {
561  int ret=RET_INVALID;
562  double curMinDist=std::numeric_limits<double>::max();
563 
564  for (int i=0; i<blobs.size(); i++)
565  {
566  cv::Point cog=getBlobCOG(blobs,i);
567  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
568  continue;
569 
570  Vector x;
571  if (get3DPosition(cog,x))
572  {
573  double dist=norm(loc-x);
574  if (dist<curMinDist)
575  {
576  ret=i;
577  curMinDist=dist;
578  }
579  }
580  }
581 
582  return ret;
583 }
584 
585 
586 /**********************************************************/
587 Bottle Manager::classify(const Bottle &blobs,
588  const bool rtlocalization)
589 {
590  // grab resources
591  mutexResources.lock();
592 
593  if (rtlocalization)
594  imgClassifier.write(imgRtLoc);
595  else
596  imgClassifier.write(img);
597 
598  Bottle cmd,reply;
599  cmd.addVocab32("classify");
600  Bottle &options=cmd.addList();
601  for (int i=0; i<blobs.size(); i++)
602  {
603  ostringstream tag;
604  tag<<"blob_"<<i;
605  Bottle &item=options.addList();
606  item.addString(tag.str());
607  item.addList()=*blobs.get(i).asList();
608  }
609  yInfo("Sending classification request: %s",cmd.toString().c_str());
610  rpcClassifier.write(cmd,reply);
611  yInfo("Received reply: %s",reply.toString().c_str());
612 
613  // release resources
614  mutexResources.unlock();
615 
616  return reply;
617 }
618 
619 
620 /**********************************************************/
621 void Manager::burst(const string &tag)
622 {
623  if (trainBurst && (tag!=""))
624  {
625  Bottle cmd,reply;
626  cmd.addVocab32("burst");
627  cmd.addVocab32(tag);
628 
629  yInfo("Sending burst training request: %s",cmd.toString().c_str());
630  rpcClassifier.write(cmd,reply);
631  yInfo("Received reply: %s",reply.toString().c_str());
632  }
633 }
634 
635 
636 /**********************************************************/
637 void Manager::train(const string &object, const Bottle &blobs,
638  const int i)
639 {
640  // grab resources
641  mutexResources.lock();
642 
643  imgClassifier.write(img);
644 
645  Bottle cmd,reply;
646  cmd.addVocab32("train");
647  Bottle &options=cmd.addList().addList();
648  options.addString(object);
649 
650  if (i<0)
651  {
652  Vector z=zeros(4);
653  options.addList().read(z);
654  }
655  else
656  options.add(blobs.get(i));
657 
658  yInfo("Sending training request: %s",cmd.toString().c_str());
659  rpcClassifier.write(cmd,reply);
660  yInfo("Received reply: %s",reply.toString().c_str());
661 
662  if (trainOnFlipped && (i>=0))
663  {
664  ImageOf<PixelBgr> imgFlipped=img;
665  if (Bottle *item=blobs.get(i).asList())
666  {
667  cv::Point tl,br;
668  tl.x=(int)item->get(0).asFloat64();
669  tl.y=(int)item->get(1).asFloat64();
670  br.x=(int)item->get(2).asFloat64();
671  br.y=(int)item->get(3).asFloat64();
672 
673  cv::Mat roi(toCvMat(imgFlipped),cv::Rect(tl.x,tl.y,br.x-tl.x,br.y-tl.y));
674  cv::flip(roi,roi,1);
675 
676  imgClassifier.write(imgFlipped);
677 
678  yInfo("Sending training request (for flipped image): %s",cmd.toString().c_str());
679  rpcClassifier.write(cmd,reply);
680  yInfo("Received reply (for flipped image): %s",reply.toString().c_str());
681  }
682  }
683 
684  // release resources
685  mutexResources.unlock();
686 }
687 
688 
689 /**********************************************************/
690 void Manager::improve_train(const string &object, const Bottle &blobs,
691  const int i)
692 {
693  cv::Point ref_cog=getBlobCOG(blobs,i);
694  if ((ref_cog.x==RET_INVALID) || (ref_cog.y==RET_INVALID))
695  return;
696 
697  double t0=Time::now();
698  while (Time::now()-t0<improve_train_period)
699  {
700  // acquire image for training
701  acquireImage();
702 
703  // grab the blobs
704  Bottle blobs=getBlobs();
705 
706  // failure handling
707  if (blobs.size()==0)
708  continue;
709 
710  // enforce 2D consistency
711  int exploredBlob=-1;
712  double curMinDist=10.0;
713  double curMinDist2=curMinDist*curMinDist;
714  for (int i=0; i<blobs.size(); i++)
715  {
716  cv::Point cog=getBlobCOG(blobs,i);
717  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
718  continue;
719 
720  double dx=ref_cog.x-cog.x;
721  double dy=ref_cog.y-cog.y;
722  double dist2=dx*dx+dy*dy;
723  if (dist2<curMinDist2)
724  {
725  exploredBlob=i;
726  curMinDist2=dist2;
727  }
728  }
729 
730  // no candidate found => skip
731  if (exploredBlob<0)
732  continue;
733 
734  // train the classifier
735  train(object,blobs,exploredBlob);
736 
737  // draw the blobs highlighting the explored one
738  drawBlobs(blobs,exploredBlob);
739  }
740 }
741 
742 
743 /**********************************************************/
744 void Manager::home(const string &part)
745 {
746  Bottle cmdMotor,replyMotor;
747  cmdMotor.addVocab32("home");
748  cmdMotor.addString(part);
749  rpcMotor.write(cmdMotor,replyMotor);
750 }
751 
752 
753 /**********************************************************/
754 void Manager::calibTable()
755 {
756  Bottle cmdMotor,replyMotor;
757  cmdMotor.addVocab32("calib");
758  cmdMotor.addVocab32("table");
759  rpcMotor.write(cmdMotor,replyMotor);
760 }
761 
762 
763 /**********************************************************/
764 bool Manager::calibKinStart(const string &object, const string &hand,
765  const Vector &x, const int recogBlob)
766 {
767  Bottle replyHuman;
768  bool ret=false;
769 
770  // some known object has been recognized
771  if (recogBlob>=0)
772  {
773  deque<string> param;
774  param.push_back(hand);
775  param.push_back("still");
776 
777  Vector y;
778  if (interruptableAction("touch",&param,object,x,y))
779  {
780  Bottle cmdMotor,replyMotor;
781  cmdMotor.addVocab32("calib");
782  cmdMotor.addVocab32("kinematics");
783  cmdMotor.addString("start");
784  if (y.length()>0)
785  cmdMotor.addList().read(y);
786  cmdMotor.addString(hand);
787  rpcMotor.write(cmdMotor,replyMotor);
788 
789  objectToBeKinCalibrated=object;
790  speaker.speak("Ok, now teach me the correct position");
791  replyHuman.addString("ack");
792  ret=true;
793  }
794  else
795  {
796  speaker.speak("I might be wrong");
797  replyHuman.addString("nack");
798  }
799  }
800  // no known object has been recognized in the scene
801  else
802  {
803  ostringstream reply;
804  reply<<"I am sorry, I cannot see any "<<object;
805  reply<<" around. Should I try again?";
806  speaker.speak(reply.str());
807  replyHuman.addString("nack");
808  }
809 
810  rpcHuman.reply(replyHuman);
811  return ret;
812 }
813 
814 
815 /**********************************************************/
816 void Manager::calibKinStop()
817 {
818  Bottle cmdMotor,replyMotor;
819  cmdMotor.addVocab32("calib");
820  cmdMotor.addVocab32("kinematics");
821  cmdMotor.addString("stop");
822  cmdMotor.addString(objectToBeKinCalibrated);
823  rpcMotor.write(cmdMotor,replyMotor);
824 
825  speaker.speak("Thanks for the help");
826  home();
827 }
828 
829 
830 /**********************************************************/
831 void Manager::addDropPosition(Bottle &cmd)
832 {
833  if (dropPosition!=nullptr)
834  {
835  if (dropPosition->size()>=3)
836  {
837  Bottle &sub1=cmd.addList();
838  sub1.addString("target");
839  Bottle &sub2=sub1.addList();
840  sub2.addString("cartesian");
841  sub2.addList()=*dropPosition;
842  }
843  }
844 }
845 
846 
847 /**********************************************************/
848 void Manager::motorHelper(const string &cmd, const string &object)
849 {
850  Bottle cmdMotor,replyMotor;
851  cmdMotor.addVocab32(cmd);
852 
853  if (cmd=="look")
854  {
855  cmdMotor.addString(object);
856  cmdMotor.addString("wait");
857  }
858  else
859  {
860  string hand; Vector x,y;
861  get3DPositionFromMemory(object,x,false);
862  if (getCalibratedLocation(object,hand,x,y))
863  x=y;
864  cmdMotor.addList().read(x);
865  cmdMotor.addString(hand);
866 
867  ostringstream reply;
868  reply<<"I think this is the "<<object;
869  speaker.speak(reply.str());
870  }
871 
872  rpcMotor.write(cmdMotor,replyMotor);
873 
874  if (cmd=="point")
875  {
876  cmdMotor.clear();
877  cmdMotor.addVocab32("home");
878  cmdMotor.addString("hands");
879  rpcMotor.write(cmdMotor,replyMotor);
880  }
881 }
882 
883 
884 /**********************************************************/
885 bool Manager::getCalibratedLocation(const string &object,
886  string &hand,
887  const Vector &x,
888  Vector &y)
889 {
890  hand=(x[1]>0.0?"right":"left");
891  if (rpcReachCalib.getOutputCount()>0)
892  {
893  Bottle cmd,rep;
894  cmd.addString("get_location");
895  cmd.addString(hand);
896  cmd.addString(object);
897  cmd.addString("iol-"+hand);
898  rpcReachCalib.write(cmd,rep);
899 
900  y.resize(3);
901  y[0]=rep.get(1).asFloat64();
902  y[1]=rep.get(2).asFloat64();
903  y[2]=rep.get(3).asFloat64();
904  return true;
905  }
906 
907  return false;
908 }
909 
910 
911 /**********************************************************/
912 Vector Manager::applyObjectPosOffsets(const string &object,
913  const string &hand)
914 {
915  Vector offs(3,0.0);
916  if (rpcMemory.getOutputCount()>0)
917  {
918  mutexResourcesMemory.lock();
919  auto id=memoryIds.find(object);
920  auto memoryIdsEnd=memoryIds.end();
921  mutexResourcesMemory.unlock();
922 
923  if (id!=memoryIdsEnd)
924  {
925  // get the relevant properties
926  // [get] (("id" <num>) ("propSet" ("kinematic_offset_$hand")))
927  string prop("kinematic_offset_"+hand);
928 
929  Bottle cmdMemory,replyMemory;
930  cmdMemory.addVocab32("get");
931  Bottle &content=cmdMemory.addList();
932  Bottle &list_bid=content.addList();
933  list_bid.addString("id");
934  list_bid.addInt32(id->second);
935  Bottle &list_propSet=content.addList();
936  list_propSet.addString("propSet");
937  Bottle &list_items=list_propSet.addList();
938  list_items.addString(prop);
939  rpcMemory.write(cmdMemory,replyMemory);
940 
941  // retrieve kinematic offset
942  if (replyMemory.get(0).asVocab32()==Vocab32::encode("ack"))
943  {
944  if (Bottle *propField=replyMemory.get(1).asList())
945  {
946  if (propField->check(prop))
947  {
948  if (Bottle *pPos=propField->find(prop).asList())
949  {
950  if (pPos->size()>=3)
951  {
952  offs[0]=pPos->get(0).asFloat64();
953  offs[1]=pPos->get(1).asFloat64();
954  offs[2]=pPos->get(2).asFloat64();
955  }
956  }
957  }
958  }
959  }
960  }
961  }
962 
963  return offs;
964 }
965 
966 
967 /**********************************************************/
968 bool Manager::interruptableAction(const string &action,
969  deque<string> *param,
970  const string &object,
971  const Vector &x,
972  Vector &y)
973 {
974  // remap "hold" into "take" without final "drop"
975  string actionRemapped=action;
976  if (actionRemapped=="hold")
977  actionRemapped="take";
978 
979  Bottle cmdMotor,replyMotor;
980  RpcClient *port;
981  if (action=="grasp")
982  {
983  port=&rpcMotorGrasp;
984  cmdMotor.addString("grasp");
985  cmdMotor.addString(object);
986  cmdMotor.addString(x[1]>0.0?"right":"left");
987  }
988  else
989  {
990  string hand;
991  bool calib=getCalibratedLocation(object,hand,x,y);
992 
993  port=&rpcMotor;
994  cmdMotor.addVocab32(actionRemapped);
995  if (action=="drop")
996  cmdMotor.addString("over");
997 
998  if (calib)
999  {
1000  y+=applyObjectPosOffsets(object,hand);
1001  cmdMotor.addList().read(y);
1002  }
1003  else
1004  cmdMotor.addString(object);
1005 
1006  if (action=="drop")
1007  cmdMotor.addString("gently");
1008  if (param!=NULL)
1009  {
1010  for (size_t i=0; i<param->size(); i++)
1011  cmdMotor.addString((*param)[i]);
1012  }
1013 
1014  if (calib)
1015  cmdMotor.addString(hand);
1016  }
1017 
1018  actionInterrupted=false;
1019  enableInterrupt=true;
1020  port->write(cmdMotor,replyMotor);
1021  bool ack=(replyMotor.get(0).asVocab32()==Vocab32::encode("ack"));
1022 
1023  if ((action=="grasp") && !ack)
1024  {
1025  string why=replyMotor.get(1).asString();
1026  string sentence="Hmmm. The ";
1027  sentence+=object;
1028  if (why=="too_far")
1029  sentence+=" seems too far. Could you push it closer?";
1030  else
1031  sentence+=" seems in bad position for me. Could you help moving it a little bit?";
1032  speaker.speak(sentence);
1033  }
1034 
1035  // this switch might be turned on asynchronously
1036  // by a request received on a dedicated port
1037  if (actionInterrupted)
1038  {
1039  reinstateMotor();
1040  home();
1041  }
1042  // drop the object in the hand
1043  else if (ack && ((action=="take") || (action=="grasp")))
1044  {
1045  cmdMotor.clear();
1046  cmdMotor.addVocab32("drop");
1047  addDropPosition(cmdMotor);
1048  rpcMotor.write(cmdMotor,replyMotor);
1049  }
1050 
1051  enableInterrupt=false;
1052  return !actionInterrupted;
1053 }
1054 
1055 
1056 /**********************************************************/
1057 void Manager::interruptMotor()
1058 {
1059  if (enableInterrupt)
1060  {
1061  actionInterrupted=true; // keep this line before the call to write
1062  enableInterrupt=false;
1063  Bottle cmdMotorStop,replyMotorStop;
1064  cmdMotorStop.addVocab32("interrupt");
1065  rpcMotorStop.write(cmdMotorStop,replyMotorStop);
1066 
1067  speaker.speak("Ouch!");
1068  }
1069 }
1070 
1071 
1072 /**********************************************************/
1073 void Manager::reinstateMotor(const bool saySorry)
1074 {
1075  Bottle cmdMotorStop,replyMotorStop;
1076  cmdMotorStop.addVocab32("reinstate");
1077  rpcMotorStop.write(cmdMotorStop,replyMotorStop);
1078 
1079  if (saySorry)
1080  speaker.speak("Sorry");
1081 }
1082 
1083 
1084 /**********************************************************/
1085 void Manager::point(const string &object)
1086 {
1087  motorHelper("point",object);
1088 }
1089 
1090 
1091 /**********************************************************/
1092 void Manager::look(const string &object)
1093 {
1094  motorHelper("look",object);
1095 }
1096 
1097 
1098 /**********************************************************/
1099 void Manager::look(const Bottle &blobs, const int i,
1100  const Bottle &options)
1101 {
1102  cv::Point cog=getBlobCOG(blobs,i);
1103  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
1104  return;
1105 
1106  Bottle cmdMotor,replyMotor;
1107  cmdMotor.addVocab32("look");
1108  Bottle &opt=cmdMotor.addList();
1109  opt.addString(camera);
1110  opt.addInt32(cog.x);
1111  opt.addInt32(cog.y);
1112  cmdMotor.append(options);
1113  cmdMotor.addString("wait");
1114  rpcMotor.write(cmdMotor,replyMotor);
1115 }
1116 
1117 
1118 /**********************************************************/
1119 int Manager::recognize(const string &object, Bottle &blobs,
1120  Classifier **ppClassifier)
1121 {
1122  auto it=db.find(object);
1123  if (it==db.end())
1124  {
1125  // if not, create a brand new one
1126  db[object]=new Classifier(object,classification_threshold);
1127  trackersPool[object]=Tracker(tracker_type,tracker_timeout);
1128  it=db.find(object);
1129  yInfo("created classifier for %s",object.c_str());
1130  }
1131 
1132  // acquire image for classification/training
1133  acquireImage();
1134 
1135  // grab the blobs
1136  blobs=getBlobs();
1137 
1138  // failure handling
1139  if (blobs.size()==0)
1140  return RET_INVALID;
1141 
1142  // get the scores from the learning machine
1143  Bottle scores=classify(blobs);
1144 
1145  // failure handling
1146  if (scores.size()==1)
1147  {
1148  if (scores.get(0).asString()=="failed")
1149  {
1150  speaker.speak("Ooops! Sorry, something went wrong in my brain");
1151  return RET_INVALID;
1152  }
1153  }
1154 
1155  // find the best blob
1156  int recogBlob=db.processScores(it->second,scores);
1157 
1158  // draw the blobs highlighting the recognized one (if any)
1159  drawBlobs(blobs,recogBlob);
1160 
1161  // prepare output
1162  if (ppClassifier!=NULL)
1163  *ppClassifier=it->second;
1164 
1165  return recogBlob;
1166 }
1167 
1168 
1169 /**********************************************************/
1170 int Manager::recognize(Bottle &blobs, Bottle &scores, string &object)
1171 {
1172  object=OBJECT_UNKNOWN;
1173 
1174  // acquire image for classification/training
1175  acquireImage();
1176 
1177  // grab the blobs
1178  blobs=getBlobs();
1179 
1180  // failure handling
1181  if (blobs.size()==0)
1182  return RET_INVALID;
1183 
1184  // get the scores from the learning machine
1185  scores=classify(blobs);
1186 
1187  // failure handling
1188  if (scores.size()==1)
1189  {
1190  if (scores.get(0).asString()=="failed")
1191  {
1192  speaker.speak("Ooops! Sorry, something went wrong in my brain");
1193  return RET_INVALID;
1194  }
1195  }
1196 
1197  // handle the human-pointed object
1198  if (whatGood)
1199  {
1200  int closestBlob=findClosestBlob(blobs,whatLocation);
1201  drawBlobs(blobs,closestBlob);
1202  look(blobs,closestBlob);
1203 
1204  ostringstream tag;
1205  tag<<"blob_"<<closestBlob;
1206  object=db.findName(scores,tag.str());
1207  return closestBlob;
1208  }
1209  else
1210  {
1211  speaker.speak("Ooops! Sorry, I missed where you pointed at");
1212  return RET_INVALID;
1213  }
1214 }
1215 
1216 
1217 /**********************************************************/
1218 void Manager::execName(const string &object)
1219 {
1220  Bottle replyHuman;
1221  if (!trackStopGood)
1222  {
1223  speaker.speak("Ooops! Sorry, I missed where you pointed at");
1224  replyHuman.addString("nack");
1225  rpcHuman.reply(replyHuman);
1226  return;
1227  }
1228 
1229  auto it=db.find(object);
1230  if (it==db.end())
1231  {
1232  // if not, create a brand new one
1233  db[object]=new Classifier(object,classification_threshold);
1234  trackersPool[object]=Tracker(tracker_type,tracker_timeout);
1235  it=db.find(object);
1236  yInfo("created classifier for %s",object.c_str());
1237  }
1238 
1239  // acquire image for training
1240  acquireImage();
1241 
1242  // grab the blobs
1243  Bottle blobs=getBlobs();
1244 
1245  // failure handling
1246  if (blobs.size()==0)
1247  {
1248  speaker.speak("Ooops! Sorry, I cannot see any object");
1249  replyHuman.addString("nack");
1250  rpcHuman.reply(replyHuman);
1251  return;
1252  }
1253 
1254  // run the normal procedure straightaway
1255  Bottle scores=classify(blobs);
1256 
1257  // failure handling
1258  if (scores.size()==1)
1259  {
1260  if (scores.get(0).asString()=="failed")
1261  {
1262  speaker.speak("Ooops! Sorry, something went wrong in my brain");
1263  replyHuman.addString("nack");
1264  rpcHuman.reply(replyHuman);
1265  return;
1266  }
1267  }
1268 
1269  db.processScores(it->second,scores);
1270 
1271  // find the closest blob
1272  int closestBlob=findClosestBlob(blobs,trackStopLocation);
1273 
1274  // draw the blobs highlighting the detected one (if any)
1275  drawBlobs(blobs,closestBlob);
1276 
1277  // train
1278  burst("start");
1279  train(object,blobs,closestBlob);
1280  improve_train(object,blobs,closestBlob);
1281  burst("stop");
1282  triggerRecogInfo(object,blobs,closestBlob,"creation");
1283  ostringstream reply;
1284  reply<<"All right! Now I know what a "<<object;
1285  reply<<" is";
1286  speaker.speak(reply.str());
1287  look(blobs,closestBlob);
1288 
1289  replyHuman.addString("ack");
1290  rpcHuman.reply(replyHuman);
1291 }
1292 
1293 
1294 /**********************************************************/
1295 void Manager::execForget(const string &object)
1296 {
1297  Bottle cmdClassifier,replyClassifier,replyHuman;
1298 
1299  // grab resources
1300  mutexResources.lock();
1301 
1302  // forget the whole memory
1303  if (object=="all")
1304  {
1305  cmdClassifier.addVocab32("forget");
1306  cmdClassifier.addString("all");
1307  yInfo("Sending clearing request: %s",cmdClassifier.toString().c_str());
1308  rpcClassifier.write(cmdClassifier,replyClassifier);
1309  yInfo("Received reply: %s",replyClassifier.toString().c_str());
1310 
1311  // clear the memory too
1312  if (rpcMemory.getOutputCount()>0)
1313  {
1314  mutexResourcesMemory.lock();
1315  for (auto id=memoryIds.begin(); id!=memoryIds.end(); id++)
1316  {
1317  Bottle cmdMemory,replyMemory;
1318  cmdMemory.addVocab32("del");
1319  Bottle &bid=cmdMemory.addList().addList();
1320  bid.addString("id");
1321  bid.addInt32(id->second);
1322  rpcMemory.write(cmdMemory,replyMemory);
1323  }
1324  memoryIds.clear();
1325  mutexResourcesMemory.unlock();
1326  }
1327 
1328  db.clear();
1329  trackersPool.clear();
1330  speaker.speak("I have forgotten everything");
1331  replyHuman.addString("ack");
1332  }
1333  else // forget specific object
1334  {
1335  ostringstream reply;
1336  auto it=db.find(object);
1337  if (it!=db.end())
1338  {
1339  cmdClassifier.addVocab32("forget");
1340  cmdClassifier.addString(object);
1341  yInfo("Sending clearing request: %s",cmdClassifier.toString().c_str());
1342  rpcClassifier.write(cmdClassifier,replyClassifier);
1343  yInfo("Received reply: %s",replyClassifier.toString().c_str());
1344 
1345  // remove the item from the memory too
1346  if (rpcMemory.getOutputCount()>0)
1347  {
1348  mutexResourcesMemory.lock();
1349  auto id=memoryIds.find(object);
1350  auto memoryIdsEnd=memoryIds.end();
1351  mutexResourcesMemory.unlock();
1352 
1353  if (id!=memoryIdsEnd)
1354  {
1355  Bottle cmdMemory,replyMemory;
1356  cmdMemory.addVocab32("del");
1357  Bottle &bid=cmdMemory.addList().addList();
1358  bid.addString("id");
1359  bid.addInt32(id->second);
1360  rpcMemory.write(cmdMemory,replyMemory);
1361 
1362  mutexResourcesMemory.lock();
1363  memoryIds.erase(id);
1364  mutexResourcesMemory.unlock();
1365  }
1366  }
1367 
1368  db.erase(it);
1369  trackersPool.erase(object);
1370  reply<<object<<" forgotten";
1371  speaker.speak(reply.str());
1372  replyHuman.addString("ack");
1373  }
1374  else
1375  {
1376  yInfo("%s object is unknown",object.c_str());
1377  reply<<"I do not know any "<<object;
1378  speaker.speak(reply.str());
1379  replyHuman.addString("nack");
1380  }
1381  }
1382 
1383  rpcHuman.reply(replyHuman);
1384 
1385  // release resources
1386  mutexResources.unlock();
1387 }
1388 
1389 
1390 /**********************************************************/
1391 void Manager::execWhere(const string &object, const Bottle &blobs,
1392  const int recogBlob, Classifier *pClassifier,
1393  const string &recogType)
1394 {
1395  Bottle cmdHuman,valHuman,replyHuman;
1396 
1397  // some known object has been recognized
1398  if (recogBlob>=0)
1399  {
1400  // issue a [point] and wait for action completion
1401  point(object);
1402 
1403  yInfo("I think the %s is blob %d",object.c_str(),recogBlob);
1404  speaker.speak("Am I right?");
1405 
1406  replyHuman.addString("ack");
1407  replyHuman.addInt32(recogBlob);
1408  }
1409  // no known object has been recognized in the scene
1410  else
1411  {
1412  ostringstream reply;
1413  reply<<"I have not found any "<<object;
1414  reply<<", am I right?";
1415  speaker.speak(reply.str());
1416  yInfo("No object recognized");
1417 
1418  replyHuman.addString("nack");
1419  }
1420 
1421  rpcHuman.reply(replyHuman);
1422 
1423  // enter the human interaction mode to refine the knowledge
1424  bool ok=false;
1425  while (!ok)
1426  {
1427  replyHuman.clear();
1428  rpcHuman.read(cmdHuman,true);
1429 
1430  if (isStopping())
1431  return;
1432 
1433  int type=processHumanCmd(cmdHuman,valHuman);
1434  // do nothing
1435  if (type==Vocab32::encode("skip"))
1436  {
1437  speaker.speak("Skipped");
1438  replyHuman.addString("ack");
1439  ok=true;
1440  }
1441  // good job is done
1442  else if (type==Vocab32::encode("ack"))
1443  {
1444  // reinforce if an object is available
1445  if (!skipLearningUponSuccess && (recogBlob>=0) && (pClassifier!=NULL))
1446  {
1447  burst("start");
1448  train(object,blobs,recogBlob);
1449  improve_train(object,blobs,recogBlob);
1450  burst("stop");
1451  pClassifier->positive();
1452  triggerRecogInfo(object,blobs,recogBlob,"recognition");
1453  updateClassifierInMemory(pClassifier);
1454  }
1455 
1456  speaker.speak("Cool!");
1457  replyHuman.addString("ack");
1458  ok=true;
1459  }
1460  // misrecognition
1461  else if (type==Vocab32::encode("nack"))
1462  {
1463  // update the threshold if an object is available
1464  if ((recogBlob>=0) && (pClassifier!=NULL))
1465  {
1466  pClassifier->negative();
1467  updateClassifierInMemory(pClassifier);
1468  }
1469 
1470  // handle the human-pointed object
1471  cv::Point loc;
1472  if (pointedLoc.getLoc(loc))
1473  {
1474  int closestBlob=findClosestBlob(blobs,loc);
1475  burst("start");
1476  train(object,blobs,closestBlob);
1477  improve_train(object,blobs,closestBlob);
1478  burst("stop");
1479  triggerRecogInfo(object,blobs,closestBlob,recogType);
1480  speaker.speak("Oooh, I see");
1481  look(blobs,closestBlob);
1482  }
1483  else
1484  speaker.speak("Ooops! Sorry, I missed where you pointed at");
1485 
1486  replyHuman.addString("ack");
1487  ok=true;
1488  }
1489  else
1490  {
1491  speaker.speak("Hmmm hmmm hmmm! Try again");
1492  replyHuman.addString("nack");
1493  }
1494 
1495  rpcHuman.reply(replyHuman);
1496  }
1497 }
1498 
1499 
1500 /**********************************************************/
1501 void Manager::execWhat(const Bottle &blobs, const int pointedBlob,
1502  const Bottle &scores, const string &object)
1503 {
1504  Bottle cmdHuman,valHuman,replyHuman;
1505  Classifier *pClassifier=NULL;
1506 
1507  // some known object has been recognized
1508  if (object!=OBJECT_UNKNOWN)
1509  {
1510  ostringstream reply;
1511  reply<<"I think it is the "<<object;
1512  speaker.speak(reply.str());
1513  speaker.speak("Am I right?");
1514  yInfo("I think the blob %d is the %s",pointedBlob,object.c_str());
1515 
1516  // retrieve the corresponding classifier
1517  auto it=db.find(object);
1518  if (it!=db.end())
1519  pClassifier=it->second;
1520 
1521  replyHuman.addString("ack");
1522  replyHuman.addString(object);
1523  }
1524  // no known object has been recognized in the scene
1525  else
1526  {
1527  speaker.speak("I do not know this object");
1528  speaker.speak("What is it?");
1529  yInfo("No object recognized");
1530  replyHuman.addString("nack");
1531  }
1532 
1533  rpcHuman.reply(replyHuman);
1534 
1535  // enter the human interaction mode to refine the knowledge
1536  bool ok=false;
1537  while (!ok)
1538  {
1539  replyHuman.clear();
1540  rpcHuman.read(cmdHuman,true);
1541 
1542  if (isStopping())
1543  return;
1544 
1545  int type=processHumanCmd(cmdHuman,valHuman);
1546  // do nothing
1547  if (type==Vocab32::encode("skip"))
1548  {
1549  speaker.speak("Skipped");
1550  replyHuman.addString("ack");
1551  ok=true;
1552  }
1553  // good job is done
1554  else if ((object!=OBJECT_UNKNOWN) && (type==Vocab32::encode("ack")))
1555  {
1556  // reinforce if an object is available
1557  if (!skipLearningUponSuccess && (pointedBlob>=0) && (pClassifier!=NULL))
1558  {
1559  burst("start");
1560  train(object,blobs,pointedBlob);
1561  improve_train(object,blobs,pointedBlob);
1562  burst("stop");
1563  db.processScores(pClassifier,scores);
1564  pClassifier->positive();
1565  triggerRecogInfo(object,blobs,pointedBlob,"recognition");
1566  updateClassifierInMemory(pClassifier);
1567  }
1568 
1569  speaker.speak("Cool!");
1570  replyHuman.addString("ack");
1571  ok=true;
1572  }
1573  // misrecognition
1574  else if (type==Vocab32::encode("nack"))
1575  {
1576  // update the threshold
1577  if ((pointedBlob>=0) && (pClassifier!=NULL))
1578  {
1579  db.processScores(pClassifier,scores);
1580  pClassifier->negative();
1581  updateClassifierInMemory(pClassifier);
1582  }
1583 
1584  speaker.speak("Sorry");
1585  replyHuman.addString("ack");
1586  ok=true;
1587  }
1588  // handle new/unrecognized/misrecognized object
1589  else if ((type==Vocab32::encode("name")) && (valHuman.size()>0))
1590  {
1591  string objectName=valHuman.get(0).asString();
1592 
1593  // check whether the object is already known
1594  // and, if not, allocate space for it
1595  auto it=db.find(objectName);
1596  if (it==db.end())
1597  {
1598  db[objectName]=new Classifier(objectName,classification_threshold);
1599  trackersPool[objectName]=Tracker(tracker_type,tracker_timeout);
1600  it=db.find(objectName);
1601  speaker.speak("Oooh, I see");
1602  yInfo("created classifier for %s",objectName.c_str());
1603  }
1604  else
1605  {
1606  // update the threshold for the case of misrecognition
1607  if ((pClassifier!=NULL) && (object!=objectName) && (object!=OBJECT_UNKNOWN))
1608  {
1609  db.processScores(pClassifier,scores);
1610  pClassifier->negative();
1611  updateClassifierInMemory(pClassifier);
1612  }
1613 
1614  ostringstream reply;
1615  reply<<"Sorry, I should have recognized the "<<objectName;
1616  speaker.speak(reply.str());
1617  }
1618 
1619  // trigger the classifier
1620  if (pointedBlob>=0)
1621  {
1622  burst("start");
1623  train(objectName,blobs,pointedBlob);
1624  improve_train(objectName,blobs,pointedBlob);
1625  burst("stop");
1626  triggerRecogInfo(objectName,blobs,pointedBlob,
1627  (object==OBJECT_UNKNOWN)?"creation":"recognition");
1628  }
1629 
1630  db.processScores(it->second,scores);
1631 
1632  replyHuman.addString("ack");
1633  ok=true;
1634  }
1635  else
1636  {
1637  speaker.speak("Hmmm hmmm hmmm! Try again");
1638  replyHuman.addString("nack");
1639  }
1640 
1641  rpcHuman.reply(replyHuman);
1642  }
1643 }
1644 
1645 
1646 /**********************************************************/
1647 void Manager::execThis(const string &object, const string &detectedObject,
1648  const Bottle &blobs, const int &pointedBlob)
1649 {
1650  if (pointedBlob>=0)
1651  {
1652  Bottle replyHuman;
1653 
1654  string recogType="recognition";
1655  auto it=db.find(object);
1656  if (it==db.end())
1657  {
1658  // if not, create a brand new one
1659  db[object]=new Classifier(object,classification_threshold);
1660  trackersPool[object]=Tracker(tracker_type,tracker_timeout);
1661  it=db.find(object);
1662  yInfo("created classifier for %s",object.c_str());
1663  recogType="creation";
1664  }
1665 
1666  ostringstream reply;
1667 
1668  //if the classifier recognized the object
1669  if (object.compare(detectedObject)==0)
1670  reply<<"Yes, I know that is a "<<object<<"!";
1671  else if(detectedObject.compare(OBJECT_UNKNOWN)==0)
1672  reply<<"All right! Now I know what a "<<object<<" is!";
1673  else
1674  {
1675  reply<<"Oh dear, I thought that was a "<<detectedObject<<"?";
1676 
1677  auto it_detected=db.find(detectedObject);
1678  if (it_detected==db.end())
1679  {
1680  it_detected->second->negative();
1681  updateClassifierInMemory(it_detected->second);
1682  }
1683  }
1684 
1685  burst("start");
1686  train(object,blobs,pointedBlob);
1687  improve_train(object,blobs,pointedBlob);
1688  burst("stop");
1689 
1690  speaker.speak(reply.str());
1691  look(blobs,pointedBlob);
1692 
1693  replyHuman.addString("ack");
1694  rpcHuman.reply(replyHuman);
1695  }
1696 }
1697 
1698 
1699 /**********************************************************/
1700 void Manager::execExplore(const string &object)
1701 {
1702  Bottle cmdMotor,replyMotor,replyHuman;
1703  Vector position;
1704 
1705  if (get3DPositionFromMemory(object,position))
1706  {
1707  cmdMotor.addVocab32("look");
1708  cmdMotor.addString(object);
1709  cmdMotor.addString("fixate");
1710  rpcMotor.write(cmdMotor,replyMotor);
1711 
1712  if (replyMotor.get(0).asVocab32()==Vocab32::encode("ack"))
1713  {
1714  ostringstream reply;
1715  reply<<"I will explore the "<<object;
1716  speaker.speak(reply.str());
1717 
1718  exploration.setInfo(object,position);
1719 
1720  burst("start");
1721  exploration.start();
1722 
1723  cmdMotor.clear();
1724  cmdMotor.addVocab32("explore");
1725  cmdMotor.addVocab32("torso");
1726  rpcMotor.write(cmdMotor,replyMotor);
1727 
1728  exploration.stop();
1729  do Time::delay(0.1);
1730  while (exploration.isRunning());
1731  burst("stop");
1732 
1733  home();
1734 
1735  cmdMotor.clear();
1736  cmdMotor.addVocab32("idle");
1737  rpcMotor.write(cmdMotor,replyMotor);
1738  speaker.speak("I'm done");
1739 
1740  replyHuman.addString("ack");
1741  }
1742  else
1743  {
1744  speaker.speak("Sorry, something went wrong with the exploration");
1745  replyHuman.addString("nack");
1746  }
1747  }
1748  else
1749  {
1750  speaker.speak("Sorry, something went wrong with the exploration");
1751  replyHuman.addString("nack");
1752  }
1753 
1754  rpcHuman.reply(replyHuman);
1755 }
1756 
1757 
1758 /**********************************************************/
1759 void Manager::execReinforce(const string &object,
1760  const Vector &position)
1761 {
1762  bool ret=false;
1763  if (db.find(object)!=db.end())
1764  {
1765  burst("start");
1766  ret=doExploration(object,position);
1767  burst("stop");
1768  }
1769 
1770  Bottle replyHuman(ret?"ack":"nack");
1771  rpcHuman.reply(replyHuman);
1772 }
1773 
1774 
1775 /**********************************************************/
1776 void Manager::execInterruptableAction(const string &action,
1777  const string &object,
1778  const Vector &x,
1779  const Bottle &blobs,
1780  const int recogBlob)
1781 {
1782  Bottle replyHuman;
1783 
1784  // the object has been recognized
1785  if (recogBlob>=0)
1786  {
1787  ostringstream reply;
1788  reply<<"Ok, I will "<<action;
1789  if (action=="drop")
1790  reply<<" over ";
1791  reply<<" the "<<object;
1792  speaker.speak(reply.str());
1793  yInfo("I think the %s is blob %d",object.c_str(),recogBlob);
1794 
1795  // issue the action and wait for action completion/interruption
1796  Vector y;
1797  if (interruptableAction(action,NULL,object,x,y))
1798  {
1799  replyHuman.addString("ack");
1800  replyHuman.addInt32(recogBlob);
1801  }
1802  else
1803  replyHuman.addString("nack");
1804  }
1805  // drop straightaway what's in the hand
1806  else if ((action=="drop") && (object==""))
1807  {
1808  speaker.speak("Ok");
1809 
1810  Bottle cmdMotor,replyMotor;
1811  cmdMotor.addVocab32("drop");
1812  addDropPosition(cmdMotor);
1813  actionInterrupted=false;
1814  enableInterrupt=true;
1815  rpcMotor.write(cmdMotor,replyMotor);
1816 
1817  if (replyMotor.get(0).asVocab32()==Vocab32::encode("nack"))
1818  {
1819  speaker.speak("I have nothing in my hands");
1820  replyHuman.addString("nack");
1821  }
1822  else if (actionInterrupted)
1823  {
1824  reinstateMotor();
1825  home();
1826  replyHuman.addString("nack");
1827  }
1828  else
1829  replyHuman.addString("ack");
1830 
1831  enableInterrupt=false;
1832  }
1833  // no object has been recognized in the scene
1834  else
1835  {
1836  ostringstream reply;
1837  reply<<"I am sorry, I cannot see any "<<object;
1838  reply<<" around";
1839  speaker.speak(reply.str());
1840 
1841  replyHuman.addString("nack");
1842  }
1843 
1844  rpcHuman.reply(replyHuman);
1845 }
1846 
1847 
1848 /**********************************************************/
1849 void Manager::switchAttention()
1850 {
1851  // skip if connection with motor interface is not in place
1852  if (rpcMotor.getOutputCount()>0)
1853  {
1854  lock_guard<mutex> lg(mutexAttention);
1855 
1856  // grab the blobs
1857  Bottle blobs=getBlobs();
1858  for (int i=0; i<blobs.size(); i++)
1859  {
1860  // make a guess
1861  int guess=(int)Rand::scalar(0.0,blobs.size());
1862  if (guess>=blobs.size())
1863  guess=blobs.size()-1;
1864 
1865  cv::Point cog=getBlobCOG(blobs,guess);
1866  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
1867  continue;
1868 
1869  look(blobs,guess);
1870  return;
1871  }
1872 
1873  // if no good blob found go home
1874  home("gaze");
1875  }
1876 }
1877 
1878 
1879 /**********************************************************/
1880 void Manager::doLocalization()
1881 {
1882  // acquire image for classification/training
1883  acquireImage(true);
1884  // grab the blobs
1885  Bottle blobs=getBlobs();
1886  // get the scores from the learning machine
1887  Bottle scores=classify(blobs,true);
1888  // update location of histogram display
1889  if (Bottle *loc=histObjLocPort.read(false))
1890  {
1891  if (loc->size()>=2)
1892  {
1893  Vector x;
1894  clickLocation=cv::Point(loc->get(0).asInt32(),loc->get(1).asInt32());
1895  if (get3DPosition(clickLocation,x))
1896  histObjLocation=x;
1897  }
1898  }
1899  // find the closest blob to the location of histogram display
1900  int closestBlob=findClosestBlob(blobs,histObjLocation);
1901  // draw the blobs
1902  drawBlobs(blobs,closestBlob,&scores);
1903  // draw scores histogram
1904  drawScoresHistogram(blobs,scores,closestBlob);
1905 
1906  // data for memory update
1907  mutexResourcesMemory.lock();
1908  memoryBlobs=blobs;
1909  memoryScores=scores;
1910  mutexResourcesMemory.unlock();
1911 }
1912 
1913 
1914 /**********************************************************/
1915 bool Manager::get3DPositionFromMemory(const string &object,
1916  Vector &position,
1917  const bool lockMemory)
1918 {
1919  bool ret=false;
1920  if (rpcMemory.getOutputCount()>0)
1921  {
1922  // grab resources
1923  if (lockMemory)
1924  lock_guard<mutex> lg(mutexMemoryUpdate);
1925 
1926  mutexResourcesMemory.lock();
1927  auto id=memoryIds.find(object);
1928  auto memoryIdsEnd=memoryIds.end();
1929  mutexResourcesMemory.unlock();
1930 
1931  if (id!=memoryIdsEnd)
1932  {
1933  // get the relevant properties
1934  // [get] (("id" <num>) ("propSet" ("position_3d")))
1935  Bottle cmdMemory,replyMemory;
1936  cmdMemory.addVocab32("get");
1937  Bottle &content=cmdMemory.addList();
1938  Bottle &list_bid=content.addList();
1939  list_bid.addString("id");
1940  list_bid.addInt32(id->second);
1941  Bottle &list_propSet=content.addList();
1942  list_propSet.addString("propSet");
1943  Bottle &list_items=list_propSet.addList();
1944  list_items.addString("position_3d");
1945  rpcMemory.write(cmdMemory,replyMemory);
1946 
1947  // retrieve 3D position
1948  if (replyMemory.get(0).asVocab32()==Vocab32::encode("ack"))
1949  {
1950  if (Bottle *propField=replyMemory.get(1).asList())
1951  {
1952  if (propField->check("position_3d"))
1953  {
1954  if (Bottle *pPos=propField->find("position_3d").asList())
1955  {
1956  if (pPos->size()>=3)
1957  {
1958  position.resize(3);
1959  position[0]=pPos->get(0).asFloat64();
1960  position[1]=pPos->get(1).asFloat64();
1961  position[2]=pPos->get(2).asFloat64();
1962  ret=true;
1963  }
1964  }
1965  }
1966  }
1967  }
1968  }
1969  }
1970 
1971  return ret;
1972 }
1973 
1974 
1975 /**********************************************************/
1976 bool Manager::doExploration(const string &object,
1977  const Vector &position)
1978 {
1979  // acquire image for training
1980  acquireImage();
1981 
1982  // grab the blobs
1983  Bottle blobs=getBlobs();
1984 
1985  // failure handling
1986  if (blobs.size()==0)
1987  return false;
1988 
1989  // enforce 3D consistency
1990  int exploredBlob=RET_INVALID;
1991  double curMinDist=0.05;
1992  for (int i=0; i<blobs.size(); i++)
1993  {
1994  cv::Point cog=getBlobCOG(blobs,i);
1995  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
1996  continue;
1997 
1998  Vector x;
1999  if (get3DPosition(cog,x))
2000  {
2001  double dist=norm(position-x);
2002  if (dist<curMinDist)
2003  {
2004  exploredBlob=i;
2005  curMinDist=dist;
2006  }
2007  }
2008  }
2009 
2010  // no candidate found => skip
2011  if (exploredBlob<0)
2012  return false;
2013 
2014  // train the classifier
2015  train(object,blobs,exploredBlob);
2016 
2017  // draw the blobs highlighting the explored one
2018  drawBlobs(blobs,exploredBlob);
2019  return true;
2020 }
2021 
2022 
2023 /**********************************************************/
2024 void Manager::updateMemory()
2025 {
2026  if (rpcMemory.getOutputCount()>0)
2027  {
2028  // grab resources
2029  mutexMemoryUpdate.lock();
2030 
2031  // load memory on connection event
2032  if (scheduleLoadMemory)
2033  {
2034  loadMemory();
2035  scheduleLoadMemory=false;
2036  }
2037 
2038  // latch image
2039  ImageOf<PixelBgr> &imgLatch=imgTrackOut.prepare();
2040  cv::Mat imgLatchMat=toCvMat(imgLatch);
2041 
2042  mutexResourcesMemory.lock();
2043  Bottle blobs=memoryBlobs;
2044  Bottle scores=memoryScores;
2045  imgLatch=imgRtLoc;
2046  mutexResourcesMemory.unlock();
2047 
2048  // reset internal tracking state
2049  for (auto it=trackersPool.begin(); it!=trackersPool.end(); it++)
2050  it->second.prepare();
2051 
2052  // Classification scores for each object
2053  std::map<string, double> scoresMap;
2054 
2055  for (int j=0; j<blobs.size(); j++)
2056  {
2057  Bottle *item=blobs.get(j).asList();
2058  if (item==NULL)
2059  continue;
2060 
2061  ostringstream tag;
2062  tag<<"blob_"<<j;
2063 
2064  // find the blob name and classification score (or unknown)
2065  double score = 0;
2066  mutexResources.lock();
2067  string object=db.findName(scores,tag.str(),&score);
2068  mutexResources.unlock();
2069 
2070  if (object!=OBJECT_UNKNOWN)
2071  {
2072  scoresMap[object] = score;
2073 
2074  // compute the bounding box
2075  cv::Point tl,br;
2076  tl.x=(int)item->get(0).asFloat64();
2077  tl.y=(int)item->get(1).asFloat64();
2078  br.x=(int)item->get(2).asFloat64();
2079  br.y=(int)item->get(3).asFloat64();
2080 
2081  cv::Rect bbox(tl.x,tl.y,br.x-tl.x,br.y-tl.y);
2082  if (thresBBox(bbox,imgLatch))
2083  {
2084  auto tracker=trackersPool.find(object);
2085  if (tracker!=trackersPool.end())
2086  tracker->second.latchBBox(bbox);
2087  }
2088  }
2089  }
2090 
2091  // cycle over objects to handle tracking
2092  set<int> avalObjIds;
2093  for (auto it=trackersPool.begin(); it!=trackersPool.end(); it++)
2094  {
2095  string object=it->first;
2096  it->second.track(imgLatch);
2097 
2098  cv::Rect bbox;
2099  if (it->second.is_tracking(bbox))
2100  {
2101  // threshold bbox
2102  if (!thresBBox(bbox,imgLatch))
2103  continue;
2104 
2105  cv::Point tl(bbox.x,bbox.y);
2106  cv::Point br(bbox.x+bbox.width,bbox.y+bbox.height);
2107  cv::Point cog((tl.x+br.x)>>1,(tl.y+br.y)>>1);
2108  Vector x;
2109 
2110  // find 3d position
2111  if (get3DPosition(cog,x))
2112  {
2113  // prepare position_2d property
2114  Bottle position_2d;
2115  Bottle &list_2d=position_2d.addList();
2116  list_2d.addString("position_2d_"+camera);
2117  Bottle &list_2d_c=list_2d.addList();
2118  list_2d_c.addFloat64(tl.x);
2119  list_2d_c.addFloat64(tl.y);
2120  list_2d_c.addFloat64(br.x);
2121  list_2d_c.addFloat64(br.y);
2122 
2123  // prepare position_3d property
2124  Bottle position_3d;
2125  Bottle &list_3d=position_3d.addList();
2126  list_3d.addString("position_3d");
2127  list_3d.addList().read(x);
2128 
2129  // prepare class_score property if the object's been freshly recognized
2130  Bottle class_score;
2131  auto it=scoresMap.find(object);
2132  if (it!=scoresMap.end())
2133  {
2134  Bottle &list_score=class_score.addList();
2135  list_score.addString("class_score");
2136  list_score.addFloat64(it->second);
2137  }
2138 
2139  mutexResourcesMemory.lock();
2140  auto id=memoryIds.find(object);
2141  auto memoryIdsEnd=memoryIds.end();
2142  mutexResourcesMemory.unlock();
2143 
2144  Bottle cmdMemory,replyMemory;
2145  if (id==memoryIdsEnd) // the object is not available => [add]
2146  {
2147  cmdMemory.addVocab32("add");
2148  Bottle &content=cmdMemory.addList();
2149  Bottle &list_entity=content.addList();
2150  list_entity.addString("entity");
2151  list_entity.addString("object");
2152  Bottle &list_name=content.addList();
2153  list_name.addString("name");
2154  list_name.addString(object);
2155  content.append(position_2d);
2156  content.append(position_3d);
2157  if (class_score.size()>0)
2158  content.append(class_score);
2159 
2160  rpcMemory.write(cmdMemory,replyMemory);
2161 
2162  if (replyMemory.size()>1)
2163  {
2164  // store the id for later usage
2165  if (replyMemory.get(0).asVocab32()==Vocab32::encode("ack"))
2166  {
2167  if (Bottle *idField=replyMemory.get(1).asList())
2168  {
2169  int id=idField->get(1).asInt32();
2170  mutexResourcesMemory.lock();
2171  memoryIds[object]=id;
2172  mutexResourcesMemory.unlock();
2173 
2174  avalObjIds.insert(id);
2175  }
2176  else
2177  continue;
2178  }
2179  }
2180  }
2181  else // the object is already available => [set]
2182  {
2183  // prepare id property
2184  Bottle bid;
2185  Bottle &list_bid=bid.addList();
2186  list_bid.addString("id");
2187  list_bid.addInt32(id->second);
2188 
2189  cmdMemory.addVocab32("set");
2190  Bottle &content=cmdMemory.addList();
2191  content.append(bid);
2192  content.append(position_2d);
2193  content.append(position_3d);
2194  if (class_score.size()>0)
2195  content.append(class_score);
2196 
2197  rpcMemory.write(cmdMemory,replyMemory);
2198 
2199  avalObjIds.insert(id->second);
2200  }
2201 
2202  // highlight location of tracked blobs within images
2203  cv::rectangle(imgLatchMat,tl,br,cv::Scalar(255,0,0),2);
2204  cv::putText(imgLatchMat,object,cv::Point(tl.x,tl.y-5),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(255,0,0),2);
2205  }
2206  }
2207  }
2208 
2209  if (imgTrackOut.getOutputCount()>0)
2210  imgTrackOut.write();
2211  else
2212  imgTrackOut.unprepare();
2213 
2214  // remove position properties of objects not in scene
2215  mutexResourcesMemory.lock();
2216  for (auto it=memoryIds.begin(); it!=memoryIds.end(); it++)
2217  {
2218  int id=it->second;
2219  if (avalObjIds.find(id)==avalObjIds.end())
2220  {
2221  Bottle cmdMemory,replyMemory;
2222  cmdMemory.addVocab32("del");
2223  Bottle &content=cmdMemory.addList();
2224  Bottle &list_bid=content.addList();
2225  list_bid.addString("id");
2226  list_bid.addInt32(id);
2227  Bottle &list_propSet=content.addList();
2228  list_propSet.addString("propSet");
2229  Bottle &list_items=list_propSet.addList();
2230  list_items.addString("position_2d_"+camera);
2231  list_items.addString("position_3d");
2232  list_items.addString("class_score");
2233  rpcMemory.write(cmdMemory,replyMemory);
2234  }
2235  }
2236  mutexResourcesMemory.unlock();
2237 
2238  // release resources
2239  mutexMemoryUpdate.unlock();
2240  }
2241 }
2242 
2243 
2244 /**********************************************************/
2245 void Manager::updateClassifierInMemory(Classifier *pClassifier)
2246 {
2247  if ((rpcMemory.getOutputCount()>0) && (pClassifier!=NULL))
2248  {
2249  string objectName=pClassifier->getName();
2250 
2251  // prepare classifier_thresholds property
2252  Bottle classifier_property;
2253  Bottle &list_classifier=classifier_property.addList();
2254  list_classifier.addString("classifier_thresholds");
2255  list_classifier.addList().append(pClassifier->toBottle());
2256 
2257  mutexResourcesMemory.lock();
2258  auto id=memoryIds.find(objectName);
2259  auto memoryIdsEnd=memoryIds.end();
2260  mutexResourcesMemory.unlock();
2261 
2262  Bottle cmdMemory,replyMemory;
2263  if (id==memoryIdsEnd) // the object is not available => [add]
2264  {
2265  cmdMemory.addVocab32("add");
2266  Bottle &content=cmdMemory.addList();
2267  Bottle &list_entity=content.addList();
2268  list_entity.addString("entity");
2269  list_entity.addString("object");
2270  Bottle &list_name=content.addList();
2271  list_name.addString("name");
2272  list_name.addString(objectName);
2273  content.append(classifier_property);
2274  rpcMemory.write(cmdMemory,replyMemory);
2275 
2276  if (replyMemory.size()>1)
2277  {
2278  // store the id for later usage
2279  if (replyMemory.get(0).asVocab32()==Vocab32::encode("ack"))
2280  {
2281  if (Bottle *idField=replyMemory.get(1).asList())
2282  {
2283  mutexResourcesMemory.lock();
2284  memoryIds[objectName]=idField->get(1).asInt32();
2285  mutexResourcesMemory.unlock();
2286  }
2287  }
2288  }
2289  }
2290  else // the object is already available => [set]
2291  {
2292  // prepare id property
2293  Bottle bid;
2294  Bottle &list_bid=bid.addList();
2295  list_bid.addString("id");
2296  list_bid.addInt32(id->second);
2297 
2298  cmdMemory.addVocab32("set");
2299  Bottle &content=cmdMemory.addList();
2300  content.append(bid);
2301  content.append(classifier_property);
2302  rpcMemory.write(cmdMemory,replyMemory);
2303  }
2304  }
2305 }
2306 
2307 
2308 /**********************************************************/
2309 Vector Manager::updateObjCartPosInMemory(const string &object,
2310  const Bottle &blobs,
2311  const int i)
2312 {
2313  Vector x(3,0.0);
2314  if ((rpcMemory.getOutputCount()>0) && (i!=RET_INVALID) && (i<blobs.size()))
2315  {
2316  mutexResourcesMemory.lock();
2317  auto id=memoryIds.find(object);
2318  auto memoryIdsEnd=memoryIds.end();
2319  mutexResourcesMemory.unlock();
2320 
2321  Bottle *item=blobs.get(i).asList();
2322  if ((id!=memoryIdsEnd) && (item!=NULL))
2323  {
2324  cv::Point cog=getBlobCOG(blobs,i);
2325  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
2326  return x;
2327 
2328  if (get3DPosition(cog,x))
2329  {
2330  Bottle cmdMemory,replyMemory;
2331 
2332  // prepare id property
2333  Bottle bid;
2334  Bottle &list_bid=bid.addList();
2335  list_bid.addString("id");
2336  list_bid.addInt32(id->second);
2337 
2338  // prepare position_2d property
2339  Bottle position_2d;
2340  Bottle &list_2d=position_2d.addList();
2341  list_2d.addString("position_2d_"+camera);
2342  Bottle &list_2d_c=list_2d.addList();
2343  list_2d_c.addFloat64(item->get(0).asFloat64());
2344  list_2d_c.addFloat64(item->get(1).asFloat64());
2345  list_2d_c.addFloat64(item->get(2).asFloat64());
2346  list_2d_c.addFloat64(item->get(3).asFloat64());
2347 
2348  // prepare position_3d property
2349  Bottle position_3d;
2350  Bottle &list_3d=position_3d.addList();
2351  list_3d.addString("position_3d");
2352  list_3d.addList().read(x);
2353 
2354  cmdMemory.addVocab32("set");
2355  Bottle &content=cmdMemory.addList();
2356  content.append(bid);
2357  content.append(position_2d);
2358  content.append(position_3d);
2359  rpcMemory.write(cmdMemory,replyMemory);
2360  }
2361  }
2362  }
2363 
2364  return x;
2365 }
2366 
2367 
2368 /**********************************************************/
2369 void Manager::triggerRecogInfo(const string &object, const Bottle &blobs,
2370  const int i, const string &recogType)
2371 {
2372  if ((recogTriggerPort.getOutputCount()>0) && (i!=RET_INVALID) && (i<blobs.size()))
2373  {
2374  cv::Point cog=getBlobCOG(blobs,i);
2375  if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
2376  return;
2377 
2378  Vector x;
2379  if (get3DPosition(cog,x))
2380  {
2381  Property &msg=recogTriggerPort.prepare();
2382  msg.clear();
2383 
2384  Bottle pos; pos.addList().read(x);
2385  msg.put("label",object);
2386  msg.put("position_3d",pos.get(0));
2387  msg.put("type",recogType);
2388 
2389  recogTriggerPort.write();
2390  }
2391  }
2392 }
2393 
2394 
2395 /**********************************************************/
2396 void Manager::loadMemory()
2397 {
2398  yInfo("Loading memory ...");
2399  // grab resources
2400  mutexResourcesMemory.lock();
2401 
2402  // purge internal databases
2403  memoryIds.clear();
2404  db.clear();
2405  trackersPool.clear();
2406 
2407  // ask for all the items stored in memory
2408  Bottle cmdMemory,replyMemory,replyMemoryProp;
2409  cmdMemory.addVocab32("ask");
2410  Bottle &content=cmdMemory.addList().addList();
2411  content.addString("entity");
2412  content.addString("==");
2413  content.addString("object");
2414  rpcMemory.write(cmdMemory,replyMemory);
2415 
2416  if (replyMemory.size()>1)
2417  {
2418  if (replyMemory.get(0).asVocab32()==Vocab32::encode("ack"))
2419  {
2420  if (Bottle *idField=replyMemory.get(1).asList())
2421  {
2422  if (Bottle *idValues=idField->get(1).asList())
2423  {
2424  // cycle over items
2425  for (int i=0; i<idValues->size(); i++)
2426  {
2427  int id=idValues->get(i).asInt32();
2428 
2429  // get the relevant properties
2430  // [get] (("id" <num>) ("propSet" ("name" "classifier_thresholds")))
2431  cmdMemory.clear();
2432  cmdMemory.addVocab32("get");
2433  Bottle &content=cmdMemory.addList();
2434  Bottle &list_bid=content.addList();
2435  list_bid.addString("id");
2436  list_bid.addInt32(id);
2437  Bottle &list_propSet=content.addList();
2438  list_propSet.addString("propSet");
2439  Bottle &list_items=list_propSet.addList();
2440  list_items.addString("name");
2441  list_items.addString("classifier_thresholds");
2442  rpcMemory.write(cmdMemory,replyMemoryProp);
2443 
2444  // update internal databases
2445  if (replyMemoryProp.get(0).asVocab32()==Vocab32::encode("ack"))
2446  {
2447  if (Bottle *propField=replyMemoryProp.get(1).asList())
2448  {
2449  if (propField->check("name"))
2450  {
2451  string object=propField->find("name").asString();
2452  memoryIds[object]=id;
2453 
2454  if (propField->check("classifier_thresholds"))
2455  db[object]=new Classifier(*propField->find("classifier_thresholds").asList());
2456  else
2457  db[object]=new Classifier(object,classification_threshold);
2458  trackersPool[object]=Tracker(tracker_type,tracker_timeout);
2459  }
2460  }
2461  }
2462  }
2463  }
2464  }
2465  }
2466  }
2467 
2468  yInfo("Objects in memory: %d",(int)db.size());
2469  for (auto it=db.begin(); it!=db.end(); it++)
2470  {
2471  string object=it->first;
2472  string properties=it->second->toBottle().toString();
2473  yInfo("classifier for %s: memory_id=%d; properties=%s",
2474  object.c_str(),memoryIds[object],properties.c_str());
2475  }
2476 
2477  // release resources
2478  mutexResourcesMemory.unlock();
2479  yInfo("Memory loaded");
2480 }
2481 
2482 
2483 /**********************************************************/
2484 bool Manager::configure(ResourceFinder &rf)
2485 {
2486  name=rf.check("name",Value("iolStateMachineHandler")).asString();
2487  camera=rf.check("camera",Value("left")).asString();
2488  if ((camera!="left") && (camera!="right"))
2489  camera="left";
2490 
2491  imgIn.open("/"+name+"/img:i");
2492  blobExtractor.open("/"+name+"/blobs:i");
2493  imgOut.open("/"+name+"/img:o");
2494  imgRtLocOut.open("/"+name+"/imgLoc:o");
2495  imgTrackOut.open("/"+name+"/imgTrack:o");
2496  imgClassifier.open("/"+name+"/imgClassifier:o");
2497  imgHistogram.open("/"+name+"/imgHistogram:o");
2498  histObjLocPort.open("/"+name+"/histObjLocation:i");
2499  recogTriggerPort.open("/"+name+"/recog:o");
2500 
2501  rpcPort.open("/"+name+"/rpc");
2502  rpcHuman.open("/"+name+"/human:rpc");
2503  rpcClassifier.open("/"+name+"/classify:rpc");
2504  rpcMotor.open("/"+name+"/motor:rpc");
2505  rpcMotorGrasp.open("/"+name+"/motor_grasp:rpc");
2506  rpcReachCalib.open("/"+name+"/reach_calib:rpc");
2507  rpcGet3D.open("/"+name+"/get3d:rpc");
2508  rpcMotorStop.open("/"+name+"/motor_stop:rpc");
2509  rxMotorStop.open("/"+name+"/motor_stop:i");
2510  rxMotorStop.setManager(this);
2511 
2512  pointedLoc.open("/"+name+"/point:i");
2513  speaker.open("/"+name+"/speak:o");
2514 
2515  memoryReporter.setManager(this);
2516  rpcMemory.setReporter(memoryReporter);
2517  rpcMemory.open("/"+name+"/memory:rpc");
2518 
2519  skim_blobs_x_bounds.resize(2);
2520  skim_blobs_x_bounds[0]=-0.50;
2521  skim_blobs_x_bounds[1]=-0.10;
2522  if (rf.check("skim_blobs_x_bounds"))
2523  {
2524  if (Bottle *bounds=rf.find("skim_blobs_x_bounds").asList())
2525  {
2526  if (bounds->size()>=2)
2527  {
2528  skim_blobs_x_bounds[0]=bounds->get(0).asFloat64();
2529  skim_blobs_x_bounds[1]=bounds->get(1).asFloat64();
2530  }
2531  }
2532  }
2533 
2534  skim_blobs_y_bounds.resize(2);
2535  skim_blobs_y_bounds[0]=-0.30;
2536  skim_blobs_y_bounds[1]=+0.30;
2537  if (rf.check("skim_blobs_y_bounds"))
2538  {
2539  if (Bottle *bounds=rf.find("skim_blobs_y_bounds").asList())
2540  {
2541  if (bounds->size()>=2)
2542  {
2543  skim_blobs_y_bounds[0]=bounds->get(0).asFloat64();
2544  skim_blobs_y_bounds[1]=bounds->get(1).asFloat64();
2545  }
2546  }
2547  }
2548 
2549  // location used to display the
2550  // histograms upon the closest blob
2551  clickLocation=cv::Point(0,0);
2552  histObjLocation.resize(3);
2553  histObjLocation[0]=-0.3;
2554  histObjLocation[1]=0.0;
2555  histObjLocation[2]=-0.1;
2556 
2557  attention.setManager(this);
2558  attention.start();
2559 
2560  doAttention=rf.check("attention",Value("on")).asString()=="on";
2561  if (!doAttention)
2562  attention.suspend();
2563 
2564  lastBlobsArrivalTime=0.0;
2565  rtLocalization.setManager(this);
2566  rtLocalization.setPeriod((double)rf.check("rt_localization_period",Value(30)).asInt32()/1000.0);
2567  rtLocalization.start();
2568 
2569  exploration.setPeriod((double)rf.check("exploration_period",Value(30)).asInt32()/1000.0);
2570  exploration.setManager(this);
2571 
2572  memoryUpdater.setManager(this);
2573  memoryUpdater.setPeriod((double)rf.check("memory_update_period",Value(60)).asInt32()/1000.0);
2574  memoryUpdater.start();
2575 
2576  blobs_detection_timeout=rf.check("blobs_detection_timeout",Value(0.2)).asFloat64();
2577  improve_train_period=rf.check("improve_train_period",Value(0.0)).asFloat64();
2578  trainOnFlipped=rf.check("train_flipped_images",Value("off")).asString()=="on";
2579  trainBurst=rf.check("train_burst_images",Value("off")).asString()=="on";
2580  skipLearningUponSuccess=rf.check("skip_learning_upon_success",Value("off")).asString()=="on";
2581  classification_threshold=rf.check("classification_threshold",Value(0.5)).asFloat64();
2582  tracker_type=rf.check("tracker_type",Value("BOOSTING")).asString();
2583  tracker_timeout=std::max(0.0,rf.check("tracker_timeout",Value(5.0)).asFloat64());
2584 
2585  tracker_min_blob_size.resize(2,0);
2586  if (rf.check("tracker_min_blob_size"))
2587  {
2588  if (Bottle *size=rf.find("tracker_min_blob_size").asList())
2589  {
2590  if (size->size()>=2)
2591  {
2592  tracker_min_blob_size[0]=size->get(0).asInt32();
2593  tracker_min_blob_size[1]=size->get(1).asInt32();
2594  }
2595  }
2596  }
2597 
2598  histFilterLength=std::max(1,rf.check("hist_filter_length",Value(10)).asInt32());
2599  blockEyes=rf.check("block_eyes",Value(-1.0)).asFloat64();
2600  dropPosition=rf.find("drop_position").asList();
2601 
2602  img.resize(320,240);
2603  imgRtLoc.resize(320,240);
2604  img.zero();
2605  imgRtLoc.zero();
2606 
2607  attach(rpcPort);
2608  Rand::init();
2609 
2610  busy=false;
2611  scheduleLoadMemory=false;
2612  enableInterrupt=false;
2613  trackStopGood=false;
2614  whatGood=false;
2615  skipGazeHoming=false;
2616 
2617  objectToBeKinCalibrated="";
2618 
2619  histColorsCode.push_back(cv::Scalar( 65, 47,213));
2620  histColorsCode.push_back(cv::Scalar(122, 79, 58));
2621  histColorsCode.push_back(cv::Scalar(154,208, 72));
2622  histColorsCode.push_back(cv::Scalar( 71,196,249));
2623  histColorsCode.push_back(cv::Scalar(224,176, 96));
2624  histColorsCode.push_back(cv::Scalar( 22,118,238));
2625 
2626  return true;
2627 }
2628 
2629 
2630 /**********************************************************/
2631 bool Manager::interruptModule()
2632 {
2633  imgIn.interrupt();
2634  imgOut.interrupt();
2635  imgRtLocOut.interrupt();
2636  imgTrackOut.interrupt();
2637  imgClassifier.interrupt();
2638  imgHistogram.interrupt();
2639  histObjLocPort.interrupt();
2640  recogTriggerPort.interrupt();
2641  rpcPort.interrupt();
2642  rpcHuman.interrupt();
2643  blobExtractor.interrupt();
2644  rpcClassifier.interrupt();
2645  rpcMotor.interrupt();
2646  rpcMotorGrasp.interrupt();
2647  rpcReachCalib.interrupt();
2648  rpcGet3D.interrupt();
2649  rpcMotorStop.interrupt();
2650  rxMotorStop.interrupt();
2651  pointedLoc.interrupt();
2652  speaker.interrupt();
2653  rpcMemory.interrupt();
2654 
2655  rtLocalization.stop();
2656  memoryUpdater.stop();
2657  attention.stop();
2658 
2659  return true;
2660 }
2661 
2662 
2663 /**********************************************************/
2664 bool Manager::close()
2665 {
2666  imgIn.close();
2667  imgOut.close();
2668  imgRtLocOut.close();
2669  imgTrackOut.close();
2670  imgClassifier.close();
2671  imgHistogram.close();
2672  histObjLocPort.close();
2673  recogTriggerPort.close();
2674  rpcPort.close();
2675  rpcHuman.close();
2676  blobExtractor.close();
2677  rpcClassifier.close();
2678  rpcMotor.close();
2679  rpcMotorGrasp.close();
2680  rpcReachCalib.close();
2681  rpcGet3D.close();
2682  rpcMotorStop.close();
2683  rxMotorStop.close();
2684  pointedLoc.close();
2685  speaker.close();
2686  rpcMemory.close();
2687 
2688  // dispose filters used for scores histogram
2689  for (auto it=histFiltersPool.begin(); it!=histFiltersPool.end(); it++)
2690  delete it->second;
2691 
2692  return true;
2693 }
2694 
2695 
2696 /**********************************************************/
2697 bool Manager::updateModule()
2698 {
2699  Bottle cmdHuman,valHuman,replyHuman;
2700  rpcHuman.read(cmdHuman,true);
2701 
2702  BusyGate busyGate(busy);
2703 
2704  if (isStopping())
2705  return false;
2706 
2707  attention.suspend();
2708 
2709  int rxCmd=processHumanCmd(cmdHuman,valHuman);
2710  if ((rxCmd==Vocab32::encode("attention")) && (valHuman.size()>0))
2711  if (valHuman.get(0).asString()=="stop")
2712  skipGazeHoming=true;
2713 
2714  if (!skipGazeHoming)
2715  {
2716  home("gaze");
2717 
2718  // this wait-state gives the memory
2719  // time to be updated with the 3D
2720  // location of the objects
2721  Time::delay(0.1);
2722  }
2723 
2724  skipGazeHoming=false;
2725 
2726  if (rxCmd==Vocab32::encode("home"))
2727  {
2728  reinstateMotor(false);
2729  home();
2730  replyHuman.addString("ack");
2731  rpcHuman.reply(replyHuman);
2732  }
2733  else if (rxCmd==Vocab32::encode("cata"))
2734  {
2735  calibTable();
2736  replyHuman.addString("ack");
2737  rpcHuman.reply(replyHuman);
2738  }
2739  else if ((rxCmd==Vocab32::encode("caki")) && (valHuman.size()>0))
2740  {
2741  string type=valHuman.get(0).asString();
2742  if (type=="start")
2743  {
2744  Bottle blobs;
2745  string hand=cmdHuman.get(2).toString();
2746  string activeObject=cmdHuman.get(3).toString();
2747 
2748  mutexMemoryUpdate.lock();
2749  int recogBlob=recognize(activeObject,blobs);
2750  Vector x=updateObjCartPosInMemory(activeObject,blobs,recogBlob);
2751  if (calibKinStart(activeObject,hand,x,recogBlob))
2752  {
2753  busyGate.release();
2754  return true; // avoid resuming the attention
2755  }
2756  else
2757  mutexMemoryUpdate.unlock();
2758  }
2759  else
2760  {
2761  calibKinStop();
2762  mutexMemoryUpdate.unlock();
2763  replyHuman.addString("ack");
2764  rpcHuman.reply(replyHuman);
2765  }
2766  }
2767  else if ((rxCmd==Vocab32::encode("track")) && (valHuman.size()>0))
2768  {
2769  Bottle cmdMotor,replyMotor;
2770  string type=valHuman.get(0).asString();
2771  if (type=="start")
2772  {
2773  cmdMotor.addVocab32("track");
2774  cmdMotor.addVocab32("motion");
2775  cmdMotor.addString("no_sacc");
2776  rpcMotor.write(cmdMotor,replyMotor);
2777  speaker.speak("Great! Show me the new toy");
2778  trackStopGood=false;
2779  busyGate.release();
2780  }
2781  else
2782  {
2783  cmdMotor.addVocab32("idle");
2784  rpcMotor.write(cmdMotor,replyMotor);
2785 
2786  // avoid being distracted by the human hand
2787  // while it is being removed: save the last
2788  // pointed object
2789  trackStopGood=pointedLoc.getLoc(trackStopLocation);
2790  }
2791 
2792  replyHuman.addString("ack");
2793  rpcHuman.reply(replyHuman);
2794  skipGazeHoming=true;
2795  return true; // avoid resuming the attention
2796  }
2797  else if ((rxCmd==Vocab32::encode("name")) && (valHuman.size()>0))
2798  {
2799  string activeObject=valHuman.get(0).asString();
2800  execName(activeObject);
2801  }
2802  else if ((rxCmd==Vocab32::encode("forget")) && (valHuman.size()>0))
2803  {
2804  string activeObject=valHuman.get(0).asString();
2805 
2806  mutexMemoryUpdate.lock();
2807  execForget(activeObject);
2808  mutexMemoryUpdate.unlock();
2809  }
2810  else if ((rxCmd==Vocab32::encode("where")) && (valHuman.size()>0))
2811  {
2812  Bottle blobs;
2813  Classifier *pClassifier;
2814  string activeObject=valHuman.get(0).asString();
2815 
2816  mutexMemoryUpdate.lock();
2817  string recogType=(db.find(activeObject)==db.end())?"creation":"recognition";
2818  int recogBlob=recognize(activeObject,blobs,&pClassifier);
2819  updateObjCartPosInMemory(activeObject,blobs,recogBlob);
2820  execWhere(activeObject,blobs,recogBlob,pClassifier,recogType);
2821  mutexMemoryUpdate.unlock();
2822  }
2823  else if (rxCmd==Vocab32::encode("what"))
2824  {
2825  // avoid being distracted by the human hand
2826  // while it is being removed: save the last
2827  // pointed object
2828  whatGood=pointedLoc.getLoc(whatLocation);
2829  Time::delay(1.0);
2830 
2831  Bottle blobs,scores;
2832  string activeObject;
2833  int pointedBlob=recognize(blobs,scores,activeObject);
2834  execWhat(blobs,pointedBlob,scores,activeObject);
2835  }
2836  else if ((rxCmd==Vocab32::encode("this")) && (valHuman.size()>0))
2837  {
2838  // name of the object to be learned
2839  string activeObject=valHuman.get(0).asString();
2840 
2841  // get location from a click on the viewer
2842  if (valHuman.size()>=2)
2843  {
2844  if (valHuman.get(1).asString()=="click")
2845  {
2846  whatLocation=clickLocation;
2847  whatGood=true;
2848  }
2849  else
2850  whatGood=false;
2851  }
2852  // get location via pointing action
2853  else
2854  {
2855  whatGood=pointedLoc.getLoc(whatLocation);
2856  Time::delay(1.0);
2857  }
2858 
2859  mutexMemoryUpdate.lock();
2860  Bottle blobs,scores;
2861  string detectedObject;
2862  int pointedBlob=recognize(blobs,scores,detectedObject);
2863 
2864  execThis(activeObject,detectedObject,blobs,pointedBlob);
2865  updateObjCartPosInMemory(activeObject,blobs,pointedBlob);
2866  mutexMemoryUpdate.unlock();
2867  }
2868  else if ((rxCmd==Vocab32::encode("take")) || (rxCmd==Vocab32::encode("grasp")) ||
2869  (rxCmd==Vocab32::encode("touch")) || (rxCmd==Vocab32::encode("push")) ||
2870  (rxCmd==Vocab32::encode("hold")) || (rxCmd==Vocab32::encode("drop")))
2871  {
2872  Bottle blobs;
2873  string activeObject="";
2874  int recogBlob=RET_INVALID;
2875  Vector x(3,0.0);
2876 
2877  mutexMemoryUpdate.lock();
2878  if (valHuman.size()>0)
2879  {
2880  activeObject=valHuman.get(0).asString();
2881  recogBlob=recognize(activeObject,blobs);
2882  if ((recogBlob>=0) && (rxCmd==Vocab32::encode("grasp")))
2883  {
2884  Bottle lookOptions;
2885  if (blockEyes>=0.0)
2886  {
2887  Bottle &opt=lookOptions.addList();
2888  opt.addString("block_eyes");
2889  opt.addFloat64(blockEyes);
2890  }
2891 
2892  look(blobs,recogBlob,lookOptions);
2893  Time::delay(1.0);
2894  recogBlob=recognize(activeObject,blobs);
2895  }
2896 
2897  x=updateObjCartPosInMemory(activeObject,blobs,recogBlob);
2898  }
2899 
2900  string action;
2901  if (rxCmd==Vocab32::encode("take"))
2902  action="take";
2903  else if (rxCmd==Vocab32::encode("grasp"))
2904  action="grasp";
2905  else if (rxCmd==Vocab32::encode("touch"))
2906  action="touch";
2907  else if (rxCmd==Vocab32::encode("push"))
2908  action="push";
2909  else if (rxCmd==Vocab32::encode("hold"))
2910  action="hold";
2911  else
2912  action="drop";
2913 
2914  execInterruptableAction(action,activeObject,x,blobs,recogBlob);
2915  mutexMemoryUpdate.unlock();
2916  }
2917  else if ((rxCmd==Vocab32::encode("explore")) && (valHuman.size()>0))
2918  {
2919  string activeObject=valHuman.get(0).asString();
2920  execExplore(activeObject);
2921  }
2922  else if ((rxCmd==Vocab32::encode("reinforce")) && (valHuman.size()>1))
2923  {
2924  string activeObject=valHuman.get(0).asString();
2925  if (Bottle *pl=valHuman.get(1).asList())
2926  {
2927  Vector position; pl->write(position);
2928  execReinforce(activeObject,position);
2929  }
2930  else
2931  replyHuman.addString("nack");
2932  }
2933  else if ((rxCmd==Vocab32::encode("attention")) && (valHuman.size()>0))
2934  {
2935  string type=valHuman.get(0).asString();
2936  if (type=="stop")
2937  {
2938  doAttention=false;
2939  replyHuman.addString("ack");
2940  }
2941  else if (type=="start")
2942  {
2943  doAttention=true;
2944  replyHuman.addString("ack");
2945  }
2946  else
2947  replyHuman.addString("nack");
2948 
2949  rpcHuman.reply(replyHuman);
2950  }
2951  else if ((rxCmd==Vocab32::encode("say")) && (valHuman.size()>0))
2952  {
2953  string speech=valHuman.get(0).asString();
2954  speaker.speak(speech);
2955  replyHuman.addString("ack");
2956  rpcHuman.reply(replyHuman);
2957  skipGazeHoming=true;
2958  return true; // avoid resuming the attention
2959  }
2960  else // manage an unknown request
2961  {
2962  speaker.speak("I don't understand what you want me to do");
2963  replyHuman.addString("nack");
2964  rpcHuman.reply(replyHuman);
2965  }
2966 
2967  if (doAttention)
2968  attention.resume();
2969 
2970  return true;
2971 }
2972 
2973 
2974 /**********************************************************/
2975 bool Manager::respond(const Bottle &command, Bottle &reply)
2976 {
2977  string ack="ack";
2978  string nack="nack";
2979  Value cmd=command.get(0);
2980 
2981  string ans=nack; string pl;
2982  if (cmd.isVocab32())
2983  {
2984  if (cmd.asVocab32()==Vocab32::encode("status"))
2985  {
2986  ans=ack;
2987  pl=busy?"busy":"idle";
2988  }
2989  }
2990 
2991  Bottle rep;
2992  if (ans==ack)
2993  {
2994  reply.addString(ack);
2995  reply.addString(pl);
2996  }
2997  else if (RFModule::respond(command,rep))
2998  reply=rep;
2999  else
3000  reply.addString(nack);
3001 
3002  return true;
3003 }
3004 
3005 
3006 /**********************************************************/
3007 double Manager::getPeriod()
3008 {
3009  // the updateModule goes through a
3010  // blocking read => no need for periodicity
3011  return 0.0;
3012 }
3013 
3014 
3015 
Generated by doxygen 1.9.1