collada_tools: static_joint_state_publisher.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 import roslib; roslib.load_manifest("hrpsys_gazebo")
00005 import rospy
00006 from sensor_msgs.msg import JointState
00007 from optparse import OptionParser
00008 
00009 if __name__ == "__main__":
00010     parser = OptionParser(description='static_joint_state_publisher')
00011 
00012     parser.add_option('--joints', action='store', type='string', dest='joints', default=None,
00013                       help='')
00014     parser.add_option('--rate', action='store', type='float', dest='rate', default=100.0,
00015                       help='')
00016     parser.add_option('--position', action='store', type='string', dest='position', default=None,
00017                       help='')
00018     parser.add_option('--velocity', action='store', type='string', dest='velocity', default=None,
00019                       help='')
00020     parser.add_option('--effort', action='store', type='string', dest='effort', default=None,
00021                       help='')
00022     parser.add_option('--topic', action='store', type='string', dest='topicname', default='joint_states',
00023                       help='')
00024 
00025     (options, args) = parser.parse_args()
00026 
00027     if options.joints is not None:
00028         jlist = options.joints.split(',')
00029     else:
00030         exit(0)
00031 
00032     plist = None
00033     vlist = None
00034     elist = None
00035     if options.position is not None:
00036         plist = options.position.split(',')
00037     if options.velocity is not None:
00038         vlist = options.velocity.split(',')
00039     if options.effort is not None:
00040         elist = options.effort.split(',')
00041 
00042     # ROSノードの準備
00043     rospy.init_node("joint_state_publisher")
00044     publisher = rospy.Publisher(options.topicname, JointState)
00045     rate = rospy.Rate(options.rate)
00046 
00047     # JointStateの準備
00048     joint_state = JointState()
00049 
00050     for i in range(len(jlist)):
00051         joint_state.name.append(jlist[i])
00052         if plist is not None and len(plist) > i:
00053             joint_state.position.append(float(plist[i]))
00054         if vlist is not None and len(vlist) > i:
00055             joint_state.velocity.append(float(vlist[i]))
00056         if elist is not None and len(elist) > i:
00057             joint_state.effort.append(float(elist[i]))
00058         if (plist is None) and (vlist is None) and (elist is None):
00059             joint_state.position.append(0)
00060 
00061     # ROSループ
00062     while not rospy.is_shutdown():
00063         # 現在時刻をstampに入れる
00064         joint_state.header.stamp = rospy.Time.now()
00065         publisher.publish(joint_state)
00066         rate.sleep()