get_diagnostic_data.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('pr2_motor_diagnostic_tool')
00003 import rospy
00004 from pr2_motor_diagnostic_tool.srv import *
00005 from pr2_mechanism_msgs.srv import LoadController, UnloadController, SwitchController, SwitchControllerRequest, ListControllers
00006 from std_msgs.msg import Float64
00007 from sensor_msgs.msg import Joy
00008 import time
00009 import sys
00010 import argparse
00011 from yaml import dump
00012 
00013 load_controller = rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)
00014 unload_controller = rospy.ServiceProxy('pr2_controller_manager/unload_controller',UnloadController)
00015 switch_controller = rospy.ServiceProxy('pr2_controller_manager/switch_controller',SwitchController)
00016 list_controllers = rospy.ServiceProxy('pr2_controller_manager/list_controllers',ListControllers)
00017 
00018 flag1 = False 
00019 flag2 = False
00020 
00021 #dictionary of actuators and their resistances
00022 r_arm_actuators = ['r_wrist_r_motor','r_wrist_l_motor','r_forearm_roll_motor','r_upper_arm_roll_motor', 'r_elbow_flex_motor','r_shoulder_lift_motor','r_shoulder_pan_motor']
00023 l_arm_actuators = ['l_wrist_r_motor','l_wrist_l_motor','l_forearm_roll_motor','l_upper_arm_roll_motor', 'l_elbow_flex_motor','l_shoulder_lift_motor','l_shoulder_pan_motor']
00024 head_actuators = ['head_pan_motor','head_tilt_motor']
00025 
00026 def wait_for_X():
00027     global flag1
00028     flag1 = False
00029     while not (flag1):
00030       rospy.sleep(0.01)
00031 
00032 def wait_for_circle():
00033     global flag2
00034     flag2 = False
00035     while not (flag2):
00036       rospy.sleep(0.01) 
00037 
00038 def start_diag_controller(actuator_name):
00039   rospy.set_param('diagnostic_controller/type', 'pr2_motor_diagnostic_controller/DiagnosticControllerPlugin')
00040   rospy.set_param('diag_actuator_name',str(actuator_name))
00041   resp = load_controller('diagnostic_controller')
00042 
00043   if resp.ok:
00044     rospy.loginfo("loaded controller dianostic_controller")
00045     switch_controller(['diagnostic_controller'],[],SwitchControllerRequest.STRICT)
00046     rospy.loginfo("Running diagnostic controller")
00047   else:
00048     raise RuntimeError("Couldn't load contorller")
00049 
00050 def get_diag_data(actuator_name):
00051     #flag2 = False
00052     get_data = rospy.ServiceProxy('diagnostic_controller/get_diagnostic_data',DiagnosticData)
00053     rospy.loginfo("getting data for %s, wait 2 seconds",actuator_name)
00054 
00055     foo = DiagnosticDataRequest();
00056     rv = get_data(foo)
00057     stream = file(str(actuator_name) + "_results" + '.yaml', 'w')
00058     dump(rv,stream)
00059 
00060     switch_controller([],['diagnostic_controller'], SwitchControllerRequest.STRICT)
00061     rospy.loginfo("stopped diagnostic_controller")
00062 
00063     unload_controller("diagnostic_controller")
00064     rospy.loginfo("unload diagnostic_controller")
00065 
00066 
00067 def callback(data):
00068   global flag1
00069   global flag2
00070 
00071   if (data.buttons[14] == 1):
00072     flag1 = True
00073 
00074   if (data.buttons[13] == 1): 
00075     flag2 = True
00076 
00077 def main():
00078   rospy.init_node('get_data', anonymous=True)
00079   rospy.Subscriber("joy", Joy, callback)
00080   parser = argparse.ArgumentParser("script to get data for Pr2 arms for diagnostic analysis")
00081   parser.add_argument("parts", help="Specifiy left, right or both for the arms you want to get diagnostic data for and head for the head.")
00082   args = parser.parse_args()
00083   actuator_list = []
00084   if (args.parts == 'left'):
00085     actuator_list = l_arm_actuators
00086   elif (args.parts == 'right'): 
00087     actuator_list = r_arm_actuators
00088   elif (args.parts == 'both'):
00089     actuator_list = r_arm_actuators + l_arm_actuators
00090   elif (args.parts == 'head'):
00091     actuator_list = head
00092   else:
00093     print "Bad arguments, exiting"
00094     sys.exit()
00095 
00096   switch_controller([],['diagnostic_controller'],SwitchControllerRequest.STRICT)
00097   unload_controller('diagnostic_controller')
00098 
00099   for actuator_name in actuator_list:
00100     print "Press X to start or for next joint"
00101     wait_for_X() 
00102     start_diag_controller(actuator_name)
00103     print "start moving %s for about 5 seconds" %(actuator_name)
00104     rospy.sleep(5.0)
00105     print "press circle when your done"
00106     #rospy.loginfo("move %s for about 5 seconds and then press circle when done", joint_name)
00107     wait_for_circle()
00108     get_diag_data(actuator_name)
00109   
00110 if __name__ == '__main__':
00111   try:
00112     #test_controller()
00113     main()
00114   except rospy.ROSInterruptException: pass
00115 


pr2_motor_diagnostic_tool
Author(s): Rahul Udasi
autogenerated on Mon Sep 14 2015 14:39:22