main.cpp

Go to the documentation of this file.

/* 
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/
 
 
#include <string>
#include <iostream>
#include <iomanip>
 
#include <yarp/os/all.h>
#include <yarp/sig/Vector.h>
 
#include <iCub/ctrl/adaptWinPolyEstimator.h>
 
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace iCub::ctrl;
 
 
// A class which handles the incoming data.
// The estimated derivatives are returned at once
// since they are computed within the onRead method.
class dataCollector : public BufferedPort<Bottle>
{
private:
    AWLinEstimator       *linEst;
    AWQuadEstimator      *quadEst;
    BufferedPort<Vector> &port_vel;
    BufferedPort<Vector> &port_acc;
 
    virtual void onRead(Bottle &b)
    {
        Stamp info;
        BufferedPort<Bottle>::getEnvelope(info);
 
        size_t sz=b.size();
        Vector x(sz);
 
        for (unsigned int i=0; i<sz; i++)
            x[i]=b.get(i).asFloat64();
 
        // for the estimation the time stamp
        // is required. If not present within the
        // packet, the actual machine time is 
        // attached to it.
        AWPolyElement el(x,info.isValid()?info.getTime():Time::now());
        port_vel.prepare()=linEst->estimate(el);
        port_acc.prepare()=quadEst->estimate(el);
 
        // the outbound packets will carry the same
        // envelope information of the inbound ones.
        if (port_vel.getOutputCount()>0)
        {
            port_vel.setEnvelope(info);
            port_vel.write();
        }
        else
            port_vel.unprepare();
        
        if (port_acc.getOutputCount()>0)
        {
            port_acc.setEnvelope(info);
            port_acc.write();
        }
        else
            port_acc.unprepare();
    }
 
public:
    dataCollector(unsigned int NVel, double DVel, BufferedPort<Vector> &_port_vel,
                  unsigned int NAcc, double DAcc, BufferedPort<Vector> &_port_acc) :
                  port_vel(_port_vel), port_acc(_port_acc)
    {
        linEst =new AWLinEstimator(NVel,DVel);
        quadEst=new AWQuadEstimator(NAcc,DAcc);
    }
 
    ~dataCollector()
    {
        delete linEst;
        delete quadEst;
    }
};
 
 
 
// Usual YARP stuff...
class velObserver: public RFModule
{
private:
    dataCollector        *port_pos;
    BufferedPort<Vector>  port_vel;
    BufferedPort<Vector>  port_acc;
    Port                  rpcPort;
 
public:
    virtual bool configure(ResourceFinder &rf)
    {
        string portName=rf.check("name",Value("/velObs")).asString();
 
        unsigned int NVel=rf.check("lenVel",Value(16)).asInt32();
        unsigned int NAcc=rf.check("lenAcc",Value(25)).asInt32();
 
        double DVel=rf.check("thrVel",Value(1.0)).asFloat64();
        double DAcc=rf.check("thrAcc",Value(1.0)).asFloat64();
 
        if (NVel<2)
        {
            yWarning()<<"lenVel cannot be lower than 2 => N=2 is assumed";
            NVel=2;
        }
 
        if (NAcc<3)
        {
            yWarning()<<"lenAcc cannot be lower than 3 => N=3 is assumed";
            NAcc=3;
        }
 
        if (DVel<0.0)
        {
            yWarning()<<"thrVel cannot be lower than 0.0 => D=0.0 is assumed";
            DVel=0.0;
        }
 
        if (DAcc<0.0)
        {
            yWarning()<<"thrAcc cannot be lower than 0.0 => D=0.0 is assumed";
            DAcc=0.0;
        }
 
        port_vel.open(portName+"/vel:o");
        port_acc.open(portName+"/acc:o");
 
        port_pos=new dataCollector(NVel,DVel,port_vel,NAcc,DAcc,port_acc);
        port_pos->useCallback();
        port_pos->open(portName+"/pos:i");
 
        rpcPort.open(portName+"/rpc");
        attach(rpcPort);
 
        return true;
    }
 
    virtual bool close()
    {
        port_pos->interrupt();
        port_vel.interrupt();
        port_acc.interrupt();
        rpcPort.interrupt();
 
        port_pos->close();
        port_vel.close();
        port_acc.close();
        rpcPort.close();
 
        delete port_pos;
 
        return true;
    }
 
    virtual double getPeriod()    { return 1.0;  }
    virtual bool   updateModule() { return true; }
};
 
 
 
int main(int argc, char *argv[])
{
    Network yarp;
 
    ResourceFinder rf;
    rf.configure(argc,argv);
 
    if (rf.check("help"))
    {
        cout<<"Options:"                                                             << endl;
        cout<<"\t--name   name: observer port name (default /velObs)"                << endl;
        cout<<"\t--lenVel    N: velocity window's max length (default: 16)"          << endl;
        cout<<"\t--thrVel    D: velocity max deviation threshold (default: 1.0)"     << endl;
        cout<<"\t--lenAcc    N: acceleration window's max length (default: 25)"      << endl;
        cout<<"\t--thrAcc    D: acceleration max deviation threshold (default: 1.0)" << endl;
        cout<<endl;
 
        return 0;
    }
    
    if (!yarp.checkNetwork())
    {
        yError()<<"YARP server not available!";
        return 1;
    }
 
    velObserver obs;
    return obs.runModule(rf);
}