iCub-main
ClientGazeController.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #ifndef __CLIENTGAZECONTROLLER_H__
20 #define __CLIENTGAZECONTROLLER_H__
21 
22 #include <string>
23 #include <set>
24 #include <map>
25 
26 #include <yarp/os/all.h>
27 #include <yarp/sig/all.h>
28 #include <yarp/dev/all.h>
29 
30 
31 // forward declaration
33 
34 
35 /************************************************************************/
36 class GazeEventHandler : public yarp::os::BufferedPort<yarp::os::Bottle>
37 {
38 protected:
40  void onRead(yarp::os::Bottle &event);
41 
42 public:
45 };
46 
47 
62 class ClientGazeController : public yarp::dev::DeviceDriver,
63  public yarp::dev::IGazeControl
64 {
65 protected:
66  bool connected;
67  bool closed;
68 
69  double timeout;
72 
73  yarp::sig::Vector fixationPoint;
74  yarp::sig::Vector angles;
75 
76  yarp::os::Stamp fpStamp;
77  yarp::os::Stamp anglesStamp;
78 
79  yarp::os::BufferedPort<yarp::sig::Vector> portStateFp;
80  yarp::os::BufferedPort<yarp::sig::Vector> portStateAng;
81  yarp::os::BufferedPort<yarp::sig::Vector> portStateHead;
82 
83  yarp::os::BufferedPort<yarp::os::Bottle> portCmdFp;
84  yarp::os::BufferedPort<yarp::os::Bottle> portCmdAng;
85  yarp::os::BufferedPort<yarp::os::Bottle> portCmdMono;
86  yarp::os::BufferedPort<yarp::os::Bottle> portCmdStereo;
87  yarp::os::RpcClient portRpc;
88 
89  std::set<int> contextIdList;
90  std::map<std::string,yarp::dev::GazeEvent*> eventsMap;
92  friend class GazeEventHandler;
93 
94  void init();
95  bool deleteContexts();
96  bool getPose(const std::string &poseSel, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
97  bool blockNeckJoint(const std::string &joint, const double min, const double max);
98  bool blockNeckJoint(const std::string &joint, const int j);
99  bool getNeckJointRange(const std::string &joint, double *min, double *max);
100  bool clearJoint(const std::string &joint);
101  void eventHandling(yarp::os::Bottle &event);
102  bool getInfoHelper(yarp::os::Bottle &info);
103 
104 public:
106  ClientGazeController(yarp::os::Searchable &config);
107 
108  bool open(yarp::os::Searchable &config);
109  bool close();
110 
111  bool setTrackingMode(const bool f);
112  bool getTrackingMode(bool *f);
113  bool setStabilizationMode(const bool f);
114  bool getStabilizationMode(bool *f);
115  bool getFixationPoint(yarp::sig::Vector &fp, yarp::os::Stamp *stamp=NULL);
116  bool getAngles(yarp::sig::Vector &ang, yarp::os::Stamp *stamp=NULL);
117  bool lookAtFixationPoint(const yarp::sig::Vector &fp);
118  bool lookAtAbsAngles(const yarp::sig::Vector &ang);
119  bool lookAtRelAngles(const yarp::sig::Vector &ang);
120  bool lookAtMonoPixel(const int camSel, const yarp::sig::Vector &px, const double z=1.0);
121  bool lookAtMonoPixelWithVergence(const int camSel, const yarp::sig::Vector &px, const double ver);
122  bool lookAtStereoPixels(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr);
123  bool lookAtFixationPointSync(const yarp::sig::Vector &fp);
124  bool lookAtAbsAnglesSync(const yarp::sig::Vector &ang);
125  bool lookAtRelAnglesSync(const yarp::sig::Vector &ang);
126  bool lookAtMonoPixelSync(const int camSel, const yarp::sig::Vector &px, const double z=1.0);
127  bool lookAtMonoPixelWithVergenceSync(const int camSel, const yarp::sig::Vector &px, const double ver);
128  bool lookAtStereoPixelsSync(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr);
129  bool getNeckTrajTime(double *t);
130  bool getEyesTrajTime(double *t);
131  bool getVORGain(double *gain);
132  bool getOCRGain(double *gain);
133  bool getSaccadesMode(bool *f);
134  bool getSaccadesInhibitionPeriod(double *period);
135  bool getSaccadesActivationAngle(double *angle);
136  bool getLeftEyePose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
137  bool getRightEyePose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
138  bool getHeadPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
139  bool get2DPixel(const int camSel, const yarp::sig::Vector &x, yarp::sig::Vector &px);
140  bool get3DPoint(const int camSel, const yarp::sig::Vector &px, const double z, yarp::sig::Vector &x);
141  bool get3DPointOnPlane(const int camSel, const yarp::sig::Vector &px, const yarp::sig::Vector &plane, yarp::sig::Vector &x);
142  bool get3DPointFromAngles(const int mode, const yarp::sig::Vector &ang, yarp::sig::Vector &x);
143  bool getAnglesFrom3DPoint(const yarp::sig::Vector &x, yarp::sig::Vector &ang);
144  bool triangulate3DPoint(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr, yarp::sig::Vector &x);
145  bool getJointsDesired(yarp::sig::Vector &qdes);
146  bool getJointsVelocities(yarp::sig::Vector &qdot);
147  bool getStereoOptions(yarp::os::Bottle &options);
148  bool setNeckTrajTime(const double t);
149  bool setEyesTrajTime(const double t);
150  bool setVORGain(const double gain);
151  bool setOCRGain(const double gain);
152  bool setSaccadesMode(const bool f);
153  bool setSaccadesInhibitionPeriod(const double period);
154  bool setSaccadesActivationAngle(const double angle);
155  bool setStereoOptions(const yarp::os::Bottle &options);
156  bool bindNeckPitch(const double min, const double max);
157  bool blockNeckPitch(const double val);
158  bool blockNeckPitch();
159  bool bindNeckRoll(const double min, const double max);
160  bool blockNeckRoll(const double val);
161  bool blockNeckRoll();
162  bool bindNeckYaw(const double min, const double max);
163  bool blockNeckYaw(const double val);
164  bool blockNeckYaw();
165  bool blockEyes(const double ver);
166  bool blockEyes();
167  bool getNeckPitchRange(double *min, double *max);
168  bool getNeckRollRange(double *min, double *max);
169  bool getNeckYawRange(double *min, double *max);
170  bool getBlockedVergence(double *ver);
171  bool clearNeckPitch();
172  bool clearNeckRoll();
173  bool clearNeckYaw();
174  bool clearEyes();
175  bool getNeckAngleUserTolerance(double *angle);
176  bool setNeckAngleUserTolerance(const double angle);
177  bool checkMotionDone(bool *f);
178  bool waitMotionDone(const double period=0.1, const double timeout=0.0);
179  bool checkSaccadeDone(bool *f);
180  bool waitSaccadeDone(const double period=0.1, const double timeout=0.0);
181  bool stopControl();
182  bool storeContext(int *id);
183  bool restoreContext(const int id);
184  bool deleteContext(const int id);
185  bool getInfo(yarp::os::Bottle &info);
186  bool registerEvent(yarp::dev::GazeEvent &event);
187  bool unregisterEvent(yarp::dev::GazeEvent &event);
188  bool tweakSet(const yarp::os::Bottle &options);
189  bool tweakGet(yarp::os::Bottle &options);
190 
191  virtual ~ClientGazeController();
192 };
193 
194 
195 #endif
196 
197 
clientgazecontroller : implements the client part of the the Gaze Control Interface.
bool getPose(const std::string &poseSel, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool getFixationPoint(yarp::sig::Vector &fp, yarp::os::Stamp *stamp=NULL)
bool lookAtFixationPointSync(const yarp::sig::Vector &fp)
bool lookAtMonoPixel(const int camSel, const yarp::sig::Vector &px, const double z=1.0)
bool waitSaccadeDone(const double period=0.1, const double timeout=0.0)
bool lookAtMonoPixelWithVergenceSync(const int camSel, const yarp::sig::Vector &px, const double ver)
bool setTrackingMode(const bool f)
bool get3DPointOnPlane(const int camSel, const yarp::sig::Vector &px, const yarp::sig::Vector &plane, yarp::sig::Vector &x)
bool triangulate3DPoint(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr, yarp::sig::Vector &x)
bool bindNeckPitch(const double min, const double max)
bool setStabilizationMode(const bool f)
bool setVORGain(const double gain)
yarp::os::BufferedPort< yarp::sig::Vector > portStateHead
bool getJointsVelocities(yarp::sig::Vector &qdot)
bool getNeckPitchRange(double *min, double *max)
bool getInfo(yarp::os::Bottle &info)
bool getNeckJointRange(const std::string &joint, double *min, double *max)
bool lookAtMonoPixelWithVergence(const int camSel, const yarp::sig::Vector &px, const double ver)
yarp::os::BufferedPort< yarp::os::Bottle > portCmdAng
bool setOCRGain(const double gain)
bool unregisterEvent(yarp::dev::GazeEvent &event)
std::map< std::string, yarp::dev::GazeEvent * > eventsMap
bool get3DPoint(const int camSel, const yarp::sig::Vector &px, const double z, yarp::sig::Vector &x)
bool tweakGet(yarp::os::Bottle &options)
yarp::os::BufferedPort< yarp::sig::Vector > portStateFp
yarp::os::BufferedPort< yarp::os::Bottle > portCmdFp
bool getSaccadesActivationAngle(double *angle)
bool getOCRGain(double *gain)
bool lookAtStereoPixelsSync(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr)
bool lookAtRelAngles(const yarp::sig::Vector &ang)
yarp::os::Stamp anglesStamp
bool getLeftEyePose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
ClientGazeController(yarp::os::Searchable &config)
bool getVORGain(double *gain)
bool getNeckAngleUserTolerance(double *angle)
yarp::os::BufferedPort< yarp::os::Bottle > portCmdMono
bool getBlockedVergence(double *ver)
bool getRightEyePose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool registerEvent(yarp::dev::GazeEvent &event)
bool setNeckAngleUserTolerance(const double angle)
bool getSaccadesInhibitionPeriod(double *period)
bool setSaccadesActivationAngle(const double angle)
bool bindNeckYaw(const double min, const double max)
yarp::os::RpcClient portRpc
bool setEyesTrajTime(const double t)
bool lookAtStereoPixels(const yarp::sig::Vector &pxl, const yarp::sig::Vector &pxr)
bool getInfoHelper(yarp::os::Bottle &info)
bool setStereoOptions(const yarp::os::Bottle &options)
bool bindNeckRoll(const double min, const double max)
bool setSaccadesMode(const bool f)
bool blockNeckJoint(const std::string &joint, const double min, const double max)
bool setSaccadesInhibitionPeriod(const double period)
yarp::sig::Vector fixationPoint
bool getAngles(yarp::sig::Vector &ang, yarp::os::Stamp *stamp=NULL)
yarp::os::BufferedPort< yarp::sig::Vector > portStateAng
bool tweakSet(const yarp::os::Bottle &options)
bool getAnglesFrom3DPoint(const yarp::sig::Vector &x, yarp::sig::Vector &ang)
bool getJointsDesired(yarp::sig::Vector &qdes)
bool clearJoint(const std::string &joint)
yarp::sig::Vector angles
bool get2DPixel(const int camSel, const yarp::sig::Vector &x, yarp::sig::Vector &px)
bool lookAtMonoPixelSync(const int camSel, const yarp::sig::Vector &px, const double z=1.0)
bool lookAtRelAnglesSync(const yarp::sig::Vector &ang)
bool getStereoOptions(yarp::os::Bottle &options)
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
bool getHeadPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool setNeckTrajTime(const double t)
bool lookAtFixationPoint(const yarp::sig::Vector &fp)
bool getNeckYawRange(double *min, double *max)
yarp::os::BufferedPort< yarp::os::Bottle > portCmdStereo
std::set< int > contextIdList
bool lookAtAbsAngles(const yarp::sig::Vector &ang)
bool deleteContext(const int id)
bool getNeckRollRange(double *min, double *max)
GazeEventHandler portEvents
bool lookAtAbsAnglesSync(const yarp::sig::Vector &ang)
bool restoreContext(const int id)
bool get3DPointFromAngles(const int mode, const yarp::sig::Vector &ang, yarp::sig::Vector &x)
bool open(yarp::os::Searchable &config)
void eventHandling(yarp::os::Bottle &event)
void setInterface(ClientGazeController *interface)
void onRead(yarp::os::Bottle &event)
ClientGazeController * interface
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
static struct bpf_program fp