iCub-main
main.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 // Copyright (C) 2008 RobotCub Consortium
3 // Author: Lorenzo Natale
4 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
5 
109 #include <stdio.h>
110 
111 #include <yarp/os/Network.h>
112 #include <yarp/os/Property.h>
113 #include <yarp/os/Time.h>
114 #include <yarp/dev/PolyDriver.h>
115 #include <yarp/os/BufferedPort.h>
116 #include <yarp/os/RFModule.h>
117 #include "velControlThread.h"
118 #include <yarp/os/Log.h>
119 #include <yarp/os/LogStream.h>
120 #include <string.h>
121 
122 /*default rate for the control loop*/
123 const int CONTROL_RATE=20;
124 
125 using namespace yarp::os;
126 using namespace yarp::dev;
127 
128 class VelControlModule: public RFModule {
129 private:
130  PolyDriver driver;
131  velControlThread *vc;
132  char partName[255];
133 
134  //added by ludovic
135  Port input_port;
137 public:
138 
139  virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
140  {
141  reply.clear();
142  yDebug("receiving command from port\n");
143  int index = 0;
144  int cmdSize = command.size();
145 
146  while(cmdSize>0)
147  {
148  switch(command.get(index).asVocab32())
149  {
150  case yarp::os::createVocab32('s','u','s','p'):
151  reply.addVocab32("ack");
152  vc->halt();
153  cmdSize--;
154  index++;
155  break;
156  case yarp::os::createVocab32('r','u','n'):
157  reply.addVocab32("ack");
158  vc->go();
159  cmdSize--;
160  index++;
161  break;
162  //this set current position reference
163  case yarp::os::createVocab32('s','e','t'):
164  if (command.size()>=3)
165  {
166  int i=command.get(index+1).asInt32();
167  double pos=command.get(index+2).asFloat64();
168  vc->setRef(i, pos);
169  index +=3;
170  cmdSize-=3;
171  }
172  else
173  {
174  cmdSize--;
175  index++;
176  yError("Invalid set message, ignoring\n");
177  }
178  reply.addVocab32("ack");
179  break;
180  //this set maximum velocity (limiter)
181  case yarp::os::createVocab32('s','v','e','l'):
182  if(command.size()>=3)
183  {
184  int i=command.get(index+1).asInt32();
185  double vel = command.get(index+2).asFloat64();
186  vc->setVel(i,vel);
187  index += 3;
188  cmdSize-=3;;
189  reply.addVocab32("ack");
190  }
191  else
192  {
193  cmdSize--;
194  index++;
195  yError("Invalid set vel message, ignoring\n");
196  reply.addVocab32("fail");
197  }
198  break;
199  case yarp::os::createVocab32('g','a','i','n'):
200  if(command.size()>=3)
201  {
202  int i=command.get(index+1).asInt32();
203  double gain = command.get(index+2).asFloat64();
204  vc->setGain(i,gain);
205  index+=3;
206  cmdSize-=3;
207  reply.addVocab32("ack");
208  }
209  else
210  {
211  cmdSize--;
212  index++;
213  yError("Invalid set gain message, ignoring\n");
214  reply.addVocab32("fail");
215  }
216  break;
217  case yarp::os::createVocab32('h','e','l','p'):
218  fprintf(stdout,"VelocityControl module, valid commands are:\n");
219  fprintf(stdout,"- [susp] suspend the controller (command zero velocity)\n");
220  fprintf(stdout,"- [run] start (and resume after being suspended) the controller\n");
221  fprintf(stdout,"- [quit] quit the module (exit)\n");
222  fprintf(stdout,"- [set] <j> <p> move joint j to p (degrees)\n");
223  fprintf(stdout,"- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
224  fprintf(stdout,"- [gain] <j> <k> set P gain for joint j to k\n");
225  fprintf(stdout,"- [help] to get this help\n");
226  fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
227 
228  reply.addVocab32("many");
229  reply.addVocab32("ack");
230  reply.addString("VelocityControl module, valid commands are:");
231  reply.addString("- [susp] suspend the controller (command zero velocity)");
232  reply.addString("- [run] start (and resume after being suspended) the controller");
233  reply.addString("- [quit] quit the module (exit)");
234  reply.addString("- [set] <j> <p> move joint j to p (degrees)");
235  reply.addString("- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
236  reply.addString("- [gain] <j> <k> set P gain for joint j to k");
237  reply.addString("- [help] to get this help");
238  reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
239  break;
240  default:
241  yError("Invalid command, ignoring\n");
242  reply.addVocab32("fail");
243  //cmdSize--;
244  //index++;
245  //return respond(command, reply); // call default
246  break;
247  }
248  return true;
249  }
250 
251  return false;
252  }
253 
254  virtual bool configure(yarp::os::ResourceFinder &rf)
255  {
256  Property options;
257  options.fromString(rf.toString());
258  char robotName[255];
259  options.put("device", "remote_controlboard");
260  if(options.check("robot"))
261  strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
262  else
263  strncpy(robotName, "icub", sizeof(robotName));
264 
265  if(options.check("part"))
266  {
267  char tmp[800];
268  snprintf(tmp, sizeof(tmp), "/%s/vc/%s/client",
269  robotName,
270  options.find("part").asString().c_str());
271  options.put("local",tmp);
272 
273  snprintf(tmp, sizeof(tmp),"/%s/%s",
274  robotName,
275  options.find("part").asString().c_str());
276  options.put("remote", tmp);
277 
278  snprintf(tmp, sizeof(tmp), "/%s/vc/%s/input",
279  robotName,
280  options.find("part").asString().c_str());
281  input_port.open(tmp);
282 
283  options.put("carrier", "mcast");
284 
285  attach(input_port);
286  }
287  else
288  {
289  yError("Please specify part (e.g. --part head)\n");
290  return false;
291  }
293 
294  if (!driver.open(options))
295  {
296  yError("Error opening device, check parameters\n");
297  return false;
298  }
299 
301  int period = CONTROL_RATE;
302  if(options.check("period"))
303  period = options.find("period").asInt32();
304 
305  yInfo("control rate is %d ms",period);
306 
307  if (!options.check("part"))
308  return false;
309 
310  snprintf(partName, sizeof(partName), "%s", options.find("part").asString().c_str());
311 
312  vc=new velControlThread(period);
313  vc->init(&driver, partName,
314  robotName);
315 
316  vc->start();
317  return true;
318  }
319 
320  virtual bool close()
321  {
322  yInfo("Closing module [%s]\n", partName);
323  vc->stop();
324  vc->halt();
325  delete vc;
326  yInfo("Thead [%s] stopped\n", partName);
327 
328  driver.close();
329  input_port.close();
330 
331  yInfo("Module [%s] closed\n", partName);
332  return true;
333  }
334 
336  {
337  return true;
338  }
339 
340 };
341 
342 int main(int argc, char *argv[])
343 {
344  Network yarp;
345  VelControlModule mod;
346  ResourceFinder rf;
347 
348  rf.configure(argc, argv);
349  if (mod.configure(rf)==false) return 1;
350  mod.runModule();
351  yInfo("Main returning\n");
352  return 0;
353 }
354 
virtual bool close()
Definition: main.cpp:320
bool updateModule()
Definition: main.cpp:335
virtual bool configure(yarp::os::ResourceFinder &rf)
Definition: main.cpp:254
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: main.cpp:139
void setGain(int i, double gain)
void setRef(int i, double pos)
bool init(yarp::dev::PolyDriver *d, std::string partName, std::string robotName)
void setVel(int i, double vel)
int main(int argc, char *argv[])
Definition: main.cpp:31
const int CONTROL_RATE
Definition: main.cpp:123
Copyright (C) 2008 RobotCub Consortium.
fprintf(fid,'\n')