Revision 6353defa examples/meka/src/mekajointinterface.cpp

View differences:

examples/meka/src/mekajointinterface.cpp
6 6 

  		




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


  		




  8
  8
  
    
    //fetch current timestamp


  		




  9
  
  
    
    double timestamp = get_timestamp_ms();


  		




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


  		




  10
  10
  
    

  		




  11
  11
  
    
    //iterate through incoming joints and filter out joints we need:


  		




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


  		




  
  13
  
    

  		




  13
  14
  
    
        string name = msg.name[i];


  		




  14
  15
  
    
        //printf("incoming data for joint '%s'\n", name.c_str());


  		




  15
  16
  
    

  		




  ......




  38
  39
  
    
        }


  		




  39
  40
  
    
    }


  		




  40
  41
  
    

  		




  
  42
  
    
    //dummy data uses current time


  		




  
  43
  
    
    timestamp = get_timestamp_s();


  		




  
  44
  
    

  		




  41
  45
  
    
    //store dummy positions for joints we do not know about:


  		




  42
  46
  
    
    store_dummy_data(ID_LIP_LEFT_UPPER, timestamp);


  		




  43
  47
  
    
    store_dummy_data(ID_LIP_LEFT_LOWER, timestamp);


  		




  ......




  97
  101
  
    
}


  		




  98
  102
  
    

  		




  99
  103
  
    

  		




  100
  
  
    
double MekaJointInterface::get_timestamp_ms(){


  		




  
  104
  
    
double MekaJointInterface::get_timestamp_s(){


  		




  101
  105
  
    
    struct timespec spec;


  		




  102
  106
  
    
    clock_gettime(CLOCK_REALTIME, &spec);


  		




  103
  
  
    
    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;


  		




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


  		




  104
  108
  
    
}


  		




  105
  109
  
    

  		




  106
  110
  
    
//! set the target position of a joint


  		




  107
  111
  
    
//! \param enum id of joint


  		




  108
  112
  
    
//! \param float value


  		




  109
  113
  
    
void MekaJointInterface::publish_target_position(int e){


  		




  110
  
  
    
    #if 0


  		




  111
  
  
    
    //first: convert humotion enum to our enum:


  		




  112
  
  
    
    int id = convert_enum_to_motorid(e);


  		




  113
  
  
    
    if (id == -1){


  		




  114
  
  
    
        return; //we are not interested in that data, so we just return here


  		




  115
  
  
    
    }


  		




  116
  
  
    

  		




  117
  
  
    
    if (id == ICUB_ID_NECK_PAN){


  		




  118
  
  
    
        //PAN seems to be swapped


  		




  119
  
  
    
        store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);


  		




  120
  
  
    
    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){


  		




  121
  
  
    
        //icub handles eyes differently, we have to set pan angle + vergence


  		




  122
  
  
    
        float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;


  		




  123
  
  
    
        float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);


  		




  124
  
  
    
        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);


  		




  125
  
  
    

  		




  126
  
  
    
        store_joint(ICUB_ID_EYES_PAN, pan);


  		




  127
  
  
    
        store_joint(ICUB_ID_EYES_VERGENCE, vergence);


  		




  128
  
  
    
    }else{


  		




  129
  
  
    
        store_joint(id, joint_target[e]);


  		




  130
  
  
    
    }


  		




  131
  
  
    
    #endif


  		




  
  114
  
    
   //we do this in execute motion for all joints at once...


  		




  132
  115
  
    
}


  		




  133
  116
  
    

  		




  134
  117
  
    

  		












Also available in:  Unified diff