humotion

#pragma once

#include <humotion/server/joint_interface.h>

#include <boost/bimap.hpp>

#include <yarp/dev/PolyDriver.h>

#include <yarp/dev/IControlLimits2.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/os/Time.h>

#include <yarp/sig/Vector.h>

#include "icub_jointinterface.h"

#include <humotion/server/server.h>

#include <yarp/os/Network.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include "icub_jointinterface.h"

//forward declaration to solve loop

class iCubJointInterface;

class iCubDataReceiver : public yarp::os::RateThread{

public:

    iCubDataReceiver(int period, yarp::dev::IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface);

    bool threadInit();

    void threadRelease();

    void run();

private:

    double get_timestamp_ms();

    yarp::sig::Vector positions;

    yarp::sig::Vector velocities;

    yarp::sig::Vector commands;

    yarp::sig::Vector timestamps;

    yarp::dev::IEncodersTimed *iencs;

    iCubJointInterface *icub_jointinterface;

};