get_arm_joint_state.cpp

Go to the documentation of this file.

#include <planning_environment/monitors/joint_state_monitor.h>
#include <boost/thread.hpp>

void spinThread()
{
  ros::spin();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "joint_state_monitor");
  
  planning_environment::JointStateMonitor joint_state_monitor;

  std::vector<std::string> joint_names;
  joint_names.resize(7);

  if(argc > 1 && *(argv+1)[0] == 'l'){
    joint_names[0] = "l_shoulder_pan_joint";
    joint_names[1] = "l_shoulder_lift_joint";
    joint_names[2] = "l_upper_arm_roll_joint";
    joint_names[3] = "l_elbow_flex_joint";
    joint_names[4] = "l_forearm_roll_joint";
    joint_names[5] = "l_wrist_flex_joint";
    joint_names[6] = "l_wrist_roll_joint";
    ROS_INFO("Printing joint states for left arm:\n");
  }
  else if(argc > 1 && *(argv+1)[0] == 'r'){
    joint_names[0] = "r_shoulder_pan_joint";
    joint_names[1] = "r_shoulder_lift_joint";
    joint_names[2] = "r_upper_arm_roll_joint";
    joint_names[3] = "r_elbow_flex_joint";
    joint_names[4] = "r_forearm_roll_joint";
    joint_names[5] = "r_wrist_flex_joint";
    joint_names[6] = "r_wrist_roll_joint";
    ROS_INFO("Printing joint states for right arm:\n");
  }
  else

  {
    printf("Prints joint states for PR2 arms at 1 Hz. \n");
    printf("Usage: get_joint_states ARM\n");
    printf(" ARM: 'l' for left arm, 'r' for right arm.\n");
    exit(0);
    return 0;
  }
  
  boost::thread spin_thread(&spinThread);
  
  for(int i = 0; i< joint_names.size(); i++)
    printf("%s ",joint_names[i].c_str());
  printf("\n");

  while(ros::ok())  
  {
    sensor_msgs::JointState joint_state;
    joint_state = joint_state_monitor.getJointState(joint_names);
    
    //for(unsigned int i=0; i < joint_state.position.size(); i++)
    {
      //ROS_INFO("JointState: %s, %f",joint_state.name[i].c_str(), joint_state.position[i]);
      ROS_INFO("%f, %f, %f, %f, %f, %f, %f\n ", 
        joint_state.position[0],
        joint_state.position[1],
        joint_state.position[2],
        joint_state.position[3],
        joint_state.position[4],
        joint_state.position[5],
        joint_state.position[6]);
    }
    ros::Duration(1.0).sleep();
  }
  ros::shutdown();
  spin_thread.join();
  return(0);
}