Revision 1f748ce7 examples/yarp_icub/src/icub_data_receiver.cpp

View differences:

examples/yarp_icub/src/icub_data_receiver.cpp
1 1 
#include "icub_data_receiver.h"
2 2 
#include <humotion/server/joint_interface.h>
3 3 
#include <yarp/os/Property.h>
4 
#include <boost/format.hpp>
5 
//using namespace yarp::dev;
6 
//using namespace yarp::sig;
7 
//using namespace yarp::os;
4 
//#include <boost/format.hpp>
8 5 
using std::cout;
6 
using std::cerr;
9 7 
using std::string;
10 

  		




  11
  8
  
    
using humotion::server::JointInterface;


  		




  12
  9
  
    
using yarp::dev::IEncodersTimed;


  		




  13
  10
  
    
using yarp::sig::Vector;


  		




  ......




  24
  21
  
    
    yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();


  		




  25
  22
  
    
    bool success = poly_driver->view(iencs_);


  		




  26
  23
  
    
    if (!success) {


  		




  27
  
  
    
        cout << "ERROR: polydriver failed to init iencs view\n";


  		




  
  24
  
    
        cerr << "ERROR: polydriver failed to init iencs view\n";


  		




  28
  25
  
    
        exit(EXIT_FAILURE);


  		




  29
  26
  
    
    }


  		




  30
  27
  
    

  		




  ......




  43
  40
  
    
void iCubDataReceiver::threadRelease() {


  		




  44
  41
  
    
}


  		




  45
  42
  
    

  		




  46
  
  
    
yarp::sig::Vector iCubDataReceiver::calculate_velocities(yarp::sig::Vector positions,


  		




  47
  
  
    
                                                         yarp::sig::Vector timestamps) {


  		




  48
  
  
    
    yarp::sig::Vector velocities;


  		




  
  43
  
    
Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {


  		




  
  44
  
    
    Vector velocities;


  		




  49
  45
  
    
    velocities.resize(positions.size());


  		




  50
  46
  
    

  		




  51
  
  
    
    cout << "\n";


  		




  52
  47
  
    
    if (previous_positions_.size() == 0){


  		




  53
  48
  
    
        // first run, no valid old position available, return zero velocities


  		




  54
  49
  
    
        // by setting all all elements to zero


  		




  ......




  60
  55
  
    
            float timediff = timestamps[i] - previous_timestamps_[i];


  		




  61
  56
  
    
            // calc speed:


  		




  62
  57
  
    
            velocities[i] = diff / timediff;


  		




  63
  
  
    
            cout << " [" << i << "]=" <<  boost::format("%6.3f") % velocities[i];


  		




  64
  58
  
    
        }


  		




  65
  59
  
    
    }


  		




  66
  
  
    
    cout << " VEL\n";


  		




  67
  60
  
    

  		




  68
  61
  
    
    previous_positions_ = positions;


  		




  69
  62
  
    
    previous_timestamps_ = timestamps;


  		












Also available in:  Unified diff