test_positions.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Author: John Hsu
00003 
00004 PKG = 'test_pr2_grasping'
00005 NAME = 'test_joint_position_controllers'
00006 
00007 import math
00008 import roslib
00009 roslib.load_manifest(PKG)
00010 roslib.load_manifest('rostest')
00011 
00012 import sys, unittest
00013 import os, os.path, threading, time
00014 import rospy, rostest
00015 import actionlib
00016 from std_msgs.msg import String, Float64
00017 from geometry_msgs.msg import Pose,Quaternion,Point, PoseStamped, PoseWithCovariance, PoseWithCovarianceStamped
00018 from pr2_controllers_msgs.msg import Pr2GripperCommand, JointControllerState
00019 from trajectory_msgs.msg import JointTrajectory
00020 from actionlib_msgs.msg import GoalStatusArray
00021 import tf.transformations as tft
00022 from numpy import float64
00023 
00024 def positionState(state):
00025   print "set_point : ",state.set_point
00026   print "process_value : ",state.process_value
00027   print "command : ",state.command
00028   print "error : ",state.error
00029 
00030 def gripperState(state):
00031   print "set_point : ",state.set_point
00032   print "process_value : ",state.process_value
00033   print "command : ",state.command
00034   print "error : ",state.error
00035 
00036 if __name__ == '__main__':
00037 
00038   sub_top = {}
00039   sub_top[0]  = "/l_elbow_flex_position_controller/state"    
00040   sub_top[1]  = "/l_forearm_roll_position_controller/state"  
00041   sub_top[2]  = "/l_gripper_position_controller/state"       
00042   sub_top[3]  = "/l_shoulder_lift_position_controller/state" 
00043   sub_top[4]  = "/l_shoulder_pan_position_controller/state"  
00044   sub_top[5]  = "/l_upper_arm_roll_position_controller/state"
00045   sub_top[6]  = "/l_wrist_flex_position_controller/state"    
00046   sub_top[7]  = "/l_wrist_roll_position_controller/state"    
00047   sub_top[8]  = "/r_elbow_flex_position_controller/state"    
00048   sub_top[9]  = "/r_forearm_roll_position_controller/state"  
00049   sub_top[10] = "/r_gripper_position_controller/state"       
00050   sub_top[11] = "/r_shoulder_lift_position_controller/state" 
00051   sub_top[12] = "/r_shoulder_pan_position_controller/state"  
00052   sub_top[13] = "/r_upper_arm_roll_position_controller/state"
00053   sub_top[14] = "/r_wrist_flex_position_controller/state"    
00054   sub_top[15] = "/r_wrist_roll_position_controller/state"    
00055   sub_top[16] = "/torso_lift_position_controller/state"      
00056   sub_top[17] = "/l_gripper_controller/state"                
00057   sub_top[18] = "/r_gripper_controller/state"                
00058   sub_top[19] = "/torso_controller/state"                    
00059 
00060   pub_top = {}
00061   pub_top[0]  = "/l_elbow_flex_position_controller/command"    
00062   pub_top[1]  = "/l_forearm_roll_position_controller/command"  
00063   pub_top[2]  = "/l_gripper_position_controller/command"       
00064   pub_top[3]  = "/l_shoulder_lift_position_controller/command" 
00065   pub_top[4]  = "/l_shoulder_pan_position_controller/command"  
00066   pub_top[5]  = "/l_upper_arm_roll_position_controller/command"
00067   pub_top[6]  = "/l_wrist_flex_position_controller/command"    
00068   pub_top[7]  = "/l_wrist_roll_position_controller/command"    
00069   pub_top[8]  = "/r_elbow_flex_position_controller/command"    
00070   pub_top[9]  = "/r_forearm_roll_position_controller/command"  
00071   pub_top[10] = "/r_gripper_position_controller/command"       
00072   pub_top[11] = "/r_shoulder_lift_position_controller/command" 
00073   pub_top[12] = "/r_shoulder_pan_position_controller/command"  
00074   pub_top[13] = "/r_upper_arm_roll_position_controller/command"
00075   pub_top[14] = "/r_wrist_flex_position_controller/command"    
00076   pub_top[15] = "/r_wrist_roll_position_controller/command"    
00077   pub_top[16] = "/torso_lift_position_controller/command"      
00078   pub_top[17] = "/l_gripper_controller/command"                
00079   pub_top[18] = "/r_gripper_controller/command"                
00080   pub_top[19] = "/torso_controller/command"                    
00081 
00082   pub_cmd = {}
00083   pub_cmd[0]  = rospy.Publisher(pub_top[0]  , Float64)
00084   pub_cmd[1]  = rospy.Publisher(pub_top[1]  , Float64)
00085   pub_cmd[2]  = rospy.Publisher(pub_top[2]  , Float64)
00086   pub_cmd[3]  = rospy.Publisher(pub_top[3]  , Float64)
00087   pub_cmd[4]  = rospy.Publisher(pub_top[4]  , Float64)
00088   pub_cmd[5]  = rospy.Publisher(pub_top[5]  , Float64)
00089   pub_cmd[6]  = rospy.Publisher(pub_top[6]  , Float64)
00090   pub_cmd[7]  = rospy.Publisher(pub_top[7]  , Float64)
00091   pub_cmd[8]  = rospy.Publisher(pub_top[8]  , Float64)
00092   pub_cmd[9]  = rospy.Publisher(pub_top[9]  , Float64)
00093   pub_cmd[10] = rospy.Publisher(pub_top[10] , Float64)
00094   pub_cmd[11] = rospy.Publisher(pub_top[11] , Float64)
00095   pub_cmd[12] = rospy.Publisher(pub_top[12] , Float64)
00096   pub_cmd[13] = rospy.Publisher(pub_top[13] , Float64)
00097   pub_cmd[14] = rospy.Publisher(pub_top[14] , Float64)
00098   pub_cmd[15] = rospy.Publisher(pub_top[15] , Float64)
00099   pub_cmd[16] = rospy.Publisher(pub_top[16] , Float64)
00100   pub_cmd[17] = rospy.Publisher(pub_top[17] , Pr2GripperCommand)
00101   pub_cmd[18] = rospy.Publisher(pub_top[18] , Pr2GripperCommand)
00102   pub_cmd[19] = rospy.Publisher(pub_top[19] , JointTrajectory)
00103 
00104   rospy.init_node(NAME, anonymous=False)
00105 
00106   rospy.Subscriber(sub_top[0]  , JointControllerState, positionState)
00107 
00108   cmd = Float64()
00109   cmd.data = -0.3
00110 
00111   timeout = 10.0
00112   while True:
00113     cmd.data = cmd.data * -1
00114     start_time = time.time()
00115     end_time = start_time + timeout
00116 
00117     while time.time() < end_time:
00118       pub_cmd[0] .publish( cmd )
00119       pub_cmd[1] .publish( cmd )
00120       pub_cmd[2] .publish( cmd )
00121       pub_cmd[3] .publish( cmd )
00122       pub_cmd[4] .publish( cmd )
00123       pub_cmd[5] .publish( cmd )
00124       pub_cmd[6] .publish( cmd )
00125       pub_cmd[7] .publish( cmd )
00126       pub_cmd[8] .publish( cmd )
00127       pub_cmd[9] .publish( cmd )
00128       pub_cmd[10].publish( cmd )
00129       pub_cmd[11].publish( cmd )
00130       pub_cmd[12].publish( cmd )
00131       pub_cmd[13].publish( cmd )
00132       pub_cmd[14].publish( cmd )
00133       pub_cmd[15].publish( cmd )
00134       pub_cmd[16].publish( cmd )
00135       #pub_cmd[17].publish(0)
00136       #pub_cmd[18].publish(0)
00137       #pub_cmd[19].publish(0)
00138       time.sleep(0.1)


pr2_gazebo
Author(s): John Hsu
autogenerated on Thu Apr 24 2014 15:48:38