coverage_3d_tools: joint_states_listener

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #test client for joint_states_listener
00004 
00005 import roslib
00006 roslib.load_manifest('coverage_3d_tools')
00007 import rospy
00008 from coverage_3d_tools.srv import ReturnJointStates
00009 import time
00010 import sys
00011 
00012 def call_return_joint_states(joint_names):
00013     rospy.wait_for_service("return_joint_states")
00014     try:
00015         s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
00016         resp = s(joint_names)
00017     except rospy.ServiceException, e:
00018         print "error when calling return_joint_states: %s"%e
00019         sys.exit(1)
00020     for (ind, joint_name) in enumerate(joint_names):
00021         if(not resp.found[ind]):
00022             print "joint %s not found!"%joint_name
00023     return (resp.position, resp.velocity, resp.effort)
00024 
00025 
00026 #pretty-print list to string
00027 def pplist(list):
00028     return ','.join(['%2.3f'%x for x in list])
00029 
00030 
00031 #print out the positions, velocities, and efforts of the right arm joints
00032 if __name__ == "__main__":
00033     joint_names_r = ["r_shoulder_pan_joint",
00034                    "r_shoulder_lift_joint",
00035                     "r_upper_arm_roll_joint",
00036                     "r_elbow_flex_joint",
00037                     "r_forearm_roll_joint",
00038                     "r_wrist_flex_joint",
00039                     "r_wrist_roll_joint"]
00040     joint_names_l = ["torso_lift_joint",
00041                     "l_shoulder_pan_joint",
00042                     "l_shoulder_lift_joint",
00043                     "l_upper_arm_roll_joint",
00044                     "l_elbow_flex_joint",
00045                     "l_forearm_roll_joint",
00046                     "l_wrist_flex_joint",
00047                     "l_wrist_roll_joint"]
00048 
00049     while(1):
00050         #print "Left Arm"
00051         #position, velocity, effort) = call_return_joint_states(joint_names_l)
00052         #print "position:", pplist(position)
00053         #print "velocity:", pplist(velocity)
00054         #print "effort:", pplist(effort)
00055 
00056         #print "Right Arm"
00057         (position, velocity, effort) = call_return_joint_states(joint_names_l)
00058         print "double pose[]={", pplist(position),"};"
00059         #print "position:", pplist(position)
00060         #print "velocity:", pplist(velocity)
00061         #print "effort:", pplist(effort)
00062         time.sleep(1)