00001 00018 #include "ros/ros.h" 00019 #include "asr_msgs/AsrGlove.h" 00020 #include "asr_msgs/AsrObject.h" 00021 #include <sensor_msgs/JointState.h> 00022 #include <tf/transform_broadcaster.h> 00023 00024 ros::Subscriber sub_states; 00025 ros::Publisher pub_states; 00026 sensor_msgs::JointState joint_state; 00027 00032 void callback(const asr_msgs::AsrGloveConstPtr& msg) 00033 { 00034 joint_state.header.stamp = ros::Time::now(); 00035 00036 // wrist: not needed because used in combination with asr_flock_of_birds 00037 joint_state.position[0] = 0; 00038 joint_state.position[1] = 0; 00039 00040 // some values are manually adjusted since its difficult to calibrate 00041 // TODO: correct mapping of thumb joints 00042 joint_state.position[2] = 0.17; 00043 joint_state.position[3] = 0.02; 00044 joint_state.position[4] = 0.17; 00045 joint_state.position[5] = msg->data[5]; 00046 00047 joint_state.position[6] = msg->data[6]; 00048 joint_state.position[7] = msg->data[7]; 00049 joint_state.position[8] = msg->data[8]; 00050 joint_state.position[9] = msg->data[9] * (-1); 00051 00052 joint_state.position[10] = msg->data[10] - 0.1; 00053 joint_state.position[11] = msg->data[11]; 00054 joint_state.position[12] = msg->data[12]; 00055 joint_state.position[13] = msg->data[13] - 0.5; 00056 00057 joint_state.position[14] = msg->data[14]; 00058 joint_state.position[15] = msg->data[15] - 0.15; 00059 joint_state.position[16] = msg->data[16]; 00060 joint_state.position[17] = msg->data[17] - 0.5; 00061 00062 joint_state.position[18] = msg->data[18] - 0.3; 00063 joint_state.position[19] = msg->data[19] - 0.3; 00064 joint_state.position[20] = msg->data[20]; 00065 joint_state.position[21] = msg->data[21] * (-1) + 0.1; 00066 00067 pub_states.publish(joint_state); 00068 } 00069 00070 int main(int argc, char **argv) 00071 { 00072 ros::init(argc, argv, "test_glove"); 00073 ros::NodeHandle node; 00074 00075 sub_states = node.subscribe("rightGloveData_radian", 1, callback); 00076 pub_states = node.advertise<sensor_msgs::JointState>("joint_states", 1); 00077 00078 joint_state.name.resize(22); 00079 joint_state.position.resize(22); 00080 00081 // the names are set accordingly to the names used in the xml model 00082 joint_state.name[0] ="wr_j0"; 00083 joint_state.name[1] ="wr_j1"; 00084 00085 joint_state.name[2] ="th_j0"; 00086 joint_state.name[3] ="th_j1"; 00087 joint_state.name[4] ="th_j2"; 00088 joint_state.name[5] ="th_j3"; 00089 00090 joint_state.name[6] ="in_j1"; 00091 joint_state.name[7] ="in_j0"; 00092 joint_state.name[8] ="in_j2"; 00093 joint_state.name[9] ="in_j3"; 00094 00095 joint_state.name[10] ="mi_j1"; 00096 joint_state.name[11] ="mi_j0"; 00097 joint_state.name[12] ="mi_j2"; 00098 joint_state.name[13] ="mi_j3"; 00099 00100 joint_state.name[14] ="ri_j1"; 00101 joint_state.name[15] ="ri_j0"; 00102 joint_state.name[16] ="ri_j2"; 00103 joint_state.name[17] ="ri_j3"; 00104 00105 joint_state.name[18] ="pi_j1"; 00106 joint_state.name[19] ="pi_j0"; 00107 joint_state.name[20] ="pi_j2"; 00108 joint_state.name[21] ="pi_j3"; 00109 00110 ros::spin(); 00111 00112 return 0; 00113 }