hand_joint_state_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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) # 3hz
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


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37