Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import random
00035 CONTROLLER_NAME = "quick_effort_controller_%08d" % random.randint(0,10**8-1)
00036
00037 import sys
00038 import signal
00039 import roslib
00040 roslib.load_manifest('robot_mechanism_controllers')
00041 import rospy
00042 from std_msgs.msg import *
00043 from pr2_controller_manager import pr2_controller_manager_interface
00044
00045 prev_handler = None
00046
00047 def shutdown(sig, stackframe):
00048 pr2_controller_manager_interface.stop_controller(CONTROLLER_NAME)
00049 pr2_controller_manager_interface.unload_controller(CONTROLLER_NAME)
00050 if prev_handler is not None:
00051 prev_handler(signal.SIGINT,None)
00052
00053 def load_joint_config(joint_name):
00054 rospy.set_param(CONTROLLER_NAME+'/type', 'JointEffortController')
00055 rospy.set_param(CONTROLLER_NAME+'/joint', joint_name)
00056
00057 def main():
00058 if len(sys.argv) < 2:
00059 print "Usage: effort.py <joint>"
00060 sys.exit(1)
00061 joint = sys.argv[1]
00062 rospy.init_node('effort', anonymous=True)
00063
00064
00065
00066 prev_handler = signal.getsignal(signal.SIGINT)
00067 signal.signal(signal.SIGINT, shutdown)
00068
00069 load_joint_config(joint)
00070 pr2_controller_manager_interface.load_controller(CONTROLLER_NAME)
00071 pr2_controller_manager_interface.start_controller(CONTROLLER_NAME)
00072
00073 pub = rospy.Publisher("%s/command" % CONTROLLER_NAME, Float64)
00074
00075 print "Enter efforts:"
00076 while not rospy.is_shutdown():
00077 effort = float(sys.stdin.readline().strip())
00078 pub.publish(Float64(effort))
00079
00080 if __name__ == '__main__':
00081 main()