collada_tools: pub_joint_trajectory

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 import roslib; roslib.load_manifest("hrpsys_gazebo")
00004 import rospy
00005 from sensor_msgs.msg import JointState
00006 from optparse import OptionParser
00007 
00008 ## trajectory_action
00009 import actionlib
00010 from actionlib_msgs.msg import *
00011 from pr2_controllers_msgs.msg import *
00012 from trajectory_msgs.msg import *
00013 
00014 import time
00015 
00016 if __name__ == "__main__":
00017     parser = OptionParser(description='pub_joint_trajectory_action')
00018 
00019     parser.add_option('--joints', action='store', type='string', dest='joints', default=None,
00020                       help='')
00021     parser.add_option('--time', action='store', type='float', dest='time', default=1.0,
00022                       help='')
00023     parser.add_option('--start_time', action='store', type='float', dest='start_after', default=0.2,
00024                       help='')
00025     parser.add_option('--position', action='store', type='string', dest='position', default=None,
00026                       help='')
00027     parser.add_option('--velocity', action='store', type='string', dest='velocity', default=None,
00028                       help='')
00029     parser.add_option('--effort', action='store', type='string', dest='effort', default=None,
00030                       help='')
00031     parser.add_option('--action', action='store', type='string', dest='action', default='joint_states',
00032                       help='')
00033 
00034     (options, args) = parser.parse_args()
00035 
00036     if options.joints is not None:
00037         jlist = options.joints.split(',')
00038     else:
00039         print 'There is no joint inputs'
00040         exit(0)
00041 
00042     plist = None
00043     vlist = None
00044     elist = None
00045     if options.position is not None:
00046         plist = options.position.split(',')
00047     if options.velocity is not None:
00048         vlist = options.velocity.split(',')
00049     if options.effort is not None:
00050         elist = options.effort.split(',')
00051 
00052     rospy.init_node("joint_state_publisher")
00053 
00054     client = actionlib.SimpleActionClient(options.action, JointTrajectoryAction)
00055     rospy.loginfo("wait server ( %s ) ... "%options.action)
00056     client.wait_for_server()
00057     rospy.loginfo("server ( %s ) found "%options.action)
00058 
00059     g = JointTrajectoryGoal()
00060 
00061     for i in range(len(jlist)):
00062         g.trajectory.joint_names.append(jlist[i])
00063 
00064     pt = JointTrajectoryPoint()
00065     for i in range(len(jlist)):
00066         if plist is not None and len(plist) > i:
00067             pt.positions.append(float(plist[i]))
00068         if vlist is not None and len(vlist) > i:
00069             pt.velocities.append(float(vlist[i]))
00070         if elist is not None and len(elist) > i:
00071             pt.accelerations.append(float(elist[i]))
00072         if (plist is None) and (vlist is None) and (elist is None):
00073             pt.positions.append(0)
00074 
00075     pt.time_from_start = rospy.Duration( options.time )
00076     g.trajectory.points.append(pt);
00077 
00078     g.trajectory.header.seq = 1
00079     g.trajectory.header.stamp = ( rospy.Time.now() + rospy.Duration( options.start_after) )
00080 
00081     client.send_goal(g)
00082     rospy.loginfo("send goal")
00083     client.wait_for_result( )
00084     #client.wait_for_result( rospy.Duration.from_sec(10.0) )
00085 
00086     print client.get_state()
00087 
00088     if client.get_state() == GoalStatus.SUCCEEDED:
00089         print "action succeeded"
00090         exit(0)
00091     else:
00092         print "action failed"
00093         exit(1)