162#include <yarp/os/all.h>
163#include <yarp/sig/Vector.h>
168using namespace yarp::os;
169using namespace yarp::sig;
181 BufferedPort<Vector> &port_vel;
182 BufferedPort<Vector> &port_acc;
184 virtual void onRead(Bottle &b)
187 BufferedPort<Bottle>::getEnvelope(info);
192 for (
unsigned int i=0; i<sz; i++)
193 x[i]=b.get(i).asFloat64();
200 port_vel.prepare()=linEst->
estimate(el);
201 port_acc.prepare()=quadEst->
estimate(el);
205 if (port_vel.getOutputCount()>0)
207 port_vel.setEnvelope(info);
211 port_vel.unprepare();
213 if (port_acc.getOutputCount()>0)
215 port_acc.setEnvelope(info);
219 port_acc.unprepare();
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)
245 BufferedPort<Vector> port_vel;
246 BufferedPort<Vector> port_acc;
252 string portName=rf.check(
"name",Value(
"/velObs")).asString();
254 unsigned int NVel=rf.check(
"lenVel",Value(16)).asInt32();
255 unsigned int NAcc=rf.check(
"lenAcc",Value(25)).asInt32();
257 double DVel=rf.check(
"thrVel",Value(1.0)).asFloat64();
258 double DAcc=rf.check(
"thrAcc",Value(1.0)).asFloat64();
262 yWarning()<<
"lenVel cannot be lower than 2 => N=2 is assumed";
268 yWarning()<<
"lenAcc cannot be lower than 3 => N=3 is assumed";
274 yWarning()<<
"thrVel cannot be lower than 0.0 => D=0.0 is assumed";
280 yWarning()<<
"thrAcc cannot be lower than 0.0 => D=0.0 is assumed";
284 port_vel.open(portName+
"/vel:o");
285 port_acc.open(portName+
"/acc:o");
287 port_pos=
new dataCollector(NVel,DVel,port_vel,NAcc,DAcc,port_acc);
288 port_pos->useCallback();
289 port_pos->open(portName+
"/pos:i");
291 rpcPort.open(portName+
"/rpc");
299 port_pos->interrupt();
300 port_vel.interrupt();
301 port_acc.interrupt();
320int main(
int argc,
char *argv[])
325 rf.configure(argc,argv);
327 if (rf.check(
"help"))
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;
340 if (!
yarp.checkNetwork())
342 yError()<<
"YARP server not available!";
347 return obs.runModule(rf);
dataCollector(unsigned int NVel, double DVel, BufferedPort< Vector > &_port_vel, unsigned int NAcc, double DAcc, BufferedPort< Vector > &_port_acc)
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
Adaptive window quadratic fitting to estimate the second derivative.
virtual double getPeriod()
virtual bool configure(ResourceFinder &rf)
virtual bool updateModule()
Copyright (C) 2008 RobotCub Consortium.