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


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