effort.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 # Copyright (c) 2008, Willow Garage, Inc.
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # This script brings up an effort controller on your joint of choice
00030 # and allows you to type in the desired efforts.
00031 #
00032 # Author: Stuart Glaser
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     # Override rospy's signal handling.  We'll invoke rospy's handler after
00065     # we're done shutting down
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()


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Thu Aug 27 2015 14:22:45