Go to the documentation of this file.00001
00002
00003 import rospy
00004 from sensor_msgs.msg import JointState
00005
00006 def joint_state_publisher():
00007 rospy.init_node('hand_joint_state_publisher', anonymous=True)
00008 pub = rospy.Publisher('joint_states', JointState, queue_size=1)
00009 joint_list = ['LHAND_JOINT0', 'LHAND_JOINT1', 'LHAND_JOINT2', 'LHAND_JOINT3', 'RHAND_JOINT0', 'RHAND_JOINT1', 'RHAND_JOINT2', 'RHAND_JOINT3']
00010 rospy.loginfo("{} publishes fake joint states of {}".format(rospy.get_name(), joint_list))
00011 rate = rospy.Rate(3)
00012 while not rospy.is_shutdown():
00013 joint_states = JointState()
00014 joint_states.header.stamp = rospy.Time.now()
00015 joint_states.name = []
00016 joint_states.position = []
00017 for joint in joint_list:
00018 joint_states.name.append(joint)
00019 joint_states.position.append(0)
00020 rospy.logdebug(joint_states)
00021 pub.publish(joint_states)
00022 rate.sleep()
00023
00024 if __name__ == '__main__':
00025 try:
00026 joint_state_publisher()
00027 except rospy.ROSInterruptException:
00028 pass