$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 00037 ## @package joint_states_listener 00038 #Spins off a thread to listen for joint_states messages and 00039 #provides the same information (or subsets of) as a Python function 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 ##holds the latest states obtained from joint_states messages 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 # if run_server: 00074 # s = rospy.Service('return_joint_states', ReturnJointStates, self.return_joint_states_service) 00075 00076 00077 ##thread function: listen for joint_states messages 00078 def joint_states_listener(self): 00079 rospy.Subscriber('joint_states', JointState, self.joint_states_callback) 00080 rospy.spin() 00081 00082 00083 ##callback function: when a joint_states message arrives, save the values 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 ##returns (found, position, velocity, effort) for the joint joint_name 00094 #(found is 1 if found, 0 otherwise) 00095 def return_joint_state(self, joint_name): 00096 00097 #no messages yet 00098 if self.name == []: 00099 rospy.logerr("No robot_state messages received!\n") 00100 return (0, 0., 0., 0.) 00101 00102 #return info for this joint 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 #unless it's not found 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 # #server callback: returns arrays of position, velocity, and effort 00120 # #for a list of joints specified by name 00121 # def return_joint_states_service(self, req): 00122 # (joints_found, positions, velocities, efforts) = self.return_joint_states(req.name) 00123 # return ReturnJointStatesResponse(joints_found, positions, velocities, efforts) 00124 00125 00126 ##return the positions, velocities, and efforts for a list of joint names 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 #keep printing the current arm angles 00142 if __name__ == "__main__": 00143 00144 #which arm to print 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 #pretty-print list to string 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 #rospy.loginfo("joints_states_listener server started, waiting for queries") 00180 #rospy.spin()