Revision 5e4bc615 examples/meka/src/mekajointinterface.cpp

View differences:

examples/meka/src/mekajointinterface.cpp
1 1 
#include "mekajointinterface.h"
2 
using namespace std;
2 

  		




  
  3
  
    
using std::cout;


  		




  
  4
  
    
using std::cerr;


  		




  
  5
  
    

  		




  
  6
  
    
using humotion::Timestamp;


  		




  3
  7
  
    

  		




  4
  8
  
    
//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED


  		




  5
  9
  
    
#define POSITION_CONTROL 1


  		




  ......




  28
  32
  
    

  		




  29
  33
  
    
void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){


  		




  30
  34
  
    
    //fetch current timestamp


  		




  31
  
  
    
    double timestamp = msg.header.stamp.toSec();


  		




  
  35
  
    
    Timestamp timestamp = Timestamp(msg.header.stamp.toSec());


  		




  32
  36
  
    

  		




  33
  37
  
    
    //iterate through incoming joints and filter out joints we need:


  		




  34
  38
  
    
    for(int i=0; i<msg.name.size(); i++){


  		




  ......




  68
  72
  
    
    }


  		




  69
  73
  
    

  		




  70
  74
  
    
    //dummy data uses current time


  		




  71
  
  
    
    timestamp = get_timestamp_s();


  		




  
  75
  
    
    timestamp = Timestamp::now();


  		




  72
  76
  
    

  		




  73
  77
  
    
    //store dummy positions for joints we do not know about:


  		




  74
  78
  
    
    store_dummy_data(ID_LIP_LEFT_UPPER, timestamp);


  		




  ......




  91
  95
  
    

  		




  92
  96
  
    
}


  		




  93
  97
  
    

  		




  94
  
  
    
void MekaJointInterface::store_dummy_data(int id, double timestamp){


  		




  
  98
  
    
void MekaJointInterface::store_dummy_data(int id, Timestamp timestamp){


  		




  95
  99
  
    
    JointInterface::store_incoming_position(id, 0.0, timestamp);


  		




  96
  100
  
    
    JointInterface::store_incoming_velocity(id, 0.0, timestamp);


  		




  97
  101
  
    
}


  		




  ......




  131
  135
  
    
   ros::spin();


  		




  132
  136
  
    
}


  		




  133
  137
  
    

  		




  134
  
  
    

  		




  135
  
  
    
double MekaJointInterface::get_timestamp_s(){


  		




  136
  
  
    
    struct timespec spec;


  		




  137
  
  
    
    clock_gettime(CLOCK_REALTIME, &spec);


  		




  138
  
  
    
    return spec.tv_sec + spec.tv_nsec / 1.0e9;


  		




  139
  
  
    
}


  		




  140
  
  
    

  		




  141
  138
  
    
//! set the target position of a joint


  		




  142
  139
  
    
//! \param enum id of joint


  		




  143
  140
  
    
//! \param float value


  		












Also available in:  Unified diff