Revision 5e4bc615

View differences:

examples/meka/include/mekajointinterface.h
2 2 
#include <humotion/server/joint_interface.h>
3 3 
#include <humotion/server/server.h>
4 4 
#include <boost/bimap.hpp>
5 
//#include "meka_data_receiver.h"
6 
//class MekaDataReceiver;
5 

  		




  7
  6
  
    
#include "ros/ros.h"


  		




  8
  7
  
    
#include "sensor_msgs/JointState.h"


  		




  9
  8
  
    
#include "trajectory_msgs/JointTrajectory.h"


  		




  
  9
  
    

  		




  10
  10
  
    
#include <m3meka_msgs/M3ControlStates.h>


  		




  11
  11
  
    

  		




  12
  12
  
    
class MekaJointInterface : public humotion::server::JointInterface{


  		




  ......




  31
  31
  
    
private:


  		




  32
  32
  
    
    void incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state);


  		




  33
  33
  
    
    void incoming_jointstates(const sensor_msgs::JointState & msg);


  		




  34
  
  
    
    void store_dummy_data(int id, double timestamp);


  		




  
  34
  
    
    void store_dummy_data(int id, humotion::Timestamp timestamp);


  		




  35
  35
  
    
    void store_min_max(int id, float min, float max);


  		




  36
  36
  
    
    ros::Subscriber joint_state_subscriber;


  		




  37
  37
  
    
    ros::Subscriber control_state_subscriber;


  		




  38
  38
  
    
    ros::Publisher target_publisher;


  		




  39
  39
  
    

  		




  40
  
  
    
    double get_timestamp_s();


  		




  41
  
  
    

  		




  42
  40
  
    
    void set_eyelid_angle(double angle);


  		




  43
  41
  
    
    void set_eyebrow_angle(int id);


  		




  44
  42
  
    
    void set_mouth();


  		












  

    
      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