iCub-main
WorldManager.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Vadim Tikhanoff, Paul Fitzpatrick
6 * email: vadim.tikhanoff@iit.it, paulfitz@alum.mit.edu
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
20 
21 #include "WorldManager.h"
22 
23 #include <cstdio>
24 #include <cstring>
25 #include <string>
26 
27 #include <yarp/os/Value.h>
28 
29 class ManagerState {
30 public:
31  bool failed;
33  const yarp::os::Bottle& command;
36  int offset;
37  bool needIndex;
39 
40  ManagerState(const yarp::os::Bottle &command,
41  WorldOp& op,
44  command(command),
45  op(op),
46  result(result),
48  {
49  offset = 2;
50  failed = false;
51  needIndex = false;
52  }
53 
54  const yarp::os::Value& get(int offset) {
55  return command.get(offset);
56  }
57 
58  bool more() {
59  return command.size()>offset;
60  }
61 
62  void fail(const char *msg) {
63  failed = true;
64  why = std::string("could not set ") + msg;
65  result.setFail(why.c_str());
66  }
67 
68  bool consume(WorldOpTriplet& x, const char *msg) {
69  if (failed) return false;
70  bool ok0 = (get(offset).isFloat64()||get(offset).isInt32());
71  bool ok1 = (get(offset+1).isFloat64()||get(offset+1).isInt32());
72  bool ok2 = (get(offset+2).isFloat64()||get(offset+2).isInt32());
73  x.valid = false;
74  if (!(ok0&&ok1&&ok2)) {
75  fail(msg);
76  return false;
77  }
78  x.x[0] = get(offset).asFloat64();
79  x.x[1] = get(offset+1).asFloat64();
80  x.x[2] = get(offset+2).asFloat64();
81  x.valid = true;
82  offset += 3;
83  return true;
84  }
85 
86  bool consume(WorldOpIndex& x, const char *msg) {
87  if (failed) return false;
88  bool ok = (get(offset).isInt32());
89  x.valid = false;
90  if (!ok) {
91  fail(msg);
92  return false;
93  }
94  x.index = get(offset).asInt32();
95  x.valid = true;
96  offset++;
97  return true;
98  }
99 
100  bool consume(WorldOpName& x, const char *msg) {
101  if (failed) return false;
102  bool ok = get(offset).isString();
103  x.valid = false;
104  if (!ok) {
105  fail(msg);
106  return false;
107  }
108  x.name = get(offset).asString();
109  x.valid = true;
110  offset++;
111  return true;
112  }
113 
114  bool consume(WorldOpScalar& x, const char *msg) {
115  if (failed) return false;
116  bool ok = (get(offset).isFloat64()||get(offset).isInt32());
117  x.valid = false;
118  if (!ok) {
119  fail(msg);
120  return false;
121  }
122  x.val = get(offset).asFloat64();
123  x.valid = true;
124  offset++;
125  return true;
126  }
127 
128  bool consume(WorldOpFlag& x, const char *msg) {
129  if (failed) return false;
130  bool ok;
131  int getBool = 0;
132  if ( get(offset).isNull() && strcmp ( msg, "collide" ) == 0 )
133  {
134  ok = true;
135  getBool = 1;
136  }
137  else if (get(offset).isString())
138  {
139  if ( get(offset).asString() == "true" || get(offset).asString() == "TRUE" )
140  {
141  ok = true;
142  getBool = 1;
143  }
144  else if ( get(offset).asString() == "false" || get(offset).asString() == "FALSE" )
145  {
146  ok = true;
147  getBool = 0;
148  }
149  else
150  ok = false;
151  }
152  else
153  ok = (get(offset).isInt32());
154 
155  x.valid = false;
156  if (!ok) {
157  fail(msg);
158  return false;
159  }
160  if (get(offset).isString() || get(offset).isNull() && strcmp ( msg, "collide" ) == 0)
161  {
162  x.setting = getBool?true:false;
163  }
164  else
165  x.setting = get(offset).asInt32()?true:false;
166 
167  x.valid = true;
168  offset++;
169  return true;
170  }
171 
172  void debug() {
173  //yDebug("Command: %s\n", command.toString().c_str());
174  //op.show();
175  }
176 };
177 
179  state.op.kind = WorldOpName(state.command.get(2).asString());
180  int kind = state.command.get(2).asVocab32();
181  state.offset++;
182  bool static_obj = false;
183  //bool mustCollide = state.command.get(2).asVocab32()
184  switch (kind) {
185  case yarp::os::createVocab32('s','b','o','x'):
186  static_obj = true;
187  case yarp::os::createVocab32('b','o','x'):
188  state.op.kind = "box";
189  state.op.dynamic = WorldOpFlag(!static_obj);
190  state.needIndex = true;
191  break;
192  case yarp::os::createVocab32('s','c','y','l'):
193  static_obj = true;
194  case yarp::os::createVocab32('c','y','l'):
195  state.op.kind = "cyl";
196  state.op.dynamic = WorldOpFlag(!static_obj);
197  state.needIndex = true;
198  break;
199  case yarp::os::createVocab32('s','s','p','h'):
200  static_obj = true;
201  case yarp::os::createVocab32('s','p','h'):
202  state.op.kind = "sph";
203  state.op.dynamic = WorldOpFlag(!static_obj);
204  state.needIndex = true;
205  break;
206  case yarp::os::createVocab32('s','m','o','d'):
207  static_obj = true;
208  case yarp::os::createVocab32('m','o','d','e'):
209  state.op.kind = "model";
210  state.op.dynamic = WorldOpFlag(!static_obj);
211  state.needIndex = true;
212  break;
213  case yarp::os::createVocab32('l','h','a','n'):
214  state.op.kind = WorldOpName("hand");
215  //state.op.name = WorldOpName("icub_left_hand");
216  state.op.index = WorldOpIndex(1);
217  state.op.dynamic = WorldOpFlag(true);
218  state.op.rightHanded = WorldOpFlag(false);
219  break;
220  case yarp::os::createVocab32('r','h','a','n'):
221  state.op.kind = WorldOpName("hand");
222  //state.op.name = WorldOpName("icub_right_hand");
223  state.op.index = WorldOpIndex(2);
224  state.op.dynamic = WorldOpFlag(true);
225  state.op.rightHanded = WorldOpFlag(true);
226  break;
227  case yarp::os::createVocab32('m','d','i','r'):
228  state.op.parameter = WorldOpFlag(true);
229  break;
230  case yarp::os::createVocab32('t','a','b','l'):
231  case yarp::os::createVocab32('c','u','b','e'):
232  case yarp::os::createVocab32('b','a','l','l'):
233  case yarp::os::createVocab32('s','c','r','e'):
234  case yarp::os::createVocab32('a','l','l'):
235  break;
236  default:
237  state.failed = true;
238  state.why = "unrecognized object type";
239  break;
240  }
241 }
242 
244  std::string grabber = state.get(state.offset).asString();
245  state.offset++;
246  if (grabber=="left") {
247  state.op.rightHanded = WorldOpFlag(false);
248  } else if (grabber=="right") {
249  state.op.rightHanded = WorldOpFlag(true);
250  } else {
251  state.failed = "true";
252  state.why = "hand not recognized";
253  }
254 }
255 
258  if (state.needIndex) {
259  if (!state.op.index.valid) {
260  state.consume(state.op.index,"index");
261  }
262  }
263 }
264 
267  if (!state.failed) {
268  state.manager.apply(state.op,state.result);
269  }
270  return !state.failed;
271 }
272 
275  // if color supplied, set, otherwise, get.
276  if (state.more()) {
277  state.consume(state.op.color,"color");
278  }
279  if (!state.failed) {
280  state.manager.apply(state.op,state.result);
281  }
282  return !state.failed;
283  }
284 
287  if (state.op.parameter.get()) {
288  if (state.op.kind.get() == "mdir") {
289  state.consume(state.op.modelName,"model path");
290  } else {
291  state.result.setFail("parameter not recognized");
292  return false;
293  }
294  } else {
295  state.consume(state.op.location,"location");
296  }
297  if (!state.failed) {
298  state.manager.apply(state.op,state.result);
299  }
300  return !state.failed;
301 }
302 
305  std::string name = state.op.kind.name;
306  if (!(name=="box"||name=="cyl"||name=="sph"||name=="model")) {
307  state.result.setFail("cannot create object of requested type");
308  return false;
309  }
310  if (name == "box") {
311  state.consume(state.op.size,"size");
312  }
313  if (name == "cyl" || name == "sph") {
314  state.consume(state.op.radius,"radius");
315  }
316  if (name == "cyl") {
317  state.consume(state.op.length,"length");
318  }
319  if (name == "model") {
320  state.consume(state.op.modelName,"model name");
321  state.consume(state.op.modelTexture,"model texture");
322  }
323  state.consume(state.op.location,"location");
324 
325  if (name != "model") {
326  state.consume(state.op.color,"color");
327  }
328 
329  state.consume(state.op.collide,"collide");
330 
331  if (!state.failed) {
332  state.manager.apply(state.op,state.result);
333  }
334  return !state.failed;
335 }
336 
340  state.consume(state.op.active,"active");
341  if (!state.failed) {
342  state.manager.apply(state.op,state.result);
343  }
344  return !state.failed;
345 }
346 
349  if (state.more()) {
350  state.consume(state.op.rotation,"rotation");
351  }
352  if (!state.failed) {
353  state.manager.apply(state.op,state.result);
354  }
355  return !state.failed;
356 }
357 
360  if (!state.failed) {
361  state.manager.apply(state.op,state.result);
362  }
363  return !state.failed;
364 }
365 
368  if (!state.failed) {
369  state.manager.apply(state.op,state.result);
370  }
371  return !state.failed;
372 }
373 
374 bool WorldManager::respond(const yarp::os::Bottle& command,
375  yarp::os::Bottle& reply) {
376  WorldOp op;
377  WorldResult result;
378  ManagerState state(command,op,result,*this);
379  reply.clear();
380 
381  op.cmd = (WORLD_OP)command.get(1).asVocab32();
382  switch (op.cmd) {
383  case WORLD_OP_GET:
384  doGet(state);
385  break;
386  case WORLD_OP_SET:
387  doSet(state);
388  break;
389  case WORLD_OP_MK:
390  doMake(state);
391  break;
392  case WORLD_OP_GRAB:
393  doGrab(state);
394  break;
395  case WORLD_OP_ROT:
396  doRotate(state);
397  break;
398  case WORLD_OP_DEL:
399  doDelete(state);
400  break;
401  case WORLD_OP_COL:
402  doColor(state);
403  break;
404  case WORLD_OP_NUM:
405  doNumber(state);
406  break;
407  default:
408  state.failed = true;
409  state.why = "unrecognized command";
410  break;
411  }
412 
413  state.debug();
414  if (!result.success) {
415  if (reply.size()==0) {
416  reply.addVocab32(yarp::os::createVocab32('f','a','i','l'));
417  }
418  if (state.failed) {
419  reply.addString(state.why.c_str());
420  }
421  if (result.msg!="") {
422  if (result.msg!=state.why) {
423  reply.addString(result.msg.c_str());
424  }
425  }
426  return true;
427  } else {
428  if (reply.size()==0) {
429  if (result.location.isValid()) {
430  reply.addFloat64(result.location.get(0));
431  reply.addFloat64(result.location.get(1));
432  reply.addFloat64(result.location.get(2));
433  } else if (result.rotation.isValid()) {
434  reply.addFloat64(result.rotation.get(0));
435  reply.addFloat64(result.rotation.get(1));
436  reply.addFloat64(result.rotation.get(2));
437  } else if (result.color.isValid()) {
438  reply.addFloat64(result.color.get(0));
439  reply.addFloat64(result.color.get(1));
440  reply.addFloat64(result.color.get(2));
441  } else if (result.count.isValid()) {
442  reply.addInt32(result.count.get());
443  } else if (result.path.isValid()) {
444  reply.addString(result.path.get().c_str());
445  } else {
446  reply.addVocab32(yarp::os::createVocab32('o','k'));
447  }
448  }
449  }
450  return true;
451 }
WORLD_OP_GRAB
@ WORLD_OP_GRAB
Definition: WorldOp.h:33
ManagerState
Definition: WorldManager.cpp:29
ManagerState::fail
void fail(const char *msg)
Definition: WorldManager.cpp:62
WorldOpIndex
Definition: WorldOp.h:75
doRotate
bool doRotate(ManagerState &state)
Definition: WorldManager.cpp:347
WorldOpFlag
Definition: WorldOp.h:64
WorldResult::success
bool success
Definition: WorldOp.h:140
ManagerState::ManagerState
ManagerState(const yarp::os::Bottle &command, WorldOp &op, WorldResult &result, WorldManager &manager)
Definition: WorldManager.cpp:40
doSet
bool doSet(ManagerState &state)
Definition: WorldManager.cpp:285
WORLD_OP_COL
@ WORLD_OP_COL
Definition: WorldOp.h:36
WorldResult::location
WorldOpTriplet location
Definition: WorldOp.h:142
ManagerState::result
WorldResult & result
Definition: WorldManager.cpp:35
WorldResult::path
WorldOpName path
Definition: WorldOp.h:145
ManagerState::consume
bool consume(WorldOpTriplet &x, const char *msg)
Definition: WorldManager.cpp:68
WorldResult::rotation
WorldOpTriplet rotation
Definition: WorldOp.h:143
WORLD_OP_DEL
@ WORLD_OP_DEL
Definition: WorldOp.h:35
ManagerState::consume
bool consume(WorldOpScalar &x, const char *msg)
Definition: WorldManager.cpp:114
WorldManager
Definition: WorldManager.h:28
WorldOpScalar
Definition: WorldOp.h:86
ManagerState::command
const yarp::os::Bottle & command
Definition: WorldManager.cpp:33
WorldOpDatum::isValid
bool isValid() const
Definition: WorldOp.h:46
consumeKind
void consumeKind(ManagerState &state)
Definition: WorldManager.cpp:178
state
state
Definition: WholeBodyPlayerModule.h:64
ManagerState::offset
int offset
Definition: WorldManager.cpp:36
doGet
bool doGet(ManagerState &state)
Definition: WorldManager.cpp:265
WorldManager.h
ManagerState::manager
WorldManager & manager
Definition: WorldManager.cpp:38
ManagerState::consume
bool consume(WorldOpFlag &x, const char *msg)
Definition: WorldManager.cpp:128
WORLD_OP_GET
@ WORLD_OP_GET
Definition: WorldOp.h:30
WORLD_OP
WORLD_OP
Definition: WorldOp.h:28
doDelete
bool doDelete(ManagerState &state)
Definition: WorldManager.cpp:358
ManagerState::needIndex
bool needIndex
Definition: WorldManager.cpp:37
WorldManager::respond
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: FakeWorldManager.cpp:13
consumeObject
void consumeObject(ManagerState &state)
Definition: WorldManager.cpp:256
WorldOpTriplet::get
double get(int offset) const
Definition: WorldOp.h:110
WorldResult::count
WorldOpIndex count
Definition: WorldOp.h:146
ManagerState::consume
bool consume(WorldOpName &x, const char *msg)
Definition: WorldManager.cpp:100
ManagerState::consume
bool consume(WorldOpIndex &x, const char *msg)
Definition: WorldManager.cpp:86
ManagerState::why
std::string why
Definition: WorldManager.cpp:32
WorldOp::cmd
WORLD_OP cmd
Definition: WorldOp.h:116
WorldResult::msg
std::string msg
Definition: WorldOp.h:141
ManagerState::op
WorldOp & op
Definition: WorldManager.cpp:34
x
x
Definition: compute_ekf_sym.m:21
state::ok
@ ok
WorldOpIndex::get
int get() const
Definition: WorldOp.h:83
WorldOpName
Definition: WorldOp.h:51
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
doNumber
bool doNumber(ManagerState &state)
Definition: WorldManager.cpp:366
WORLD_OP_MK
@ WORLD_OP_MK
Definition: WorldOp.h:32
doMake
bool doMake(ManagerState &state)
Definition: WorldManager.cpp:303
WORLD_OP_ROT
@ WORLD_OP_ROT
Definition: WorldOp.h:34
WorldOpName::get
std::string get() const
Definition: WorldOp.h:61
WORLD_OP_SET
@ WORLD_OP_SET
Definition: WorldOp.h:31
WorldResult::color
WorldOpTriplet color
Definition: WorldOp.h:144
doColor
bool doColor(ManagerState &state)
Definition: WorldManager.cpp:273
ManagerState::get
const yarp::os::Value & get(int offset)
Definition: WorldManager.cpp:54
WorldOp
Definition: WorldOp.h:114
WORLD_OP_NUM
@ WORLD_OP_NUM
Definition: WorldOp.h:37
doGrab
bool doGrab(ManagerState &state)
Definition: WorldManager.cpp:337
consumeHand
void consumeHand(ManagerState &state)
Definition: WorldManager.cpp:243
WorldOpTriplet
Definition: WorldOp.h:97
WorldResult
Definition: WorldOp.h:138
ManagerState::failed
bool failed
Definition: WorldManager.cpp:31
WorldResult::setFail
void setFail(const char *msg)
Definition: WorldOp.h:158
ManagerState::debug
void debug()
Definition: WorldManager.cpp:172
ManagerState::more
bool more()
Definition: WorldManager.cpp:58