00001 00018 #include "ros/ros.h" 00019 #include "asr_msgs/AsrObject.h" 00020 #include <sensor_msgs/JointState.h> 00021 #include <tf/transform_broadcaster.h> 00022 00023 ros::Publisher pub_states; 00024 sensor_msgs::JointState joint_state; 00025 00026 ros::Subscriber sub_tf; 00027 tf::TransformBroadcaster *pub_tf; 00028 geometry_msgs::TransformStamped odom_trans; 00029 00034 void callback_tf(const asr_msgs::AsrObjectConstPtr& msg) 00035 { 00036 if (msg->identifier == "tracker_right") 00037 { 00038 odom_trans.header.stamp = ros::Time::now(); 00039 00040 if(!msg->sampledPoses.size()) 00041 { 00042 std::cerr << "Got a AsrObject without poses." << std::endl; 00043 std::exit(1); 00044 } 00045 00046 geometry_msgs::Pose current_pose = msg->sampledPoses.front().pose; 00047 geometry_msgs::Point position = current_pose.position; 00048 00049 odom_trans.transform.translation.x = position.x; 00050 odom_trans.transform.translation.y = position.y; 00051 odom_trans.transform.translation.z = position.z; 00052 odom_trans.transform.rotation = current_pose.orientation; 00053 00054 pub_tf->sendTransform(odom_trans); 00055 00056 // send 0-pose for hand model 00057 joint_state.header.stamp = ros::Time::now(); 00058 pub_states.publish(joint_state); 00059 } 00060 } 00061 00062 int main(int argc, char **argv) 00063 { 00064 ros::init(argc, argv, "test_fob"); 00065 ros::NodeHandle node; 00066 00067 sub_tf = node.subscribe("fob_objects", 1, callback_tf); 00068 pub_tf = new tf::TransformBroadcaster(); 00069 00070 odom_trans.header.frame_id = "odom"; 00071 odom_trans.child_frame_id = "fob_sensor"; 00072 00073 pub_states = node.advertise<sensor_msgs::JointState>("joint_states", 1); 00074 00075 joint_state.name.resize(22); 00076 joint_state.position.resize(22); 00077 00078 // the names are set accordingly to the names used in the xml model 00079 joint_state.name[0] ="wr_j0"; 00080 joint_state.name[1] ="wr_j1"; 00081 00082 joint_state.name[2] ="th_j0"; 00083 joint_state.name[3] ="th_j1"; 00084 joint_state.name[4] ="th_j2"; 00085 joint_state.name[5] ="th_j3"; 00086 00087 joint_state.name[6] ="in_j1"; 00088 joint_state.name[7] ="in_j0"; 00089 joint_state.name[8] ="in_j2"; 00090 joint_state.name[9] ="in_j3"; 00091 00092 joint_state.name[10] ="mi_j1"; 00093 joint_state.name[11] ="mi_j0"; 00094 joint_state.name[12] ="mi_j2"; 00095 joint_state.name[13] ="mi_j3"; 00096 00097 joint_state.name[14] ="ri_j1"; 00098 joint_state.name[15] ="ri_j0"; 00099 joint_state.name[16] ="ri_j2"; 00100 joint_state.name[17] ="ri_j3"; 00101 00102 joint_state.name[18] ="pi_j1"; 00103 joint_state.name[19] ="pi_j0"; 00104 joint_state.name[20] ="pi_j2"; 00105 joint_state.name[21] ="pi_j3"; 00106 00107 // 0-pose for hand model, since only the asr_flock_of_birds tracker is tested 00108 joint_state.position[0] = 0; 00109 joint_state.position[1] = 0; 00110 joint_state.position[2] = 0; 00111 joint_state.position[3] = 0; 00112 joint_state.position[4] = 0; 00113 joint_state.position[5] = 0; 00114 joint_state.position[6] = 0; 00115 joint_state.position[7] = 0; 00116 joint_state.position[8] = 0; 00117 joint_state.position[9] = 0; 00118 joint_state.position[10] = 0; 00119 joint_state.position[11] = 0; 00120 joint_state.position[12] = 0; 00121 joint_state.position[13] = 0; 00122 joint_state.position[14] = 0; 00123 joint_state.position[15] = 0; 00124 joint_state.position[16] = 0; 00125 joint_state.position[17] = 0; 00126 joint_state.position[18] = 0; 00127 joint_state.position[19] = 0; 00128 joint_state.position[20] = 0; 00129 joint_state.position[21] = 0; 00130 00131 ros::spin(); 00132 00133 return 0; 00134 }