controller_spawner.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2010-2011, Antons Rebguns.
00007 # All rights reserved.
00008 #
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions
00011 # are met:
00012 #
00013 #  * Redistributions of source code must retain the above copyright
00014 #    notice, this list of conditions and the following disclaimer.
00015 #  * Redistributions in binary form must reproduce the above
00016 #    copyright notice, this list of conditions and the following
00017 #    disclaimer in the documentation and/or other materials provided
00018 #    with the distribution.
00019 #  * Neither the name of University of Arizona nor the names of its
00020 #    contributors may be used to endorse or promote products derived
00021 #    from this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 # POSSIBILITY OF SUCH DAMAGE.
00035 
00036 
00037 __author__ = 'Antons Rebguns'
00038 __copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
00039 
00040 __license__ = 'BSD'
00041 __maintainer__ = 'Antons Rebguns'
00042 __email__ = 'anton@email.arizona.edu'
00043 
00044 
00045 import sys
00046 from optparse import OptionParser
00047 
00048 import roslib
00049 roslib.load_manifest('dynamixel_hardware_interface')
00050 
00051 import rospy
00052 from dynamixel_hardware_interface.srv import StartController
00053 from dynamixel_hardware_interface.srv import StopController
00054 from dynamixel_hardware_interface.srv import RestartController
00055 
00056 
00057 if __name__ == '__main__':
00058     try:
00059         usage_msg = 'Usage: %prog [options] CONTROLLER_NAMES'
00060         desc_msg = 'Allows to start, stop or restart joint controllers'
00061         epi_msg = 'Example: %s --manager=dxl_manager --port=dxl_port pan_controller tilt_controller' % sys.argv[0]
00062         
00063         parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
00064         parser.add_option('-m', '--manager', metavar='MANAGER',
00065                           help='specified serial port is managed by MANAGER')
00066         parser.add_option('-p', '--port', metavar='PORT',
00067                           help='motors of specified controllers are connected to PORT')
00068         parser.add_option('-c', '--command', metavar='COMMAND', default='start', choices=('start','stop','restart'),
00069                           help='command to perform on specified controllers: start, stop or restart [default: %default]')
00070                           
00071         (options, args) = parser.parse_args(rospy.myargv()[1:])
00072         
00073         if len(args) < 1:
00074             parser.error('specify at least one controller name')
00075             
00076         manager_namespace = options.manager
00077         port_namespace = options.port
00078         command = options.command
00079         joint_controllers = args
00080         
00081         start_service_name = '%s/start_controller' % manager_namespace
00082         stop_service_name = '%s/stop_controller' % manager_namespace
00083         restart_service_name = '%s/restart_controller' % manager_namespace
00084         
00085         rospy.init_node('controller_spawner', anonymous=True)
00086         
00087         node_name = rospy.get_name()[1:]
00088         parent_namespace = rospy.get_namespace()
00089         
00090         rospy.loginfo('%s: waiting for controller_manager %s to startup in %s namespace...' % (node_name, manager_namespace, parent_namespace))
00091         rospy.wait_for_service(start_service_name)
00092         rospy.wait_for_service(stop_service_name)
00093         rospy.wait_for_service(restart_service_name)
00094         
00095         start_controller = rospy.ServiceProxy(start_service_name, StartController)
00096         stop_controller = rospy.ServiceProxy(stop_service_name, StopController)
00097         restart_controller = rospy.ServiceProxy(restart_service_name, RestartController)
00098         
00099         rospy.loginfo('%s: all services are up, %sing %d controllers...' % (node_name, command.lower(), len(joint_controllers)))
00100         
00101         for controller_name in joint_controllers:
00102             try:
00103                 if command.lower() == 'start': response = start_controller(controller_name, port_namespace)
00104                 elif command.lower() == 'stop': response = stop_controller(controller_name)
00105                 elif command.lower() == 'restart': response = restart_controller(controller_name)
00106                 else: rospy.logerr("%s: invalid command '%s'" % (node_name, command)); parser.print_help(); continue
00107                 if response: rospy.loginfo("%s: command '%s %s' completed successufully" % (node_name, command.lower(), controller_name));
00108             except rospy.ServiceException, e:
00109                 rospy.logerr('%s: %s' % (node_name, e))
00110                     
00111     except rospy.ROSInterruptException: pass


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10