imu.h

#ifndef IMU_H
#define IMU_H
 
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/dev/IPositionControl.h>
#include <yarp/dev/IEncoders.h>
#include <yarp/dev/IAxisInfo.h>
#include <yarp/dev/IMultipleWrapper.h>
#include <yarp/robottestingframework/TestCase.h>
#include <yarp/manager/localbroker.h>
 
#include <iDynTree/KinDynComputations.h>
#include <iDynTree/ModelLoader.h>
#include <iDynTree/Model.h>
 
class Imu : public yarp::robottestingframework::TestCase {
    public:
        Imu();
        virtual ~Imu();
        virtual bool setup(yarp::os::Property& property);
        virtual void tearDown();
        virtual void run();
 
    private:
        std::string robotName;
        std::string portName;
        std::string modelName;
        double errorMax;
        yarp::os::Bottle sensorsList;
        std::vector<std::string> partsList;
 
        yarp::dev::PolyDriver MASclientDriver;
        yarp::dev::PolyDriver controlBoardDriver;
        yarp::dev::PolyDriver MASremapperDriver;
        yarp::dev::IOrientationSensors* iorientation;
        yarp::dev::IPositionControl* ipos;
        yarp::dev::IEncoders* ienc;
        yarp::dev::IAxisInfo* iaxes;
        yarp::dev::IMultipleWrapper* imultiwrap;
 
        yarp::os::BufferedPort <yarp::os::Bottle> outputPort;
        std::vector<yarp::sig::Vector> rpyValues;
        yarp::sig::Vector positions;
        yarp::sig::Vector velocities;
 
        int axes;
        std::vector<std::string> axesVec;
 
        iDynTree::ModelLoader model;
        iDynTree::KinDynComputations kinDynComp;
        iDynTree::VectorDynSize s;
        iDynTree::VectorDynSize ds;
        iDynTree::Vector3 gravity;
        iDynTree::Rotation baseLinkOrientation;
        iDynTree::Twist baseVelocity;
        iDynTree::Transform I_T_base;
        std::vector<iDynTree::Rotation> I_R_I_IMU;
 
        robometry::BufferManager bufferManager;
 
        bool startMove();
        bool setupRobometry();
        void setupBrokers();
 
        std::vector<yarp::manager::LocalBroker> localBroker;
        yarp::manager::LocalBroker scriptBroker;
        std::string strCmd;
        std::string strParam;
};
 
#endif //IMU_H