sr_utilities: ordered_hand_arm_joint_states

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('sr_utilities')
00003 import rospy
00004 from sensor_msgs.msg import JointState
00005 from sr_utilities.srv import getJointState
00006 import thread
00007 
00008 RATE=100
00009 
00010 class MergeMessages:
00011     def __init__(self):
00012         rospy.init_node('arm_and_hand_joint_state_merger', anonymous=True)
00013 
00014         self.msg_1_received = False
00015         self.msg_2_received = False
00016 
00017         self.subs_1 = rospy.Subscriber("/sh/joint_states", JointState, self.callback1)
00018         self.subs_2 = rospy.Subscriber("/sa/joint_states", JointState, self.callback2)
00019         self.serv = rospy.Service('/getJointState', getJointState, self.getJointStateCB)
00020         
00021         self.pub = rospy.Publisher("/joint_states", JointState)
00022 
00023         self.joint_state_msg = JointState()
00024         self.joint_state_msg_1 = JointState()
00025         self.joint_state_msg_2 = JointState()
00026         
00027         self.mutex = thread.allocate_lock()
00028 
00029         r = rospy.Rate( RATE )
00030         while not rospy.is_shutdown():
00031             self.publish()
00032             r.sleep()
00033 
00034     def callback1(self, data):
00035         self.msg_1_received = True
00036         self.joint_state_msg_1 = data
00037 
00038     def callback2(self, data):
00039         self.msg_2_received = True
00040         self.joint_state_msg_2 = data
00041         
00042     def publish(self):
00043         if self.msg_1_received == True and self.msg_2_received == True:
00044             self.joint_state_msg.header.stamp = rospy.Time.now()
00045             self.joint_state_msg.name = self.joint_state_msg_1.name + self.joint_state_msg_2.name
00046             self.joint_state_msg.position = self.joint_state_msg_1.position + self.joint_state_msg_2.position
00047             self.joint_state_msg.effort = self.joint_state_msg_1.effort + self.joint_state_msg_2.effort
00048             self.joint_state_msg.velocity = self.joint_state_msg_1.velocity + self.joint_state_msg_2.velocity
00049 
00050             self.pub.publish(self.joint_state_msg)
00051             self.msg_1_received = False
00052             self.msg_2_received = False
00053 
00054     def getJointStateCB(self,req):
00055         res=self.joint_state_msg
00056         return res
00057 
00058 
00059 if __name__ == '__main__':
00060     merger = MergeMessages()