Revision 6168b08c

View differences:

examples/meka/include/mekajointinterface.h
7 7 
#include "ros/ros.h"
8 8 
#include "sensor_msgs/JointState.h"
9 9 
#include "trajectory_msgs/JointTrajectory.h"
10 
#include <m3meka_msgs/M3ControlStates.h>
10 11 

  		




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


  		




  12
  13
  
    
public:


  		




  13
  
  
    
    MekaJointInterface(std::string _input_scope, std::string _output_scope);


  		




  
  14
  
    
    MekaJointInterface(std::string _input_scope, std::string control_scope, std::string _output_scope);


  		




  14
  15
  
    
    ~MekaJointInterface();


  		




  15
  16
  
    

  		




  16
  17
  
    
    //void fetch_position(Device *dev, double timestamp);


  		




  ......




  28
  29
  
    
    void execute_motion();


  		




  29
  30
  
    

  		




  30
  31
  
    
private:


  		




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


  		




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


  		




  32
  34
  
    
    void store_dummy_data(int id, double timestamp);


  		




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


  		




  34
  36
  
    
    ros::Subscriber joint_state_subscriber;


  		




  
  37
  
    
    ros::Subscriber control_state_subscriber;


  		




  35
  38
  
    
    ros::Publisher target_publisher;


  		




  36
  39
  
    

  		




  37
  40
  
    
    double get_timestamp_s();


  		




  ......




  47
  50
  
    
    int previous_mouth_state;


  		




  48
  51
  
    

  		




  49
  52
  
    
    std::string input_scope;


  		




  
  53
  
    
    std::string control_scope;


  		




  50
  54
  
    
    std::string output_scope;


  		




  51
  55
  
    

  		




  52
  56
  
    
    float last_pos_eye_vergence;


  		




  ......




  54
  58
  
    
    float last_vel_eye_vergence;


  		




  55
  59
  
    
    float last_vel_eye_pan;


  		




  56
  60
  
    

  		




  
  61
  
    
    bool controller_enabled;


  		




  
  62
  
    

  		




  57
  63
  
    
    void store_joint(int id, float value);


  		




  58
  64
  
    
    void set_target_in_positionmode(int id, double value);


  		




  59
  65
  
    
    void set_target_in_velocitymode(int id, double value);


  		












  

    
      examples/meka/src/main.cpp
    
  





  22
  22
  
    

  		




  23
  23
  
    
*/


  		




  24
  24
  
    

  		




  25
  
  
    
    if (argc != 4){


  		




  26
  
  
    
        printf("> ERROR: invalid number of parameters passed to server!\n\nusage: %s <humotion base topic> <jointstates topic> <control topic> (example: %s /meka /joint_states /meka_roscontrol/head_position_trajectory_controller/command)\n\n",argv[0],argv[0]);


  		




  
  25
  
    
    if (argc != 5){


  		




  
  26
  
    
        printf("> ERROR: invalid number of parameters passed to server!\n\n");


  		




  
  27
  
    
        printf("usage   : %s <humotion base topic> <jointstates topic> <controlstates topic> <control output topic>\n\n",argv[0]);


  		




  
  28
  
    
        printf("example : %s /meka /joint_states  /meka_roscontrol_state_manager/state /meka_roscontrol/head_position_trajectory_controller/command)\n\n",argv[0]);


  		




  27
  29
  
    
        exit(EXIT_FAILURE);


  		




  28
  30
  
    
    }


  		




  29
  31
  
    

  		




  30
  32
  
    
    //create humotion interface


  		




  31
  
  
    
    string humotion_scope    = argv[1];


  		




  32
  
  
    
    string jointstates_scope = argv[2];


  		




  33
  
  
    
    string control_scope     = argv[3];


  		




  
  33
  
    
    string humotion_scope      = argv[1];


  		




  
  34
  
    
    string jointstates_scope   = argv[2];


  		




  
  35
  
    
    string controlstates_scope = argv[3];


  		




  
  36
  
    
    string control_scope       = argv[4];


  		




  34
  37
  
    

  		




  35
  38
  
    

  		




  36
  
  
    
    MekaJointInterface *jointinterface = new MekaJointInterface(jointstates_scope, control_scope);


  		




  
  39
  
    
    MekaJointInterface *jointinterface = new MekaJointInterface(jointstates_scope, controlstates_scope, control_scope);


  		




  37
  40
  
    
    humotion::server::Server *humotion_server = new humotion::server::Server(humotion_scope, "ROS", jointinterface);


  		




  38
  41
  
    

  		




  39
  42
  
    
    //finally run it


  		












  

    
      examples/meka/src/mekajointinterface.cpp
    
  





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


  		




  5
  5
  
    
#define POSITION_CONTROL 1


  		




  6
  6
  
    

  		




  
  7
  
    
void MekaJointInterface::incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state){


  		




  
  8
  
    
    //incoming controller status


  		




  
  9
  
    
    for (unsigned int i = 0; i<control_state.group_name.size(); i++){


  		




  
  10
  
    
        if (control_state.group_name[i] == "head"){


  		




  
  11
  
    
            //enable/disable based on controller status


  		




  
  12
  
    
            if (control_state.state[i] == m3meka_msgs::M3ControlStates::START){


  		




  
  13
  
    
                //controller up and running


  		




  
  14
  
    
                if (!controller_enabled){


  		




  
  15
  
    
                    printf("> incoming control state (%d), enabling jointstate output\n",control_state.state[i]);


  		




  
  16
  
    
                    controller_enabled = true;


  		




  
  17
  
    
                }


  		




  
  18
  
    
            }else{


  		




  
  19
  
    
                //controller in estop/stopped etc


  		




  
  20
  
    
                if (controller_enabled){


  		




  
  21
  
    
                    printf("> incoming control state (%d), DISABLING jointstate output\n",control_state.state[i]);


  		




  
  22
  
    
                    controller_enabled = false;


  		




  
  23
  
    
                }


  		




  
  24
  
    
            }


  		




  
  25
  
    
        }


  		




  
  26
  
    
    }


  		




  
  27
  
    
}


  		




  
  28
  
    

  		




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


  		




  8
  30
  
    
    //fetch current timestamp


  		




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


  		




  ......




  75
  97
  
    
}


  		




  76
  98
  
    

  		




  77
  99
  
    
//! constructor


  		




  78
  
  
    
MekaJointInterface::MekaJointInterface(string _input_scope, string _output_scope) : humotion::server::JointInterface(){


  		




  
  100
  
    
MekaJointInterface::MekaJointInterface(string _input_scope, string control_scope, string _output_scope) : humotion::server::JointInterface(){


  		




  79
  101
  
    
    input_scope = _input_scope;


  		




  80
  102
  
    
    output_scope = _output_scope;


  		




  81
  103
  
    

  		




  
  104
  
    
    controller_enabled = false;


  		




  
  105
  
    

  		




  82
  106
  
    
    //subscribe to meka joint states


  		




  83
  107
  
    
    int argc = 0;


  		




  84
  108
  
    
    ros::init(argc, (char**)NULL, "meka_humotion");


  		




  85
  109
  
    
    ros::NodeHandle n;


  		




  86
  110
  
    

  		




  87
  111
  
    
    printf("> listening on jointstates on '%s'\n",input_scope.c_str());


  		




  88
  
  
    
    joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates , this);


  		




  
  112
  
    
    joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates, this);


  		




  
  113
  
    

  		




  
  114
  
    
    printf("> listening on controlstates on '%s'\n",control_scope.c_str());


  		




  
  115
  
    
    control_state_subscriber = n.subscribe(control_scope, 150, &MekaJointInterface::incoming_controlstate, this);


  		




  89
  116
  
    

  		




  90
  117
  
    
    printf("> sending targets on '%s'\n", output_scope.c_str());


  		




  91
  118
  
    
    target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(output_scope, 100);


  		




  ......




  121
  148
  
    

  		




  122
  149
  
    
//! actually execute the scheduled motion commands


  		




  123
  150
  
    
void MekaJointInterface::execute_motion(){


  		




  124
  
  
    
    //build msg


  		




  125
  
  
    
    trajectory_msgs::JointTrajectory msg;


  		




  126
  
  
    
    msg.joint_names.push_back("head_j0");


  		




  127
  
  
    
    msg.joint_names.push_back("head_j1");


  		




  128
  
  
    

  		




  129
  
  
    
    trajectory_msgs::JointTrajectoryPoint p;


  		




  130
  
  
    
    p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);


  		




  131
  
  
    
    //pan joint is inverted!


  		




  132
  
  
    
    p.positions.push_back(-joint_target[ID_NECK_PAN] * M_PI / 180.0);


  		




  133
  
  
    
    //printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);


  		




  134
  
  
    

  		




  135
  
  
    
    p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);


  		




  136
  
  
    

  		




  137
  
  
    
    msg.points.push_back(p);


  		




  138
  
  
    

  		




  139
  
  
    
    target_publisher.publish(msg);


  		




  140
  
  
    

  		




  141
  
  
    
    /*


  		




  142
  
  
    
     void MekaJointInterface::store_min_max(int id, float min, float max){


  		




  143
  
  
    
header:


  		




  144
  
  
    
  seq: 636


  		




  145
  
  
    
  stamp:


  		




  146
  
  
    
    secs: 0


  		




  147
  
  
    
    nsecs: 0


  		




  148
  
  
    
  frame_id: ''


  		




  149
  
  
    
joint_names: ['head_j0', 'head_j1']


  		




  150
  
  
    
points:


  		




  151
  
  
    
  -


  		




  152
  
  
    
    positions: [-0.31, 0.01954768762234005]


  		




  153
  
  
    
    velocities: []


  		




  154
  
  
    
    accelerations: []


  		




  155
  
  
    
    effort: []


  		




  156
  
  
    
    time_from_start:


  		




  157
  
  
    
      secs: 1


  		




  158
  
  
    
      nsecs: 0


  		




  159
  
  
    

  		




  160
  
  
    
     */


  		




  161
  
  
    

  		




  162
  
  
    
    #if 0


  		




  163
  
  
    
    // set up neck and eye motion commands:


  		




  164
  
  
    
    if (POSITION_CONTROL){


  		




  165
  
  
    
        //position control


  		




  166
  
  
    
        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){


  		




  167
  
  
    
            set_target_in_positionmode(i, target_angle[i]);


  		




  168
  
  
    
        }


  		




  169
  
  
    
    }else{


  		




  170
  
  
    
        //velocity control


  		




  171
  
  
    
        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){


  		




  172
  
  
    
            set_target_in_velocitymode(i, target_angle[i]);


  		




  173
  
  
    
        }


  		




  174
  
  
    
    }


  		




  175
  
  
    
    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);


  		




  176
  
  
    

  		




  
  151
  
    
    if (controller_enabled){


  		




  
  152
  
    
        //build msg


  		




  
  153
  
    
        trajectory_msgs::JointTrajectory msg;


  		




  
  154
  
    
        msg.joint_names.push_back("head_j0");


  		




  
  155
  
    
        msg.joint_names.push_back("head_j1");


  		




  177
  156
  
    

  		




  178
  
  
    
    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:


  		




  179
  
  
    
    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);


  		




  
  157
  
    
        trajectory_msgs::JointTrajectoryPoint p;


  		




  
  158
  
    
        p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);


  		




  
  159
  
    
        //pan joint is inverted!


  		




  
  160
  
    
        p.positions.push_back(-joint_target[ID_NECK_PAN] * M_PI / 180.0);


  		




  
  161
  
    
        //printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);


  		




  180
  162
  
    

  		




  181
  
  
    
    //eyebrows are set using a special command as well:


  		




  182
  
  
    
    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);


  		




  183
  
  
    
    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);


  		




  
  163
  
    
        p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);


  		




  184
  164
  
    

  		




  185
  
  
    
    //mouth


  		




  186
  
  
    
    set_mouth();


  		




  
  165
  
    
        msg.points.push_back(p);


  		




  187
  166
  
    

  		




  188
  
  
    
    #endif


  		




  
  167
  
    
        target_publisher.publish(msg);


  		




  
  168
  
    
    }


  		




  189
  169
  
    
}


  		




  190
  170
  
    

  		




  191
  171
  
    

  		












Also available in:  Unified diff