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