00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 from __future__ import division
00042 import roslib
00043 roslib.load_manifest('pr2_gripper_reactive_approach')
00044 import rospy
00045 from sensor_msgs.msg import JointState
00046 import threading
00047 import time
00048 import sys
00049
00050
00051 class LatestJointStates:
00052
00053 def __init__(self, run_server = 0):
00054 self.lock = threading.Lock()
00055 self.name = []
00056 self.position = []
00057 self.velocity = []
00058 self.effort = []
00059 self.thread = threading.Thread(target=self.joint_states_listener)
00060 self.thread.setDaemon(True)
00061 self.thread.start()
00062
00063 '''
00064 Service removed due to not having a home for the following service type:
00065 ReturnJointStates.srv
00066 string[] name
00067 ---
00068 uint32[] found
00069 float64[] position
00070 float64[] velocity
00071 float64[] effort
00072 '''
00073
00074
00075
00076
00077
00078 def joint_states_listener(self):
00079 rospy.Subscriber('joint_states', JointState, self.joint_states_callback)
00080 rospy.spin()
00081
00082
00083
00084 def joint_states_callback(self, msg):
00085 self.lock.acquire()
00086 self.name = msg.name
00087 self.position = msg.position
00088 self.velocity = msg.velocity
00089 self.effort = msg.effort
00090 self.lock.release()
00091
00092
00093
00094
00095 def return_joint_state(self, joint_name):
00096
00097
00098 if self.name == []:
00099 rospy.logerr("No robot_state messages received!\n")
00100 return (0, 0., 0., 0.)
00101
00102
00103 self.lock.acquire()
00104 if joint_name in self.name:
00105 index = self.name.index(joint_name)
00106 position = self.position[index]
00107 velocity = self.velocity[index]
00108 effort = self.effort[index]
00109
00110
00111 else:
00112 rospy.logerr("Joint %s not found!", (joint_name,))
00113 self.lock.release()
00114 return (0, 0., 0., 0.)
00115 self.lock.release()
00116 return (1, position, velocity, effort)
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 def return_joint_states(self, joint_list):
00128 joints_found = []
00129 positions = []
00130 velocities = []
00131 efforts = []
00132 for joint_name in joint_list:
00133 (found, position, velocity, effort) = self.return_joint_state(joint_name)
00134 joints_found.append(found)
00135 positions.append(position)
00136 velocities.append(velocity)
00137 efforts.append(effort)
00138 return (joints_found, positions, velocities, efforts)
00139
00140
00141
00142 if __name__ == "__main__":
00143
00144
00145 if len(sys.argv) < 2 or sys.argv[1] != 'r' and sys.argv[1] != 'l':
00146 rospy.logerr("usage: joint_states_listener.py which_arm (which_arm is r or l)")
00147 sys.exit(1)
00148 which_arm = sys.argv[1]
00149
00150
00151 def pplist(list):
00152 return ' '.join(['%5.3f'%x for x in list])
00153
00154 rospy.init_node(which_arm+'_joint_states_listener')
00155
00156 latest_joint_states = LatestJointStates(run_server = 0)
00157
00158 joint_names = ["_shoulder_pan_joint",
00159 "_shoulder_lift_joint",
00160 "_upper_arm_roll_joint",
00161 "_elbow_flex_joint",
00162 "_forearm_roll_joint",
00163 "_wrist_flex_joint",
00164 "_wrist_roll_joint"]
00165 r_joint_names = ["r" + joint_name for joint_name in joint_names]
00166 l_joint_names = ["l" + joint_name for joint_name in joint_names]
00167 while not rospy.is_shutdown():
00168 time.sleep(0.5)
00169
00170 if which_arm == 'r':
00171 (joints_found, current_r_arm_angs, vels, efforts) = \
00172 latest_joint_states.return_joint_states(r_joint_names)
00173 rospy.loginfo("current right arm angles:"+pplist(current_r_arm_angs))
00174 else:
00175 (joints_found, current_l_arm_angs, vels, efforts) = \
00176 latest_joint_states.return_joint_states(l_joint_names)
00177 rospy.loginfo("current left arm angles:"+pplist(current_l_arm_angs))
00178
00179
00180