get_arm_joint_state.cpp
Go to the documentation of this file.
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pr2_handy_tools
Author(s): Adam Leeper
autogenerated on Wed Jan 23 2013 17:01:17