r2_readypose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python  
00002 import roslib;
00003 roslib.load_manifest('r2_gazebo')
00004 #roslib.load_manifest('r2_controllers')
00005 
00006 import rospy
00007 import tf
00008 
00009 from std_msgs.msg import String
00010 from sensor_msgs.msg import JointState
00011 
00012 from nasa_r2_common_msgs.srv import *
00013 
00014 import math
00015 import time
00016 
00017 def send_command_data() :
00018 
00019     print "setting left arm to joint mode"
00020     rospy.wait_for_service('r2_controller/set_joint_mode')
00021     try:
00022         set_joint_mode = rospy.ServiceProxy('r2_controller/set_joint_mode', SetJointMode)
00023         resp1 = set_joint_mode('left')
00024         #print resp1.result
00025     except rospy.ServiceException, e:
00026         print "Service call failed: %s"%e
00027 
00028     print "setting right arm to joint mode"
00029     rospy.wait_for_service('r2_controller/set_joint_mode')
00030     try:
00031         set_joint_mode = rospy.ServiceProxy('r2_controller/set_joint_mode', SetJointMode)
00032         resp1 = set_joint_mode('right')
00033         #print resp1.result
00034     except rospy.ServiceException, e:
00035         print "Service call failed: %s"%e
00036 
00037     # joint command publishers
00038     left_arm_pub = rospy.Publisher('r2_controller/left_arm/joint_command', JointState)
00039     right_arm_pub = rospy.Publisher('r2_controller/right_arm/joint_command', JointState)
00040     neck_pub = rospy.Publisher('r2_controller/neck/joint_command', JointState)
00041     waist_pub = rospy.Publisher('r2_controller/waist/joint_command', JointState)
00042 
00043     rospy.init_node('r2_readypose')
00044 
00045     TORAD = math.pi/180.0
00046 
00047     waist_js = JointState()
00048     left_arm_js = JointState()
00049     right_arm_js = JointState()
00050     neck_js = JointState()
00051 
00052     left_arm_js.name = ['/r2/left_arm/joint0', 
00053                         '/r2/left_arm/joint1', 
00054                         '/r2/left_arm/joint2', 
00055                         '/r2/left_arm/joint3', 
00056                         '/r2/left_arm/joint4', 
00057                         '/r2/left_arm/joint5', 
00058                         '/r2/left_arm/joint6',
00059                         '/r2/left_arm/hand/thumb/joint0',
00060                         '/r2/left_arm/hand/thumb/joint1', 
00061                         '/r2/left_arm/hand/thumb/joint2', 
00062                         '/r2/left_arm/hand/thumb/joint3', 
00063                         '/r2/left_arm/hand/index/joint0', 
00064                         '/r2/left_arm/hand/index/joint1', 
00065                         '/r2/left_arm/hand/index/joint2', 
00066                         '/r2/left_arm/hand/middle/joint0', 
00067                         '/r2/left_arm/hand/middle/joint1', 
00068                         '/r2/left_arm/hand/middle/joint2', 
00069                         '/r2/left_arm/hand/ring/joint0', 
00070                         '/r2/left_arm/hand/little/joint0'] 
00071 
00072     right_arm_js.name = ['/r2/right_arm/joint0', 
00073                          '/r2/right_arm/joint1', 
00074                          '/r2/right_arm/joint2', 
00075                          '/r2/right_arm/joint3',                    
00076                          '/r2/right_arm/joint4', 
00077                          '/r2/right_arm/joint5', 
00078                          '/r2/right_arm/joint6',
00079                          '/r2/right_arm/hand/thumb/joint0',
00080                          '/r2/right_arm/hand/thumb/joint1', 
00081                          '/r2/right_arm/hand/thumb/joint2', 
00082                          '/r2/right_arm/hand/thumb/joint3', 
00083                          '/r2/right_arm/hand/index/joint0', 
00084                          '/r2/right_arm/hand/index/joint1', 
00085                          '/r2/right_arm/hand/index/joint2', 
00086                          '/r2/right_arm/hand/middle/joint0', 
00087                          '/r2/right_arm/hand/middle/joint1', 
00088                          '/r2/right_arm/hand/middle/joint2', 
00089                          '/r2/right_arm/hand/ring/joint0', 
00090                          '/r2/right_arm/hand/little/joint0']
00091 
00092     neck_js.name = ['/r2/neck/joint0', 
00093                     '/r2/neck/joint1', 
00094                     '/r2/neck/joint2']
00095     waist_js.name = ['/r2/waist/joint0']
00096 
00097 
00098     left_arm_js.position = [0]*(7+12)
00099     right_arm_js.position = [0]*(7+12)
00100     neck_js.position = [0]*3
00101     waist_js.position = [180.0*TORAD]*1
00102 
00103     #left_arm_js.velocity = [0]*(7+12)
00104     #right_arm_js.velocity = [0]*(7+12)
00105     #neck_js.velocity = [0]*3
00106     #waist_js.velocity = [0]*1
00107 
00108     #left_arm_js.effort = [0]*(7+12)
00109     #right_arm_js.effort = [0]*(7+12)
00110     #neck_js.effort = [0]*3
00111     #waist_js.effort = [0]*1
00112 
00113     left_arm_js.header.seq = 0
00114     right_arm_js.header.seq = 0
00115     waist_js.header.seq = 0
00116     neck_js.header.seq = 0
00117 
00118     left_arm_js.header.stamp = rospy.get_rostime()
00119     right_arm_js.header.stamp = rospy.get_rostime()
00120     waist_js.header.stamp = rospy.get_rostime()
00121     neck_js.header.stamp = rospy.get_rostime()
00122 
00123     left_arm_js.header.frame_id = "world"
00124     right_arm_js.header.frame_id = "world"
00125     waist_js.header.frame_id = "world"
00126     neck_js.header.frame_id = "world"
00127 
00128     c = 0
00129     go_to_ready = 0
00130 
00131     left_arm_js.position = [50.0*TORAD, -80.0*TORAD, -105.0*TORAD, -140.0*TORAD, 80.0*TORAD, 0.0*TORAD, 0.0*TORAD]+[0*TORAD]*12
00132     right_arm_js.position = [-50.0*TORAD, -80.0*TORAD, 105.0*TORAD, -140.0*TORAD, -80.0*TORAD, 0.0*TORAD, 0.0*TORAD]+[0*TORAD]*12
00133     neck_js.position = [-5*TORAD, -0*TORAD, -0*TORAD]
00134     waist_js.position = [180.0*TORAD]*1
00135 
00136     rospy.sleep(1)        
00137     left_arm_pub.publish(left_arm_js); #rospy.sleep(1)
00138     right_arm_pub.publish(right_arm_js); #rospy.sleep(1)
00139     neck_pub.publish(neck_js); #rospy.sleep(0.2)
00140     
00141     rospy.sleep(5)
00142     waist_pub.publish(waist_js); #rospy.sleep(3)
00143    
00144     print "subscriber shutting down..."
00145 
00146 
00147 if __name__ == '__main__':
00148     try:
00149         send_command_data()
00150     except rospy.ROSInterruptException: pass


r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Mon Oct 6 2014 02:48:44