$search
00001 #!/usr/bin/env python 00002 import roslib; roslib.load_manifest('sr_utilities') 00003 import rospy 00004 from sensor_msgs.msg import JointState 00005 from sr_utilities.srv import getJointState 00006 import thread 00007 00008 RATE=100 00009 00010 class MergeMessages: 00011 def __init__(self): 00012 rospy.init_node('arm_and_hand_joint_state_merger', anonymous=True) 00013 00014 self.msg_1_received = False 00015 self.msg_2_received = False 00016 00017 self.subs_1 = rospy.Subscriber("/sh/joint_states", JointState, self.callback1) 00018 self.subs_2 = rospy.Subscriber("/sa/joint_states", JointState, self.callback2) 00019 self.serv = rospy.Service('/getJointState', getJointState, self.getJointStateCB) 00020 00021 self.pub = rospy.Publisher("/joint_states", JointState) 00022 00023 self.joint_state_msg = JointState() 00024 self.joint_state_msg_1 = JointState() 00025 self.joint_state_msg_2 = JointState() 00026 00027 self.mutex = thread.allocate_lock() 00028 00029 r = rospy.Rate( RATE ) 00030 while not rospy.is_shutdown(): 00031 self.publish() 00032 r.sleep() 00033 00034 def callback1(self, data): 00035 self.msg_1_received = True 00036 self.joint_state_msg_1 = data 00037 00038 def callback2(self, data): 00039 self.msg_2_received = True 00040 self.joint_state_msg_2 = data 00041 00042 def publish(self): 00043 if self.msg_1_received == True and self.msg_2_received == True: 00044 self.joint_state_msg.header.stamp = rospy.Time.now() 00045 self.joint_state_msg.name = self.joint_state_msg_1.name + self.joint_state_msg_2.name 00046 self.joint_state_msg.position = self.joint_state_msg_1.position + self.joint_state_msg_2.position 00047 self.joint_state_msg.effort = self.joint_state_msg_1.effort + self.joint_state_msg_2.effort 00048 self.joint_state_msg.velocity = self.joint_state_msg_1.velocity + self.joint_state_msg_2.velocity 00049 00050 self.pub.publish(self.joint_state_msg) 00051 self.msg_1_received = False 00052 self.msg_2_received = False 00053 00054 def getJointStateCB(self,req): 00055 res=self.joint_state_msg 00056 return res 00057 00058 00059 if __name__ == '__main__': 00060 merger = MergeMessages()