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