ordered_position_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('position_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("/positions/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         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 
00081 if __name__ == '__main__':
00082     merger = MergeMessages()


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:44