00001
00002
00003
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
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
00029 def joint_states_listener(self):
00030 rospy.Subscriber('joint_states', JointState, self.joint_states_callback)
00031 rospy.spin()
00032
00033
00034
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
00045
00046 def return_joint_state(self, joint_name):
00047
00048
00049 if self.name == []:
00050 rospy.logerr("No robot_state messages received!\n")
00051 return (0, 0., 0., 0.)
00052
00053
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
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
00071
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
00087 if __name__ == "__main__":
00088
00089 latestjointstates = LatestJointStates()
00090
00091 print "joints_states_listener server started, waiting for queries"
00092 rospy.spin()