joint_states_merger.py
Go to the documentation of this file.
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()


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:42:15