Revision 7adf90be examples/yarp_icub/src/icub_data_receiver.cpp

View differences:

examples/yarp_icub/src/icub_data_receiver.cpp
5 5 
using namespace yarp::os;
6 6 
using namespace std;
7 7 

  		




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


  		




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


  		




  9
  9
  
    
    iencs = _iencs;


  		




  10
  10
  
    
    icub_jointinterface = _icub_jointinterface;


  		




  11
  11
  
    
    int joints;


  		




  12
  12
  
    
    iencs->getAxes(&joints);


  		




  13
  13
  
    
    positions.resize(joints);


  		




  14
  14
  
    
    velocities.resize(joints);


  		




  
  15
  
    
    timestamps.resize(joints);


  		




  15
  16
  
    
}


  		




  16
  17
  
    

  		




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


  		




  ......




  24
  25
  
    
}


  		




  25
  26
  
    

  		




  26
  27
  
    
void iCubDataReceiver::run(){


  		




  
  28
  
    

  		




  27
  29
  
    
    //grab pos+vel data:


  		




  28
  
  
    
    iencs->getEncoders(positions.data());


  		




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


  		




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


  		




  30
  
  
    
    double timestamp = get_timestamp_ms();


  		




  
  32
  
    

  		




  
  33
  
    
    //double timestamp = get_timestamp_ms();


  		




  31
  34
  
    

  		




  32
  35
  
    
    //publish data to humotion


  		




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


  		




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


  		




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


  		




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


  		




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


  		




  36
  39
  
    
    }


  		




  37
  40
  
    

  		




  38
  41
  
    
    //tell humotion to update lid angle (hack)


  		




  39
  
  
    
    icub_jointinterface->fetch_position(100, 0.0, timestamp);


  		




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


  		




  40
  43
  
    
    //fixme: use real id


  		




  41
  44
  
    
}


  		












Also available in:  Unified diff