test_glove.cpp
Go to the documentation of this file.
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 }


asr_cyberglove_visualization
Author(s): Heller Florian, Jäkel Rainer, Kasper Alexander, Meißner Pascal, Nguyen Trung, Yi Xie
autogenerated on Thu Jun 6 2019 18:29:01