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('target_joint_state_merger', anonymous=True) 00010 self.subs_1 = rospy.Subscriber("/srh/target/joint_states", JointState, self.callback1) 00011 self.subs_2 = rospy.Subscriber("/sr_arm/target/joint_states", JointState, self.callback2) 00012 00013 self.pub = rospy.Publisher("/targets/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 = data.name 00035 self.joint_state_msg.name = tmp 00036 00037 tmp = data.position 00038 self.joint_state_msg.position = tmp 00039 00040 tmp = data.velocity 00041 self.joint_state_msg.velocity = tmp 00042 00043 tmp = data.effort 00044 self.joint_state_msg.effort = tmp 00045 00046 self.mutex.release() 00047 00048 def callback2(self, data): 00049 self.mutex.acquire() 00050 00051 self.msg_2_received = True 00052 00053 self.joint_state_msg.header.stamp = rospy.Time.now() 00054 00055 tmp = self.joint_state_msg.name 00056 tmp += data.name 00057 self.joint_state_msg.name = tmp 00058 00059 tmp = self.joint_state_msg.position 00060 tmp += data.position 00061 self.joint_state_msg.position = tmp 00062 00063 tmp = self.joint_state_msg.velocity 00064 tmp += data.velocity 00065 self.joint_state_msg.velocity = tmp 00066 00067 tmp = self.joint_state_msg.effort 00068 tmp += data.effort 00069 self.joint_state_msg.effort = tmp 00070 00071 if self.msg_1_received == True: 00072 self.pub.publish(self.joint_state_msg) 00073 self.msg_1_received = False 00074 self.msg_2_received = False 00075 self.joint_state_msg = JointState() 00076 00077 self.mutex.release() 00078 00079 00080 if __name__ == '__main__': 00081 merger = MergeMessages()