r2_controllers_gazebo: r2_gazebo_controller

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 from geometry_msgs.msg import PoseStamped
00012 
00013 from tf.transformations import quaternion_from_euler
00014 from nasa_r2_common_msgs.srv import *
00015 
00016 import math
00017 import time
00018 
00019 def send_command_data() :
00020 
00021     print "setting left arm to joint mode"
00022     rospy.wait_for_service('/r2_controller/set_joint_mode')
00023     try:
00024         set_joint_mode = rospy.ServiceProxy('/r2_controller/set_joint_mode', SetJointMode)
00025         resp1 = set_joint_mode('left')
00026         #print resp1.result
00027     except rospy.ServiceException, e:
00028         print "Service call failed: %s"%e
00029 
00030     print "setting right arm to joint mode"
00031     rospy.wait_for_service('/r2_controller/set_joint_mode')
00032     try:
00033         set_joint_mode = rospy.ServiceProxy('/r2_controller/set_joint_mode', SetJointMode)
00034         resp1 = set_joint_mode('right')
00035         #print resp1.result
00036     except rospy.ServiceException, e:
00037         print "Service call failed: %s"%e
00038 
00039     # joint command publishers
00040     left_arm_pub = rospy.Publisher('/r2_controller/left_arm/joint_command', JointState)
00041     right_arm_pub = rospy.Publisher('/r2_controller/right_arm/joint_command', JointState)
00042     neck_pub = rospy.Publisher('/r2_controller/neck/joint_command', JointState)
00043     waist_pub = rospy.Publisher('/r2_controller/waist/joint_command', JointState)
00044 
00045     # pose command publishers
00046     left_pose_pub = rospy.Publisher('/r2_controller/left/pose_command', PoseStamped)
00047     right_pose_pub = rospy.Publisher('/r2_controller/right/pose_command', PoseStamped)
00048 
00049     rospy.init_node('r2_gazebo_controller_test')
00050 
00051     TORAD = math.pi/180.0
00052 
00053     waist_js = JointState()
00054     left_arm_js = JointState()
00055     right_arm_js = JointState()
00056     neck_js = JointState()
00057 
00058     left_pose = PoseStamped()
00059     right_pose = PoseStamped()
00060 
00061     left_arm_js.name = ['/r2/left_arm/joint0', 
00062                         '/r2/left_arm/joint1', 
00063                         '/r2/left_arm/joint2', 
00064                         '/r2/left_arm/joint3', 
00065                         '/r2/left_arm/joint4', 
00066                         '/r2/left_arm/joint5', 
00067                         '/r2/left_arm/joint6',
00068                         '/r2/left_arm/hand/thumb/joint0', 
00069                         '/r2/left_arm/hand/thumb/joint1', 
00070                         '/r2/left_arm/hand/thumb/joint2', 
00071                         '/r2/left_arm/hand/thumb/joint3', 
00072                         '/r2/left_arm/hand/index/joint0', 
00073                         '/r2/left_arm/hand/index/joint1', 
00074                         '/r2/left_arm/hand/index/joint2', 
00075                         '/r2/left_arm/hand/middle/joint0', 
00076                         '/r2/left_arm/hand/middle/joint1', 
00077                         '/r2/left_arm/hand/middle/joint2', 
00078                         '/r2/left_arm/hand/ring/joint0', 
00079                         '/r2/left_arm/hand/little/joint0'] 
00080 
00081     right_arm_js.name = ['/r2/right_arm/joint0', 
00082                          '/r2/right_arm/joint1', 
00083                          '/r2/right_arm/joint2', 
00084                          '/r2/right_arm/joint3',                    
00085                          '/r2/right_arm/joint4', 
00086                          '/r2/right_arm/joint5', 
00087                          '/r2/right_arm/joint6',
00088                          '/r2/right_arm/hand/thumb/joint0', 
00089                          '/r2/right_arm/hand/thumb/joint1', 
00090                          '/r2/right_arm/hand/thumb/joint2', 
00091                          '/r2/right_arm/hand/thumb/joint3', 
00092                          '/r2/right_arm/hand/index/joint0', 
00093                          '/r2/right_arm/hand/index/joint1', 
00094                          '/r2/right_arm/hand/index/joint2', 
00095                          '/r2/right_arm/hand/middle/joint0', 
00096                          '/r2/right_arm/hand/middle/joint1', 
00097                          '/r2/right_arm/hand/middle/joint2', 
00098                          '/r2/right_arm/hand/ring/joint0', 
00099                          '/r2/right_arm/hand/little/joint0']
00100 
00101     neck_js.name = ['/r2/neck/joint0', 
00102                     '/r2/neck/joint1', 
00103                     '/r2/neck/joint2']
00104     waist_js.name = ['/r2/waist/joint0']
00105 
00106 
00107     left_arm_js.position = [0]*(7+12)
00108     right_arm_js.position = [0]*(7+12)
00109     neck_js.position = [0]*3
00110     waist_js.position = [180.0*TORAD]*1
00111 
00112     #left_arm_js.velocity = [0]*(7+12)
00113     #right_arm_js.velocity = [0]*(7+12)
00114     #neck_js.velocity = [0]*3
00115     #waist_js.velocity = [0]*1
00116 
00117     #left_arm_js.effort = [0]*(7+12)
00118     #right_arm_js.effort = [0]*(7+12)
00119     #neck_js.effort = [0]*3
00120     #waist_js.effort = [0]*1
00121 
00122     left_arm_js.header.seq = 0
00123     right_arm_js.header.seq = 0
00124     waist_js.header.seq = 0
00125     neck_js.header.seq = 0
00126 
00127     left_arm_js.header.stamp = rospy.get_rostime()
00128     right_arm_js.header.stamp = rospy.get_rostime()
00129     waist_js.header.stamp = rospy.get_rostime()
00130     neck_js.header.stamp = rospy.get_rostime()
00131 
00132     left_arm_js.header.frame_id = "world"
00133     right_arm_js.header.frame_id = "world"
00134     waist_js.header.frame_id = "world"
00135     neck_js.header.frame_id = "world"
00136 
00137     c = 0
00138     go_to_ready = 0
00139 
00140     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
00141     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
00142     neck_js.position = [-5*TORAD, -0*TORAD, -0*TORAD]
00143     waist_js.position = [180.0*TORAD]*1
00144 
00145     rospy.sleep(1)        
00146     waist_pub.publish(waist_js); #rospy.sleep(3)
00147     left_arm_pub.publish(left_arm_js); #rospy.sleep(1)
00148     right_arm_pub.publish(right_arm_js); #rospy.sleep(1)
00149     neck_pub.publish(neck_js); #rospy.sleep(0.2)
00150     
00151     rospy.sleep(7)
00152 
00153     print "setting left tip to palm"
00154     rospy.wait_for_service('/r2_controller/set_tip_name')
00155     try:
00156         set_tip_name = rospy.ServiceProxy('/r2_controller/set_tip_name', SetTipName)
00157         resp1 = set_tip_name('left', '/r2/left_palm')
00158         print resp1.result
00159     except rospy.ServiceException, e:
00160         print "Service call failed: %s"%e
00161 
00162     print "setting right tip to palm"
00163     rospy.wait_for_service('/r2_controller/set_tip_name')
00164     try:
00165         set_tip_name = rospy.ServiceProxy('/r2_controller/set_tip_name', SetTipName)
00166         resp1 = set_tip_name('right', '/r2/right_palm')
00167         print resp1.result
00168     except rospy.ServiceException, e:
00169         print "Service call failed: %s"%e
00170 
00171     # create pose message
00172     # set orientations
00173     roll = -1.57
00174     pitch = 0
00175     yaw = 0
00176     q = quaternion_from_euler(roll, pitch, yaw)
00177 
00178     left_pose.header.seq = 0
00179     left_pose.header.stamp = rospy.get_rostime()
00180     left_pose.header.frame_id = '/r2/robot_reference'
00181     left_pose.pose.position.x = 0.6
00182     left_pose.pose.position.y = -.35
00183     left_pose.pose.position.z = -.5
00184     left_pose.pose.orientation.x = q[0]
00185     left_pose.pose.orientation.y = q[1]
00186     left_pose.pose.orientation.z = q[2]
00187     left_pose.pose.orientation.w = q[3]
00188     print left_pose.pose.orientation
00189     left_pose_pub.publish(left_pose); 
00190 
00191     roll = 0
00192     pitch = 0
00193     yaw = 0
00194     q = quaternion_from_euler(roll, pitch, yaw)
00195 
00196     right_pose.header.seq = 0
00197     right_pose.header.stamp = rospy.get_rostime()
00198     right_pose.header.frame_id = '/r2/robot_reference'
00199     right_pose.pose.position.x = 0.6
00200     right_pose.pose.position.y = .35
00201     right_pose.pose.position.z = -.5
00202     right_pose.pose.orientation.x = q[0]
00203     right_pose.pose.orientation.y = q[1]
00204     right_pose.pose.orientation.z = q[2]
00205     right_pose.pose.orientation.w = q[3]
00206     print right_pose.pose.orientation
00207     right_pose_pub.publish(right_pose); 
00208 
00209     rospy.sleep(2)
00210 
00211     while not rospy.is_shutdown():
00212             
00213         if c%2 == 0:
00214             go_to_ready = 1-go_to_ready
00215             print "going to ready: ", go_to_ready       
00216 
00217         if(go_to_ready): 
00218             left_pose.pose.position.x = 0.6
00219             left_pose.pose.position.y = -.5
00220             left_pose.pose.position.z = -.5
00221 
00222             right_pose.pose.position.x = 0.6
00223             right_pose.pose.position.y = .5
00224             right_pose.pose.position.z = -.5
00225 
00226             roll = -1.57
00227             pitch = 0
00228             yaw = 0
00229 
00230             q = quaternion_from_euler(roll, pitch, yaw)
00231             left_pose.pose.orientation.x = q[0]
00232             left_pose.pose.orientation.y = q[1]
00233             left_pose.pose.orientation.z = q[2]
00234             left_pose.pose.orientation.w = q[3]
00235             print left_pose.pose.orientation
00236 
00237 
00238         else :
00239 
00240             left_pose.pose.position.x = 0.6
00241             left_pose.pose.position.y = -.5
00242             left_pose.pose.position.z = -.7
00243 
00244             right_pose.pose.position.x = 0.6
00245             right_pose.pose.position.y = .5
00246             right_pose.pose.position.z = -.7
00247 
00248             roll = 0
00249             pitch = 0
00250             yaw = 0
00251             
00252             q = quaternion_from_euler(roll, pitch, yaw)
00253             left_pose.pose.orientation.x = q[0]
00254             left_pose.pose.orientation.y = q[1]
00255             left_pose.pose.orientation.z = q[2]
00256             left_pose.pose.orientation.w = q[3]
00257             print left_pose.pose.orientation
00258 
00259         waist_pub.publish(waist_js); #rospy.sleep(3)
00260 
00261         left_pose.header.seq = c
00262         left_pose.header.stamp = rospy.get_rostime()
00263         left_pose_pub.publish(left_pose); #rospy.sleep(0.2)
00264 
00265         right_pose.header.seq = c
00266         right_pose.header.stamp = rospy.get_rostime()
00267         right_pose_pub.publish(right_pose); #rospy.sleep(0.2)
00268 
00269         rospy.sleep(2)
00270         c += 1
00271 
00272     print "subscriber shutting down..."
00273 
00274 
00275 if __name__ == '__main__':
00276     try:
00277         send_command_data()
00278     except rospy.ROSInterruptException: pass