iCub-main
VisuoThread.h
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Carlo Ciliberto, Vadim Tikhanoff
4 * email: carlo.ciliberto@iit.it vadim.tikhanoff@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18 
19 #ifndef __VISUO_THREAD__
20 #define __VISUO_THREAD__
21 
22 
23 
24 #include <yarp/os/PeriodicThread.h>
25 #include <yarp/os/ResourceFinder.h>
26 #include <yarp/os/BufferedPort.h>
27 
28 #include <yarp/sig/Vector.h>
29 #include <yarp/sig/Image.h>
30 
31 #include <opencv2/core/core_c.h>
32 
33 #include <mutex>
34 #include <string>
35 #include <deque>
36 #include <map>
37 
38 #include <iCub/utils.h>
39 
40 #define PARAM_FIXATION yarp::os::createVocab32('f','i','x','a')
41 #define PARAM_MOTION yarp::os::createVocab32('m','o','t','i')
42 #define PARAM_TRACK yarp::os::createVocab32('t','r','a','c')
43 #define PARAM_RAW yarp::os::createVocab32('r','a','w')
44 
45 #define MODE_TRACK_TEMPLATE 0
46 #define MODE_TRACK_MOTION 1
47 
48 using namespace std;
49 using namespace yarp::os;
50 using namespace yarp::sig;
51 
52 
54 {
55  Vector vec;
56  int side;
57 };
58 
59 
60 
61 
62 class VisuoThread: public PeriodicThread
63 {
64 private:
65  ResourceFinder &rf;
66 
67  StereoTarget &stereo_target;
69 
70  int trackMode;
71 
72  bool interrupted;
73 
74  Port outPort[2];
75  BufferedPort<ImageOf<PixelRgb> > imgPort[2];
76  BufferedPort<Bottle> mCUTPort[2];
77  BufferedPort<Bottle> rawInPort[2];
78  BufferedPort<Vector> pftInPort;
79  Port pftOutPort; //send template to the pft throught this port
80 
81 
82  Port segPort;
83 
84  //MIL Ports
85  Port boundMILPort;
86  RpcClient cmdMILPort;
87  BufferedPort<Bottle> recMILPort;
88 
89  //multiSensoryObjectRecognition Ports
90  Port cmdMSRPort;
91  BufferedPort<Bottle> recMSRPort;
92 
93  mutex imgMutex;
94  mutex motMutex;
95  mutex MILMutex;
96  mutex trackMutex;
97 
98  unsigned int minMotionBufSize;
99  unsigned int minTrackBufSize;
100  unsigned int maxTrackBufSize;
101  double timeTol;
102  double motionStdThresh;
103  double speedStdThresh;
104  double stereoDistThresh;
105 
106  double rawWaitThresh;
107  double motionWaitThresh;
108  double objectWaitThresh;
109 
110  int dominant_eye;
111  ImageOf<PixelRgb> *img[2];
112  ImageOf<PixelBgr> tpl;
113 
114  StereoTracker stereoTracker;
115  deque<Vector> trackBuffer;
116  bool tracking;
117 
118  bool newImage[2];
119 
120  bool show;
121  bool closed;
122 
123  struct Item
124  {
125  double t;
126  double size;
127  CvPoint p;
128  };
129 
130  deque<Item> buffer[2];
131  //map<string,CvPoint> locations[2];
132  map<string,CvPoint> locations;
133 
134 
135  bool getFixation(Bottle &bStereo);
136  bool getMotion(Bottle &bStereo);
137  bool getTrack(Bottle &bStereo);
138  bool getRaw(Bottle &bStereo);
139  bool getObject(const string &object_name, Bottle &bStereo);
140 
141 
142  void close();
143  void updateImages();
144  void updateLocationsMIL();
145  void updateMotionCUT();
146  void updatePFTracker();
147 public:
148  VisuoThread(ResourceFinder &_rf, Initializer *initializer);
149 
150  virtual bool threadInit();
151  virtual void run();
152  virtual void threadRelease();
153 
154 
155  bool isTracking()
156  {
157  lock_guard<mutex> lck(trackMutex);
158  return tracking;
159  }
160 
161  bool checkTracker(Vector *vec);
162 
163  void startTracker(const Vector &stereo, const int &side);
164  void restartTracker();
165 
166 
167  bool trackMotion()
168  {
169  trackMode=MODE_TRACK_MOTION;
170  return true;
171  }
172 
173  bool getTarget(Value &type, Bottle &target);
174 
175  Bottle recogMSR(string &obj_name);
176 
177 
178  void doShow()
179  {
180  show=!show;
181  }
182 
183 
184 
185  bool startLearningMIL(const string &obj_name);
186  bool suspendLearningMIL();
187  bool resumeLearningMIL();
188  bool trainMIL();
189 
190  bool startLearningMSR(const string &obj_name, const int &arm);
191  bool startRecogMSR(const int &arm);
192  bool suspendLearningMSR();
193  bool resumeLearningMSR();
194  bool stopMSR();
195  void checkDoneMSR(bool &done);
196 
197  void interrupt();
198  void reinstate();
199 };
200 
201 #endif
202 
203 
VisuoThread::doShow
void doShow()
Definition: VisuoThread.h:178
ObjectPropertiesCollectorPort
Definition: utils.h:43
MODE_TRACK_MOTION
#define MODE_TRACK_MOTION
Definition: VisuoThread.h:46
VisuoThread::isTracking
bool isTracking()
Definition: VisuoThread.h:155
StereoTracker
Definition: VisuoThread.h:53
StereoTarget
Definition: utils.h:85
Initializer
Definition: utils.h:113
p
p
Definition: show_eyes_axes.m:23
VisuoThread::trackMotion
bool trackMotion()
Definition: VisuoThread.h:167
StereoTracker::vec
Vector vec
Definition: VisuoThread.h:55
StereoTracker::side
int side
Definition: VisuoThread.h:56
show
void show(const WorldOpTriplet &x)
Definition: WorldOp.cpp:25
VisuoThread
Definition: VisuoThread.h:62
done
bool done
Definition: main.cpp:42