iCub-main
utils.cpp
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 
20 #include <iCub/utils.h>
21 
22 
23 #include <iostream>
24 #include <stdio.h>
25 
26 //--------------- Object Properties Port ------------------//
27 
28 
29 bool ObjectPropertiesCollectorPort::getStereoPosition(const string &obj_name, Vector &stereo)
30 {
31  //if the object property collector port is connected use it to obtain the object 2D position
32  if(this->getOutputCount()==0)
33  return false;
34 
35  //ask for the object's id
36  Bottle bAsk,bGet,bReply;
37  bAsk.addVocab32("ask");
38  Bottle &bTempAsk=bAsk.addList().addList();
39  bTempAsk.addString("name");
40  bTempAsk.addString("==");
41  bTempAsk.addString(obj_name);
42 
43  this->write(bAsk,bReply);
44 
45  if(bReply.size()==0 ||
46  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
47  bReply.get(1).asList()->check("id")==false ||
48  bReply.get(1).asList()->find("id").asList()->size()==0)
49  return false;
50 
51  bGet.addVocab32("get");
52  Bottle &bTempGet=bGet.addList().addList();
53  bTempGet.addString("id");
54  bTempGet.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
55 
56  this->write(bGet,bReply);
57 
58  if(bReply.size()==0 || bReply.get(0).asVocab32()!=Vocab32::encode("ack"))
59  return false;
60 
61  if(!bReply.get(1).asList()->check("position_2d_left") && !bReply.get(1).asList()->check("position_2d_right"))
62  return false;
63 
64  stereo.resize(4);
65  stereo=0.0;
66 
67  if(bReply.get(1).asList()->check("position_2d_left"))
68  {
69  Bottle *bStereo=bReply.get(1).asList()->find("position_2d_left").asList();
70 
71  stereo[0]=0.5*(bStereo->get(0).asFloat64()+bStereo->get(2).asFloat64());
72  stereo[1]=0.5*(bStereo->get(1).asFloat64()+bStereo->get(3).asFloat64());
73  }
74 
75  if(bReply.get(1).asList()->check("position_2d_right"))
76  {
77  Bottle *bStereo=bReply.get(1).asList()->find("position_2d_right").asList();
78 
79  stereo[2]=0.5*(bStereo->get(0).asFloat64()+bStereo->get(2).asFloat64());
80  stereo[3]=0.5*(bStereo->get(1).asFloat64()+bStereo->get(3).asFloat64());
81  }
82 
83  return true;
84 }
85 
86 
87 bool ObjectPropertiesCollectorPort::getCartesianPosition(const string &obj_name, Vector &x)
88 {
89  //if the object property collector port is connected use it to obtain the object 2D position
90  if(this->getOutputCount()==0)
91  return false;
92 
93  //ask for the object's id
94  Bottle bAsk,bGet,bReply;
95  bAsk.addVocab32("ask");
96  Bottle &bTempAsk=bAsk.addList().addList();
97  bTempAsk.addString("name");
98  bTempAsk.addString("==");
99  bTempAsk.addString(obj_name);
100 
101  this->write(bAsk,bReply);
102 
103  if(bReply.size()==0 ||
104  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
105  bReply.get(1).asList()->check("id")==false ||
106  bReply.get(1).asList()->find("id").asList()->size()==0)
107  return false;
108 
109  bGet.addVocab32("get");
110  Bottle &bTempGet=bGet.addList().addList();
111  bTempGet.addString("id");
112  bTempGet.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
113 
114  this->write(bGet,bReply);
115 
116  if(bReply.size()==0 || bReply.get(0).asVocab32()!=Vocab32::encode("ack"))
117  return false;
118 
119 
120  if(!bReply.get(1).asList()->check("position_3d"))
121  return false;
122 
123  x.resize(3);
124 
125  if(bReply.get(1).asList()->check("position_3d"))
126  {
127  Bottle *bX=bReply.get(1).asList()->find("position_3d").asList();
128 
129  for(int i=0; i<bX->size(); i++)
130  x[i]=bX->get(i).asFloat64();
131  }
132 
133 
134  return true;
135 }
136 
137 
138 
139 bool ObjectPropertiesCollectorPort::getKinematicOffsets(const string &obj_name, Vector *kinematic_offset)
140 {
141  //if the object property collector port is connected use it to obtain the object 2D position
142  if(this->getOutputCount()==0)
143  return false;
144 
145  //ask for the object's id
146  Bottle bAsk,bGet,bReply;
147  bAsk.addVocab32("ask");
148  Bottle &bTempAsk=bAsk.addList().addList();
149  bTempAsk.addString("name");
150  bTempAsk.addString("==");
151  bTempAsk.addString(obj_name);
152 
153  this->write(bAsk,bReply);
154 
155  if(bReply.size()==0 ||
156  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
157  bReply.get(1).asList()->check("id")==false ||
158  bReply.get(1).asList()->find("id").asList()->size()==0)
159  return false;
160 
161  bGet.addVocab32("get");
162  Bottle &bTempGet=bGet.addList().addList();
163  bTempGet.addString("id");
164  bTempGet.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
165 
166  this->write(bGet,bReply);
167 
168 
169  if(bReply.size()==0 || bReply.get(0).asVocab32()!=Vocab32::encode("ack"))
170  return false;
171 
172 
173  if(bReply.get(1).asList()->check("kinematic_offset_left"))
174  {
175  kinematic_offset[LEFT].resize(3);
176  Bottle *bCartesianOffset=bReply.get(1).asList()->find("kinematic_offset_left").asList();
177  for(int i=0; i<bCartesianOffset->size(); i++)
178  kinematic_offset[LEFT][i]=bCartesianOffset->get(i).asFloat64();
179  }
180 
181  if(bReply.get(1).asList()->check("kinematic_offset_right"))
182  {
183  kinematic_offset[RIGHT].resize(3);
184  Bottle *bCartesianOffset=bReply.get(1).asList()->find("kinematic_offset_right").asList();
185  for(int i=0; i<bCartesianOffset->size(); i++)
186  kinematic_offset[RIGHT][i]=bCartesianOffset->get(i).asFloat64();
187  }
188 
189  return true;
190 }
191 
192 
193 bool ObjectPropertiesCollectorPort::setKinematicOffsets(const string &obj_name, const Vector *kinematic_offset)
194 {
195  //if the object property collector port is connected use it to obtain the object 2D position
196  if(this->getOutputCount()==0)
197  return false;
198 
199  //ask for the object's id
200  Bottle bAsk,bSet,bReply;
201  bAsk.addVocab32("ask");
202  Bottle &bTempAsk=bAsk.addList().addList();
203  bTempAsk.addString("name");
204  bTempAsk.addString("==");
205  bTempAsk.addString(obj_name);
206 
207  this->write(bAsk,bReply);
208 
209  if(bReply.size()==0 ||
210  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
211  bReply.get(1).asList()->check("id")==false ||
212  bReply.get(1).asList()->find("id").asList()->size()==0)
213  return false;
214 
215  bSet.addVocab32("set");
216  Bottle &bTempSet=bSet.addList();
217 
218  Bottle &bTempSetId=bTempSet.addList();
219  bTempSetId.addString("id");
220  bTempSetId.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
221 
222  //Kinematic offset left
223  Bottle &bTempSetKinematicOffsetLeft=bTempSet.addList();
224  bTempSetKinematicOffsetLeft.addString("kinematic_offset_left");
225  Bottle &bTempSetVectorLeft=bTempSetKinematicOffsetLeft.addList();
226  for(size_t i=0; i<kinematic_offset[LEFT].size(); i++)
227  bTempSetVectorLeft.addFloat64(kinematic_offset[LEFT][i]);
228 
229  //Kinematic offset right
230  Bottle &bTempSetKinematicOffsetRight=bTempSet.addList();
231  bTempSetKinematicOffsetRight.addString("kinematic_offset_right");
232  Bottle &bTempSetVectorRight=bTempSetKinematicOffsetRight.addList();
233  for(size_t i=0; i<kinematic_offset[RIGHT].size(); i++)
234  bTempSetVectorRight.addFloat64(kinematic_offset[RIGHT][i]);
235 
236  this->write(bSet,bReply);
237 
238  return bReply.get(0).asVocab32()==Vocab32::encode("ack");
239 }
240 
241 
242 
243 
244 
246 {
247  //if the object property collector port is connected use it to obtain the object 2D position
248  if(this->getOutputCount()==0)
249  return false;
250 
251  //ask for the object's id
252  Bottle bAsk,bGet,bReply;
253  bAsk.addVocab32("ask");
254  Bottle &bTempAsk=bAsk.addList().addList();
255  bTempAsk.addString("entity");
256  bTempAsk.addString("==");
257  bTempAsk.addString("table");
258 
259  this->write(bAsk,bReply);
260 
261  if(bReply.size()==0 ||
262  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
263  bReply.get(1).asList()->check("id")==false ||
264  bReply.get(1).asList()->find("id").asList()->size()==0)
265  return false;
266 
267  bGet.addVocab32("get");
268  Bottle &bTempGet=bGet.addList().addList();
269  bTempGet.addString("id");
270  bTempGet.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
271 
272  this->write(bGet,bReply);
273 
274  if(bReply.size()==0 || bReply.get(0).asVocab32()!=Vocab32::encode("ack"))
275  return false;
276 
277  if(!bReply.get(1).asList()->check("height"))
278  return false;
279 
280  table_height=bReply.get(1).asList()->find("height").asFloat64();
281  return true;
282 }
283 
284 
285 bool ObjectPropertiesCollectorPort::setTableHeight(const double table_height)
286 {
287  //if the object property collector port is connected use it to obtain the object 2D position
288  if(this->getOutputCount()==0)
289  return false;
290 
291  //ask for the object's id
292  Bottle bAsk,bReply;
293  bAsk.addVocab32("ask");
294  Bottle &bTempAsk=bAsk.addList().addList();
295  bTempAsk.addString("entity");
296  bTempAsk.addString("==");
297  bTempAsk.addString("table");
298 
299  this->write(bAsk,bReply);
300 
301  if(bReply.size()==0 ||
302  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
303  bReply.get(1).asList()->check("id")==false)
304  return false;
305 
306  //if the table entity has not been created yet
307  if(bReply.get(1).asList()->find("id").asList()->size()==0)
308  {
309  Bottle bAdd;
310  bAdd.addVocab32("add");
311  Bottle &bTempAdd=bAdd.addList();
312 
313  Bottle &bEntity=bTempAdd.addList();
314  bEntity.addString("entity"); bEntity.addString("table");
315 
316  Bottle &bHeight=bTempAdd.addList();
317  bHeight.addString("height"); bHeight.addFloat64(table_height);
318 
319  this->write(bAdd,bReply);
320  }
321  else
322  {
323  Bottle bSet;
324  bSet.addVocab32("set");
325  Bottle &bTempSet=bSet.addList();
326 
327  Bottle &bTempSetId=bTempSet.addList();
328  bTempSetId.addString("id");
329  bTempSetId.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
330 
331  Bottle &bTableHeight=bTempSet.addList();
332  bTableHeight.addString("height");
333  bTableHeight.addFloat64(table_height);
334 
335  this->write(bSet,bReply);
336  }
337 
338  return bReply.get(0).asVocab32()==Vocab32::encode("ack");
339 }
340 
341 
342 bool ObjectPropertiesCollectorPort::setAction(const string &act_name, const Bottle *trajectory)
343 {
344  if(this->getOutputCount()==0)
345  return false;
346  // rpc add: should ask to see if the same action already exists?
347  Bottle bAdd, bReply;
348  bAdd.addVocab32("add");
349  Bottle &bTempAdd=bAdd.addList();
350 
351  Bottle &bEntity=bTempAdd.addList();
352  bEntity.addString("entity"); bEntity.addString("action");
353 
354  Bottle &bName=bTempAdd.addList();
355  bName.addString("name"); bName.addString(act_name);
356  Bottle &bTraj= bTempAdd.addList();
357  bTraj.addString("trajectory"); bTraj.addList()=*trajectory;
358 
359  this->write(bAdd,bReply);
360  return bReply.get(0).asVocab32()==Vocab32::encode("ack");
361 };
362 bool ObjectPropertiesCollectorPort::getAction(const string &act_name, Bottle *trajectory)
363 {
364  if(this->getOutputCount()==0)
365  return false;
366  //ask for the object's id
367  Bottle bAsk,bGet,bReply;
368  bAsk.addVocab32("ask");
369  Bottle &bTempAsk=bAsk.addList().addList();
370  bTempAsk.addString("name");
371  bTempAsk.addString("==");
372  bTempAsk.addString(act_name);
373 
374  this->write(bAsk,bReply);
375 
376  if(bReply.size()==0 ||
377  bReply.get(0).asVocab32()!=Vocab32::encode("ack") ||
378  bReply.get(1).asList()->check("id")==false ||
379  bReply.get(1).asList()->find("id").asList()->size()==0)
380  return false;
381 
382  bGet.addVocab32("get");
383  Bottle &bTempGet=bGet.addList().addList();
384  bTempGet.addString("id");
385  bTempGet.addInt32(bReply.get(1).asList()->find("id").asList()->get(0).asInt32());
386 
387  this->write(bGet,bReply);
388 
389  if(bReply.size()==0 || bReply.get(0).asVocab32()!=Vocab32::encode("ack"))
390  return false;
391 
392  if(!bReply.get(1).asList()->check("trajectory"))
393  return false;
394 
395  *trajectory = *(bReply.get(1).asList()->find("trajectory").asList());
396 
397  return true;
398 
399 };
400 
401 
402 
ObjectPropertiesCollectorPort::getCartesianPosition
bool getCartesianPosition(const string &obj_name, Vector &x)
Definition: utils.cpp:87
strain::regulation::write
bool write(const std::string filename, const FullRegulation &reg)
ObjectPropertiesCollectorPort::setTableHeight
bool setTableHeight(const double table_height)
Definition: utils.cpp:285
x
x
Definition: compute_ekf_sym.m:21
ObjectPropertiesCollectorPort::getKinematicOffsets
bool getKinematicOffsets(const string &obj_name, Vector *kinematic_offset)
Definition: utils.cpp:139
LEFT
#define LEFT
Definition: utils.h:33
ObjectPropertiesCollectorPort::setKinematicOffsets
bool setKinematicOffsets(const string &obj_name, const Vector *kinematic_offset)
Definition: utils.cpp:193
ObjectPropertiesCollectorPort::getAction
bool getAction(const string &act_name, Bottle *trajectory)
Definition: utils.cpp:362
RIGHT
#define RIGHT
Definition: utils.h:34
ObjectPropertiesCollectorPort::getStereoPosition
bool getStereoPosition(const string &obj_name, Vector &stereo)
Definition: utils.cpp:29
ObjectPropertiesCollectorPort::setAction
bool setAction(const string &act_name, const Bottle *trajectory)
Definition: utils.cpp:342
ObjectPropertiesCollectorPort::getTableHeight
bool getTableHeight(double &table_height)
Definition: utils.cpp:245