Revision 6353defa

View differences:

examples/meka/include/mekajointinterface.h
60 60 
    ros::Publisher target_publisher;
61 61 

  		




  62
  62
  
    

  		




  63
  
  
    
    double get_timestamp_ms();


  		




  
  63
  
    
    double get_timestamp_s();


  		




  64
  64
  
    
    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];


  		




  65
  65
  
    
    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];


  		




  66
  66
  
    

  		












  

    
      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
  
    

  		












  

    
      include/humotion/server/middleware_ros.h
    
  





  59
  59
  
    
    void tick();


  		




  60
  60
  
    

  		




  61
  61
  
    
private:


  		




  62
  
  
    
    double convert_ros_to_timestamp_ms(ros::Time t);


  		




  63
  62
  
    
    bool tick_necessary;


  		




  64
  63
  
    
    void incoming_mouth_target(const humotion::mouth::ConstPtr& msg);


  		




  65
  64
  
    
    void incoming_gaze_target(const humotion::gaze::ConstPtr& msg);


  		












  

    
      src/server/middleware_ros.cpp
    
  





  98
  98
  
    
    controller->set_mouth_target(mouth_state);


  		




  99
  99
  
    
}


  		




  100
  100
  
    

  		




  101
  
  
    
//! this provides a consistent transformation of ros ts to double


  		




  102
  
  
    
double MiddlewareROS::convert_ros_to_timestamp_ms(ros::Time t){


  		




  103
  
  
    
        return t.sec + t.nsec/1000000000.0;


  		




  104
  
  
    
}


  		




  105
  101
  
    

  		




  106
  102
  
    
//! callback to handle incoming gaze  target


  		




  107
  103
  
    
void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){


  		




  ......




  131
  127
  
    
    }else{


  		




  132
  128
  
    
        gaze_state.gaze_type = GazeState::GAZETYPE_RELATIVE;


  		




  133
  129
  
    
    }


  		




  134
  
  
    
    gaze_state.timestamp = convert_ros_to_timestamp_ms(msg->header.stamp);


  		




  
  130
  
    
    gaze_state.timestamp = msg->header.stamp.toSec();


  		




  135
  131
  
    

  		




  136
  132
  
    
    controller->set_gaze_target(gaze_state);


  		




  137
  133
  
    

  		












Also available in:  Unified diff