00001 #!/usr/bin/env python 00002 import rospy 00003 from sensor_msgs.msg import JointState 00004 import thread 00005 00006 00007 class MergeMessages: 00008 def __init__(self): 00009 rospy.init_node('joint_state_merger', anonymous=True) 00010 self.subs_1 = rospy.Subscriber("/srh/position/joint_states", JointState, self.callback1) 00011 self.subs_2 = rospy.Subscriber("/sr_arm/position/joint_states", JointState, self.callback2) 00012 00013 self.pub = rospy.Publisher("/joint_states", JointState) 00014 00015 self.msg_1_received = False 00016 self.msg_2_received = False 00017 00018 self.joint_state_msg = JointState() 00019 00020 self.mutex = thread.allocate_lock() 00021 00022 rospy.spin() 00023 00024 def callback1(self, data): 00025 self.mutex.acquire() 00026 if self.msg_1_received == True: 00027 self.mutex.release() 00028 return 00029 00030 self.msg_1_received = True 00031 00032 self.joint_state_msg.header.stamp = rospy.Time.now() 00033 00034 tmp = self.joint_state_msg.name 00035 tmp += data.name 00036 self.joint_state_msg.name = tmp 00037 00038 tmp = self.joint_state_msg.position 00039 tmp += data.position 00040 self.joint_state_msg.position = tmp 00041 00042 tmp = self.joint_state_msg.velocity 00043 tmp += data.velocity 00044 self.joint_state_msg.velocity = tmp 00045 00046 tmp = self.joint_state_msg.effort 00047 tmp += data.effort 00048 self.joint_state_msg.effort = tmp 00049 00050 if self.msg_2_received == True: 00051 self.pub.publish(self.joint_state_msg) 00052 self.msg_1_received = False 00053 self.msg_2_received = False 00054 self.joint_state_msg = JointState() 00055 00056 self.mutex.release() 00057 00058 def callback2(self, data): 00059 self.mutex.acquire() 00060 if self.msg_2_received == True: 00061 self.mutex.release() 00062 return 00063 00064 self.msg_2_received = True 00065 00066 self.joint_state_msg.header.stamp = rospy.Time.now() 00067 00068 tmp = self.joint_state_msg.name 00069 tmp += data.name 00070 self.joint_state_msg.name = tmp 00071 00072 tmp = self.joint_state_msg.position 00073 tmp += data.position 00074 self.joint_state_msg.position = tmp 00075 00076 tmp = self.joint_state_msg.velocity 00077 tmp += data.velocity 00078 self.joint_state_msg.velocity = tmp 00079 00080 tmp = self.joint_state_msg.effort 00081 tmp += data.effort 00082 self.joint_state_msg.effort = tmp 00083 00084 if self.msg_1_received == True: 00085 self.pub.publish(self.joint_state_msg) 00086 self.msg_1_received = False 00087 self.msg_2_received = False 00088 self.joint_state_msg = JointState() 00089 00090 self.mutex.release() 00091 00092 00093 00094 if __name__ == '__main__': 00095 merger = MergeMessages()