$search
00001 #!/usr/bin/env python 00002 #spins off a thread to listen for joint_states messages 00003 #and provides the same information (or subsets of) as a service 00004 00005 import roslib; roslib.load_manifest('simple_robot_control') 00006 import rospy 00007 from simple_robot_control.srv import * 00008 from sensor_msgs.msg import JointState 00009 import threading 00010 00011 00012 #holds the latest states obtained from joint_states messages 00013 class LatestJointStates: 00014 00015 def __init__(self): 00016 rospy.init_node('joint_states_listener') 00017 self.lock = threading.Lock() 00018 self.name = [] 00019 self.position = [] 00020 self.velocity = [] 00021 self.effort = [] 00022 self.thread = threading.Thread(target=self.joint_states_listener) 00023 self.thread.start() 00024 00025 s = rospy.Service('return_joint_states', ReturnJointStates, self.return_joint_states) 00026 00027 00028 #thread function: listen for joint_states messages 00029 def joint_states_listener(self): 00030 rospy.Subscriber('joint_states', JointState, self.joint_states_callback) 00031 rospy.spin() 00032 00033 00034 #callback function: when a joint_states message arrives, save the values 00035 def joint_states_callback(self, msg): 00036 self.lock.acquire() 00037 self.name = msg.name 00038 self.position = msg.position 00039 self.velocity = msg.velocity 00040 self.effort = msg.effort 00041 self.lock.release() 00042 00043 00044 #returns (found, position, velocity, effort) for the joint joint_name 00045 #(found is 1 if found, 0 otherwise) 00046 def return_joint_state(self, joint_name): 00047 00048 #no messages yet 00049 if self.name == []: 00050 rospy.logerr("No robot_state messages received!\n") 00051 return (0, 0., 0., 0.) 00052 00053 #return info for this joint 00054 self.lock.acquire() 00055 if joint_name in self.name: 00056 index = self.name.index(joint_name) 00057 position = self.position[index] 00058 velocity = self.velocity[index] 00059 effort = self.effort[index] 00060 00061 #unless it's not found 00062 else: 00063 rospy.logerr("Joint %s not found!", (joint_name,)) 00064 self.lock.release() 00065 return (0, 0., 0., 0.) 00066 self.lock.release() 00067 return (1, position, velocity, effort) 00068 00069 00070 #server callback: returns arrays of position, velocity, and effort 00071 #for a list of joints specified by name 00072 def return_joint_states(self, req): 00073 joints_found = [] 00074 positions = [] 00075 velocities = [] 00076 efforts = [] 00077 for joint_name in req.name: 00078 (found, position, velocity, effort) = self.return_joint_state(joint_name) 00079 joints_found.append(found) 00080 positions.append(position) 00081 velocities.append(velocity) 00082 efforts.append(effort) 00083 return ReturnJointStatesResponse(joints_found, positions, velocities, efforts) 00084 00085 00086 #run the server 00087 if __name__ == "__main__": 00088 00089 latestjointstates = LatestJointStates() 00090 00091 print "joints_states_listener server started, waiting for queries" 00092 rospy.spin()