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, iCubJointInterface *icub_jointinterface);

    bool threadInit();

    void threadRelease();

    void run();

private:

    void store_incoming_position(int icub_id, double value, double timestamp);

    float target_eye_pan_;

    float target_eye_vergence_;

    yarp::sig::Vector yarp_positions_;

    yarp::sig::Vector yarp_commands_;

    yarp::sig::Vector yarp_timestamps_;

    iCubJointInterface *icub_jointinterface_;

    yarp::dev::IEncodersTimed *yarp_iencs_;

};