humotion

#include "icub_data_receiver.h"

#include <yarp/os/Property.h>

using namespace yarp::dev;

using namespace yarp::sig;

using namespace yarp::os;

using namespace std;

iCubDataReceiver::iCubDataReceiver(int period, IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){

    iencs = _iencs;

    icub_jointinterface = _icub_jointinterface;

    int joints;

    iencs->getAxes(&joints);

    positions.resize(joints);

    velocities.resize(joints);

    timestamps.resize(joints);

}

bool iCubDataReceiver::threadInit(){ return true; }

void iCubDataReceiver::threadRelease(){ }

double iCubDataReceiver::get_timestamp_ms(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec+spec.tv_nsec/1000000000.0;

}

void iCubDataReceiver::run(){

    //grab pos+vel data:

    iencs->getEncodersTimed(positions.data(), timestamps.data());

    iencs->getEncoderSpeeds(velocities.data());

    //double timestamp = get_timestamp_ms();

    //publish data to humotion

    for(int i=0; i<positions.size(); i++){

        icub_jointinterface->fetch_position(i, positions[i], timestamps[i]);

        icub_jointinterface->fetch_speed(i, velocities[i], timestamps[i]);

        if (i==iCubJointInterface::ICUB_ID_NECK_PAN){

            printf("\nMMM p=%f v=%f\n",positions[i],velocities[i]);

        }

    }

    //printf("\n%f %f %f TIME\n", get_timestamp_ms(), timestamps[2], positions[2]);

    //printf("TIMEDIFF %f\n", get_timestamp_ms() - timestamps[2]);

    //tell humotion to update lid angle (hack)

    icub_jointinterface->fetch_position(100, 0.0, get_timestamp_ms());

    //fixme: use real id

}