joint_0_publisher.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 Joint0Publisher:
00009     def __init__(self):
00010         rospy.init_node('joint_0_publisher', anonymous=True)
00011         self.subs_1 = rospy.Subscriber("joint_states", JointState, self.callback)
00012         self.pub = rospy.Publisher("joint_0s/joint_states", JointState)
00013         self.joint_state_msg = JointState()
00014 
00015         rospy.spin()
00016 
00017     def callback(self, data):
00018         self.joint_state_msg.header.stamp = rospy.Time.now()
00019 
00020         self.joint_state_msg.name = []
00021         self.joint_state_msg.position = []
00022         self.joint_state_msg.velocity = []
00023         self.joint_state_msg.effort = []
00024 
00025         fj0 = [0.0, 0.0, 0.0]
00026         for name,position,velocity,effort in zip(data.name,
00027                                                  data.position,
00028                                                  data.velocity,
00029                                                  data.effort):
00030             if "FJ1" in name and len(name) == 4:
00031                 fj0 = [0.0, 0.0, 0.0]
00032                 fj0 = [position, velocity, effort]
00033 
00034             if "FJ2" in name and len(name) == 4:
00035                 fj0 = [fj0[0] + position, fj0[1] + velocity, fj0[2] + effort]
00036                 self.joint_state_msg.name.append(name[0]+"FJ0")
00037                 self.joint_state_msg.position.append(fj0[0])
00038                 self.joint_state_msg.velocity.append(fj0[1])
00039                 self.joint_state_msg.effort.append(fj0[2])
00040 
00041         self.pub.publish(self.joint_state_msg)
00042 
00043 if __name__ == '__main__':
00044     j0_pub = Joint0Publisher()


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