iCub-main
Loading...
Searching...
No Matches
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
29bool 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
87bool 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
139bool 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
193bool 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
245bool ObjectPropertiesCollectorPort::getTableHeight(double &table_height)
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
285bool 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
342bool 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};
362bool 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::getStereoPosition
bool getStereoPosition(const string &obj_name, Vector &stereo)
Definition utils.cpp:29
ObjectPropertiesCollectorPort::getKinematicOffsets
bool getKinematicOffsets(const string &obj_name, Vector *kinematic_offset)
Definition utils.cpp:139
ObjectPropertiesCollectorPort::getTableHeight
bool getTableHeight(double &table_height)
Definition utils.cpp:245
ObjectPropertiesCollectorPort::setTableHeight
bool setTableHeight(const double table_height)
Definition utils.cpp:285
ObjectPropertiesCollectorPort::getCartesianPosition
bool getCartesianPosition(const string &obj_name, Vector &x)
Definition utils.cpp:87
ObjectPropertiesCollectorPort::getAction
bool getAction(const string &act_name, Bottle *trajectory)
Definition utils.cpp:362
ObjectPropertiesCollectorPort::setKinematicOffsets
bool setKinematicOffsets(const string &obj_name, const Vector *kinematic_offset)
Definition utils.cpp:193
ObjectPropertiesCollectorPort::setAction
bool setAction(const string &act_name, const Bottle *trajectory)
Definition utils.cpp:342
x
x
Definition compute_ekf_sym.m:21
LEFT
#define LEFT
Definition utils.h:33
RIGHT
#define RIGHT
Definition utils.h:34
Generated on Wed Dec 18 2024 16:34:48 for iCub-main by doxygen 1.9.8