sr_utilities: ordered_hand_arm_joint_states

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