iCub-main
main.cpp
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 
158 #include <string>
159 #include <iostream>
160 #include <iomanip>
161 
162 #include <yarp/os/all.h>
163 #include <yarp/sig/Vector.h>
164 
166 
167 using namespace std;
168 using namespace yarp::os;
169 using namespace yarp::sig;
170 using namespace iCub::ctrl;
171 
172 
173 // A class which handles the incoming data.
174 // The estimated derivatives are returned at once
175 // since they are computed within the onRead method.
176 class dataCollector : public BufferedPort<Bottle>
177 {
178 private:
179  AWLinEstimator *linEst;
180  AWQuadEstimator *quadEst;
181  BufferedPort<Vector> &port_vel;
182  BufferedPort<Vector> &port_acc;
183 
184  virtual void onRead(Bottle &b)
185  {
186  Stamp info;
187  BufferedPort<Bottle>::getEnvelope(info);
188 
189  size_t sz=b.size();
190  Vector x(sz);
191 
192  for (unsigned int i=0; i<sz; i++)
193  x[i]=b.get(i).asFloat64();
194 
195  // for the estimation the time stamp
196  // is required. If not present within the
197  // packet, the actual machine time is
198  // attached to it.
199  AWPolyElement el(x,info.isValid()?info.getTime():Time::now());
200  port_vel.prepare()=linEst->estimate(el);
201  port_acc.prepare()=quadEst->estimate(el);
202 
203  // the outbound packets will carry the same
204  // envelope information of the inbound ones.
205  if (port_vel.getOutputCount()>0)
206  {
207  port_vel.setEnvelope(info);
208  port_vel.write();
209  }
210  else
211  port_vel.unprepare();
212 
213  if (port_acc.getOutputCount()>0)
214  {
215  port_acc.setEnvelope(info);
216  port_acc.write();
217  }
218  else
219  port_acc.unprepare();
220  }
221 
222 public:
223  dataCollector(unsigned int NVel, double DVel, BufferedPort<Vector> &_port_vel,
224  unsigned int NAcc, double DAcc, BufferedPort<Vector> &_port_acc) :
225  port_vel(_port_vel), port_acc(_port_acc)
226  {
227  linEst =new AWLinEstimator(NVel,DVel);
228  quadEst=new AWQuadEstimator(NAcc,DAcc);
229  }
230 
232  {
233  delete linEst;
234  delete quadEst;
235  }
236 };
237 
238 
239 
240 // Usual YARP stuff...
241 class velObserver: public RFModule
242 {
243 private:
244  dataCollector *port_pos;
245  BufferedPort<Vector> port_vel;
246  BufferedPort<Vector> port_acc;
247  Port rpcPort;
248 
249 public:
250  virtual bool configure(ResourceFinder &rf)
251  {
252  string portName=rf.check("name",Value("/velObs")).asString();
253 
254  unsigned int NVel=rf.check("lenVel",Value(16)).asInt32();
255  unsigned int NAcc=rf.check("lenAcc",Value(25)).asInt32();
256 
257  double DVel=rf.check("thrVel",Value(1.0)).asFloat64();
258  double DAcc=rf.check("thrAcc",Value(1.0)).asFloat64();
259 
260  if (NVel<2)
261  {
262  yWarning()<<"lenVel cannot be lower than 2 => N=2 is assumed";
263  NVel=2;
264  }
265 
266  if (NAcc<3)
267  {
268  yWarning()<<"lenAcc cannot be lower than 3 => N=3 is assumed";
269  NAcc=3;
270  }
271 
272  if (DVel<0.0)
273  {
274  yWarning()<<"thrVel cannot be lower than 0.0 => D=0.0 is assumed";
275  DVel=0.0;
276  }
277 
278  if (DAcc<0.0)
279  {
280  yWarning()<<"thrAcc cannot be lower than 0.0 => D=0.0 is assumed";
281  DAcc=0.0;
282  }
283 
284  port_vel.open(portName+"/vel:o");
285  port_acc.open(portName+"/acc:o");
286 
287  port_pos=new dataCollector(NVel,DVel,port_vel,NAcc,DAcc,port_acc);
288  port_pos->useCallback();
289  port_pos->open(portName+"/pos:i");
290 
291  rpcPort.open(portName+"/rpc");
292  attach(rpcPort);
293 
294  return true;
295  }
296 
297  virtual bool close()
298  {
299  port_pos->interrupt();
300  port_vel.interrupt();
301  port_acc.interrupt();
302  rpcPort.interrupt();
303 
304  port_pos->close();
305  port_vel.close();
306  port_acc.close();
307  rpcPort.close();
308 
309  delete port_pos;
310 
311  return true;
312  }
313 
314  virtual double getPeriod() { return 1.0; }
315  virtual bool updateModule() { return true; }
316 };
317 
318 
319 
320 int main(int argc, char *argv[])
321 {
322  Network yarp;
323 
324  ResourceFinder rf;
325  rf.configure(argc,argv);
326 
327  if (rf.check("help"))
328  {
329  cout<<"Options:" << endl;
330  cout<<"\t--name name: observer port name (default /velObs)" << endl;
331  cout<<"\t--lenVel N: velocity window's max length (default: 16)" << endl;
332  cout<<"\t--thrVel D: velocity max deviation threshold (default: 1.0)" << endl;
333  cout<<"\t--lenAcc N: acceleration window's max length (default: 25)" << endl;
334  cout<<"\t--thrAcc D: acceleration max deviation threshold (default: 1.0)" << endl;
335  cout<<endl;
336 
337  return 0;
338  }
339 
340  if (!yarp.checkNetwork())
341  {
342  yError()<<"YARP server not available!";
343  return 1;
344  }
345 
346  velObserver obs;
347  return obs.runModule(rf);
348 }
349 
350 
iCub::ctrl::AWQuadEstimator
Definition: adaptWinPolyEstimator.h:207
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
velObserver::updateModule
virtual bool updateModule()
Definition: main.cpp:315
dataCollector::~dataCollector
~dataCollector()
Definition: main.cpp:231
main
int main(int argc, char *argv[])
Definition: main.cpp:31
dataCollector::dataCollector
dataCollector(unsigned int NVel, double DVel, BufferedPort< Vector > &_port_vel, unsigned int NAcc, double DAcc, BufferedPort< Vector > &_port_acc)
Definition: main.cpp:223
velObserver::configure
virtual bool configure(ResourceFinder &rf)
Definition: main.cpp:250
iCub::ctrl::AWPolyEstimator::estimate
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
Definition: adaptWinPolyEstimator.cpp:100
iCub::ctrl::AWPolyElement
Definition: adaptWinPolyEstimator.h:45
velObserver::close
virtual bool close()
Definition: main.cpp:297
velObserver::getPeriod
virtual double getPeriod()
Definition: main.cpp:314
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
x
x
Definition: compute_ekf_sym.m:21
adaptWinPolyEstimator.h
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
dataCollector
Definition: main.cpp:176
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
scripting.argc
argc
Definition: scripting.py:184
velObserver
Definition: main.cpp:241