$search
00001 #include <planning_environment/monitors/joint_state_monitor.h> 00002 #include <boost/thread.hpp> 00003 00004 void spinThread() 00005 { 00006 ros::spin(); 00007 } 00008 00009 int main(int argc, char** argv) 00010 { 00011 ros::init(argc, argv, "joint_state_monitor"); 00012 00013 planning_environment::JointStateMonitor joint_state_monitor; 00014 00015 std::vector<std::string> joint_names; 00016 joint_names.resize(7); 00017 00018 if(argc > 1 && *(argv+1)[0] == 'l'){ 00019 joint_names[0] = "l_shoulder_pan_joint"; 00020 joint_names[1] = "l_shoulder_lift_joint"; 00021 joint_names[2] = "l_upper_arm_roll_joint"; 00022 joint_names[3] = "l_elbow_flex_joint"; 00023 joint_names[4] = "l_forearm_roll_joint"; 00024 joint_names[5] = "l_wrist_flex_joint"; 00025 joint_names[6] = "l_wrist_roll_joint"; 00026 ROS_INFO("Printing joint states for left arm:\n"); 00027 } 00028 else if(argc > 1 && *(argv+1)[0] == 'r'){ 00029 joint_names[0] = "r_shoulder_pan_joint"; 00030 joint_names[1] = "r_shoulder_lift_joint"; 00031 joint_names[2] = "r_upper_arm_roll_joint"; 00032 joint_names[3] = "r_elbow_flex_joint"; 00033 joint_names[4] = "r_forearm_roll_joint"; 00034 joint_names[5] = "r_wrist_flex_joint"; 00035 joint_names[6] = "r_wrist_roll_joint"; 00036 ROS_INFO("Printing joint states for right arm:\n"); 00037 } 00038 else 00039 00040 { 00041 printf("Prints joint states for PR2 arms at 1 Hz. \n"); 00042 printf("Usage: get_joint_states ARM\n"); 00043 printf(" ARM: 'l' for left arm, 'r' for right arm.\n"); 00044 exit(0); 00045 return 0; 00046 } 00047 00048 boost::thread spin_thread(&spinThread); 00049 00050 for(int i = 0; i< joint_names.size(); i++) 00051 printf("%s ",joint_names[i].c_str()); 00052 printf("\n"); 00053 00054 while(ros::ok()) 00055 { 00056 sensor_msgs::JointState joint_state; 00057 joint_state = joint_state_monitor.getJointState(joint_names); 00058 00059 //for(unsigned int i=0; i < joint_state.position.size(); i++) 00060 { 00061 //ROS_INFO("JointState: %s, %f",joint_state.name[i].c_str(), joint_state.position[i]); 00062 ROS_INFO("%f, %f, %f, %f, %f, %f, %f\n ", 00063 joint_state.position[0], 00064 joint_state.position[1], 00065 joint_state.position[2], 00066 joint_state.position[3], 00067 joint_state.position[4], 00068 joint_state.position[5], 00069 joint_state.position[6]); 00070 } 00071 ros::Duration(1.0).sleep(); 00072 } 00073 ros::shutdown(); 00074 spin_thread.join(); 00075 return(0); 00076 }