Revision a4795834 examples/meka/src/mekajointinterface.cpp
| examples/meka/src/mekajointinterface.cpp | ||
|---|---|---|
| 5 | 5 |
#define POSITION_CONTROL 1 |
| 6 | 6 |
|
| 7 | 7 |
void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){
|
| 8 |
//joint_pos_global = msg.position[3]; |
|
| 9 |
cout << msg; |
|
| 8 |
//fetch current timestamp |
|
| 9 |
double timestamp = get_timestamp_ms(); |
|
| 10 |
|
|
| 11 |
//iterate through incoming joints and filter out joints we need: |
|
| 12 |
for(int i=0; i<msg.name.size(); i++){
|
|
| 13 |
string name = msg.name[i]; |
|
| 14 |
//printf("incoming data for joint '%s'\n", name.c_str());
|
|
| 15 |
|
|
| 16 |
int id = -1; |
|
| 17 |
if (name == "NECK_PAN"){
|
|
| 18 |
id = ID_NECK_PAN; |
|
| 19 |
}else if(name == "NECK_TILT"){
|
|
| 20 |
id = ID_NECK_TILT; |
|
| 21 |
} |
|
| 22 |
|
|
| 23 |
//store data: |
|
| 24 |
if (id != -1){
|
|
| 25 |
//printf("> storing joint data for joint id %d\n", id);
|
|
| 26 |
JointInterface::store_incoming_position(id, msg.position[i], timestamp); |
|
| 27 |
JointInterface::store_incoming_speed( id, msg.velocity[i], timestamp); |
|
| 28 |
} |
|
| 29 |
} |
|
| 30 |
|
|
| 31 |
//store dummy positions for joints we do not know about: |
|
| 32 |
store_dummy_data(ID_LIP_LEFT_UPPER, timestamp); |
|
| 33 |
store_dummy_data(ID_LIP_LEFT_LOWER, timestamp); |
|
| 34 |
store_dummy_data(ID_LIP_CENTER_UPPER, timestamp); |
|
| 35 |
store_dummy_data(ID_LIP_CENTER_LOWER, timestamp); |
|
| 36 |
store_dummy_data(ID_LIP_RIGHT_UPPER, timestamp); |
|
| 37 |
store_dummy_data(ID_LIP_RIGHT_LOWER, timestamp); |
|
| 38 |
|
|
| 39 |
store_dummy_data(ID_NECK_ROLL, timestamp); |
|
| 40 |
store_dummy_data(ID_EYES_BOTH_UD, timestamp); |
|
| 41 |
store_dummy_data(ID_EYES_LEFT_LR, timestamp); |
|
| 42 |
store_dummy_data(ID_EYES_RIGHT_LR, timestamp); |
|
| 43 |
store_dummy_data(ID_EYES_LEFT_LID_LOWER, timestamp); |
|
| 44 |
store_dummy_data(ID_EYES_LEFT_LID_UPPER, timestamp); |
|
| 45 |
store_dummy_data(ID_EYES_LEFT_BROW, timestamp); |
|
| 46 |
store_dummy_data(ID_EYES_RIGHT_LID_LOWER, timestamp); |
|
| 47 |
store_dummy_data(ID_EYES_RIGHT_LID_UPPER, timestamp); |
|
| 48 |
store_dummy_data(ID_EYES_RIGHT_BROW, timestamp); |
|
| 49 |
|
|
| 50 |
} |
|
| 51 |
|
|
| 52 |
void MekaJointInterface::store_dummy_data(int id, double timestamp){
|
|
| 53 |
JointInterface::store_incoming_position(id, 0.0, timestamp); |
|
| 54 |
JointInterface::store_incoming_speed(id, 0.0, timestamp); |
|
| 10 | 55 |
} |
| 11 | 56 |
|
| 12 | 57 |
//! constructor |
| 13 | 58 |
MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
|
| 14 | 59 |
scope = _scope; |
| 60 |
string js_topic = scope + "joint_states"; |
|
| 61 |
string target_topic = scope + "meka_roscontrol/head_position_trajectory_controller/command"; |
|
| 15 | 62 |
|
| 16 | 63 |
//subscribe to meka joint states |
| 17 | 64 |
int argc = 0; |
| 18 | 65 |
ros::init(argc, (char**)NULL, "meka_humotion"); |
| 19 | 66 |
ros::NodeHandle n; |
| 20 |
joint_state_subscriber = n.subscribe(scope + "joint_states", 150, &MekaJointInterface::incoming_jointstates , this); |
|
| 21 | 67 |
|
| 22 | 68 |
|
| 23 |
//add mapping from ids to enums: |
|
| 24 |
//this might look strange at the first sight but we need to have a generic |
|
| 25 |
//way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids |
|
| 26 |
//to access the joints. now we need to define a mapping to map those to our motor ids. |
|
| 27 |
//this is what we use the enum bimap for (convertion fro/to motorid is handled |
|
| 28 |
//by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron |
|
| 29 |
/* |
|
| 30 |
//MOUTH |
|
| 31 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER, ID_LIP_LEFT_UPPER)); |
|
| 32 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER, ID_LIP_LEFT_LOWER)); |
|
| 33 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER)); |
|
| 34 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER)); |
|
| 35 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER, ID_LIP_RIGHT_UPPER)); |
|
| 36 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER, ID_LIP_RIGHT_LOWER)); |
|
| 37 |
|
|
| 38 |
//NECK |
|
| 39 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN, ID_NECK_PAN)); |
|
| 40 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT, ID_NECK_TILT)); |
|
| 41 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL, ID_NECK_ROLL)); |
|
| 42 |
|
|
| 43 |
//EYES |
|
| 44 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LR, ID_EYES_LEFT_LR)); |
|
| 45 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LR, ID_EYES_RIGHT_LR)); |
|
| 46 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD)); |
|
| 47 |
|
|
| 48 |
//EYELIDS |
|
| 49 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER)); |
|
| 50 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER)); |
|
| 51 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW)); |
|
| 52 |
|
|
| 53 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER)); |
|
| 54 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER)); |
|
| 55 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW)); |
|
| 56 |
|
|
| 57 |
Property options; |
|
| 58 |
options.put("device", "remote_controlboard");
|
|
| 59 |
options.put("local", "/local/head");
|
|
| 60 |
options.put("remote", scope+"/head");
|
|
| 61 |
dd.open(options); |
|
| 62 |
|
|
| 63 |
//fetch views: |
|
| 64 |
dd.view(iencs); |
|
| 65 |
dd.view(ipos); |
|
| 66 |
dd.view(ivel); |
|
| 67 |
dd.view(ilimits); |
|
| 68 |
|
|
| 69 |
if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){
|
|
| 70 |
printf("> ERROR: failed to open icub views\n");
|
|
| 71 |
exit(EXIT_FAILURE); |
|
| 72 |
} |
|
| 69 |
printf("> listening on jointstates on '%s'\n",js_topic.c_str());
|
|
| 70 |
joint_state_subscriber = n.subscribe(js_topic, 150, &MekaJointInterface::incoming_jointstates , this); |
|
| 71 |
|
|
| 72 |
printf("> sending targets on '%s'\n", target_topic.c_str());
|
|
| 73 |
target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(target_topic, 100); |
|
| 73 | 74 |
|
| 74 |
int joints; |
|
| 75 | 75 |
|
| 76 | 76 |
//tell humotion about min/max joint values: |
| 77 | 77 |
init_joints(); |
| 78 |
|
|
| 79 |
iencs->getAxes(&joints); |
|
| 80 |
positions.resize(joints); |
|
| 81 |
velocities.resize(joints); |
|
| 82 |
commands.resize(joints); |
|
| 83 |
|
|
| 84 |
//set position mode: |
|
| 85 |
if (POSITION_CONTROL){
|
|
| 86 |
commands=200000.0; |
|
| 87 |
ipos->setRefAccelerations(commands.data()); |
|
| 88 |
ipos->setPositionMode(); |
|
| 89 |
}else{
|
|
| 90 |
ivel->setVelocityMode(); |
|
| 91 |
commands=1000000; |
|
| 92 |
ivel->setRefAccelerations(commands.data()); |
|
| 93 |
} |
|
| 94 |
|
|
| 95 |
//attach to facial expressions: |
|
| 96 |
string emotion_scope = scope + "/face/raw/in"; |
|
| 97 |
printf("> opening connection to %s\n", emotion_scope.c_str());
|
|
| 98 |
|
|
| 99 |
for(int i=0; i<4; i++){
|
|
| 100 |
//strange, if we use one output port only the first command is executed?! flushing issues? |
|
| 101 |
string emotion_port_out = "/emotionwriter" + to_string(i); |
|
| 102 |
if (!emotion_port[i].open(emotion_port_out.c_str())){
|
|
| 103 |
printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());
|
|
| 104 |
exit(EXIT_FAILURE); |
|
| 105 |
} |
|
| 106 |
if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())){
|
|
| 107 |
printf("> ERROR: failed to connect emotion ports\n");
|
|
| 108 |
exit(EXIT_FAILURE); |
|
| 109 |
} |
|
| 110 |
} |
|
| 111 |
*/ |
|
| 112 | 78 |
} |
| 113 | 79 |
|
| 114 | 80 |
//! destructor |
| ... | ... | |
| 120 | 86 |
void MekaJointInterface::run(){
|
| 121 | 87 |
//iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this); |
| 122 | 88 |
//data_receiver->start(); |
| 89 |
ros::spin(); |
|
| 123 | 90 |
} |
| 124 | 91 |
|
| 125 | 92 |
|
| 93 |
double MekaJointInterface::get_timestamp_ms(){
|
|
| 94 |
struct timespec spec; |
|
| 95 |
clock_gettime(CLOCK_REALTIME, &spec); |
|
| 96 |
return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6; |
|
| 97 |
} |
|
| 98 |
|
|
| 126 | 99 |
//! set the target position of a joint |
| 127 | 100 |
//! \param enum id of joint |
| 128 | 101 |
//! \param float value |
| ... | ... | |
| 154 | 127 |
|
| 155 | 128 |
//! actually execute the scheduled motion commands |
| 156 | 129 |
void MekaJointInterface::execute_motion(){
|
| 130 |
//build msg |
|
| 131 |
trajectory_msgs::JointTrajectory msg; |
|
| 132 |
msg.joint_names.push_back("head_j0");
|
|
| 133 |
msg.joint_names.push_back("head_j1");
|
|
| 134 |
|
|
| 135 |
trajectory_msgs::JointTrajectoryPoint p[2]; |
|
| 136 |
p[0].positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0); |
|
| 137 |
p[1].positions.push_back(joint_target[ID_NECK_TILT]* M_PI / 180.0); |
|
| 138 |
|
|
| 139 |
p[0].time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE); |
|
| 140 |
p[1].time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE); |
|
| 141 |
|
|
| 142 |
msg.points.push_back(p[0]); |
|
| 143 |
msg.points.push_back(p[1]); |
|
| 144 |
|
|
| 145 |
target_publisher.publish(msg); |
|
| 146 |
|
|
| 147 |
/* |
|
| 148 |
void MekaJointInterface::store_min_max(int id, float min, float max){
|
|
| 149 |
header: |
|
| 150 |
seq: 636 |
|
| 151 |
stamp: |
|
| 152 |
secs: 0 |
|
| 153 |
nsecs: 0 |
|
| 154 |
frame_id: '' |
|
| 155 |
joint_names: ['head_j0', 'head_j1'] |
|
| 156 |
points: |
|
| 157 |
- |
|
| 158 |
positions: [-0.31, 0.01954768762234005] |
|
| 159 |
velocities: [] |
|
| 160 |
accelerations: [] |
|
| 161 |
effort: [] |
|
| 162 |
time_from_start: |
|
| 163 |
secs: 1 |
|
| 164 |
nsecs: 0 |
|
| 165 |
|
|
| 166 |
*/ |
|
| 167 |
|
|
| 157 | 168 |
#if 0 |
| 158 | 169 |
// set up neck and eye motion commands: |
| 159 | 170 |
if (POSITION_CONTROL){
|
| ... | ... | |
| 252 | 263 |
*/ |
| 253 | 264 |
} |
| 254 | 265 |
|
| 266 |
void MekaJointInterface::store_min_max(int id, float min, float max){
|
|
| 267 |
joint_min[id] = min; |
|
| 268 |
joint_max[id] = max; |
|
| 269 |
} |
|
| 270 |
|
|
| 271 |
void MekaJointInterface::init_joints(){
|
|
| 272 |
store_min_max(ID_NECK_TILT, -10, 10); |
|
| 273 |
store_min_max(ID_NECK_PAN, -10, 10); |
|
| 274 |
|
|
| 275 |
store_min_max(ID_NECK_ROLL, -1, 1); |
|
| 276 |
store_min_max(ID_EYES_BOTH_UD, -1, 1); |
|
| 277 |
store_min_max(ID_EYES_LEFT_LR, -1, 1); |
|
| 278 |
store_min_max(ID_EYES_RIGHT_LR, -1, 1); |
|
| 279 |
store_min_max(ID_EYES_LEFT_LID_UPPER, -1, 1); |
|
| 280 |
store_min_max(ID_EYES_LEFT_LID_LOWER, -1, 1); |
|
| 281 |
store_min_max(ID_EYES_RIGHT_LID_UPPER, -1, 1); |
|
| 282 |
store_min_max(ID_EYES_RIGHT_LID_LOWER, -1, 1); |
|
| 283 |
store_min_max(ID_EYES_LEFT_BROW, -1, 1); |
|
| 284 |
store_min_max(ID_EYES_RIGHT_BROW, -1, 1); |
|
| 285 |
store_min_max(ID_LIP_CENTER_UPPER, -1, 1); |
|
| 286 |
store_min_max(ID_LIP_CENTER_LOWER, -1, 1); |
|
| 287 |
store_min_max(ID_LIP_LEFT_UPPER, -1, 1); |
|
| 288 |
store_min_max(ID_LIP_LEFT_LOWER, -1, 1); |
|
| 289 |
store_min_max(ID_LIP_RIGHT_UPPER, -1, 1); |
|
| 290 |
store_min_max(ID_LIP_RIGHT_LOWER, -1, 1); |
|
| 291 |
} |
|
| 292 |
|
|
| 255 | 293 |
#if 0 |
| 256 | 294 |
|
| 257 | 295 |
//! conversion table for humotion motor ids to our ids: |
Also available in: Unified diff