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 import os
00047 from optparse import OptionParser
00048 
00049 import rospy
00050 
00051 from dynamixel_controllers.srv import StartController
00052 from dynamixel_controllers.srv import StopController
00053 from dynamixel_controllers.srv import RestartController
00054 
00055 
00056 parser = OptionParser()
00057 
00058 def manage_controller(controller_name, port_namespace, controller_type, command, deps, start, stop, restart):
00059     try:
00060         controller = rospy.get_param(controller_name + '/controller')
00061         package_path = controller['package']
00062         module_name = controller['module']
00063         class_name = controller['type']
00064     except KeyError as ke:
00065         rospy.logerr('[%s] configuration error: could not find controller parameters on parameter server' % controller_name)
00066         sys.exit(1)
00067     except Exception as e:
00068         rospy.logerr('[%s]: %s' % (controller_name, e))
00069         sys.exit(1)
00070         
00071     if command.lower() == 'start':
00072         try:
00073             response = start(port_namespace, package_path, module_name, class_name, controller_name, deps)
00074             if response.success: rospy.loginfo(response.reason)
00075             else: rospy.logerr(response.reason)
00076         except rospy.ServiceException, e:
00077             rospy.logerr('Service call failed: %s' % e)
00078     elif command.lower() == 'stop':
00079         try:
00080             response = stop(controller_name)
00081             if response.success: rospy.loginfo(response.reason)
00082             else: rospy.logerr(response.reason)
00083         except rospy.ServiceException, e:
00084             rospy.logerr('Service call failed: %s' % e)
00085     elif command.lower() == 'restart':
00086         try:
00087             response = restart(port_namespace, package_path, module_name, class_name, controller_name, deps)
00088             if response.success: rospy.loginfo(response.reason)
00089             else: rospy.logerr(response.reason)
00090         except rospy.ServiceException, e:
00091             rospy.logerr('Service call failed: %s' % e)
00092     else:
00093         rospy.logerr('Invalid command.')
00094         parser.print_help()
00095 
00096 if __name__ == '__main__':
00097     try:
00098         rospy.init_node('controller_spawner', anonymous=True)
00099         
00100         parser.add_option('-m', '--manager', metavar='MANAGER',
00101                           help='specified serial port is managed by MANAGER')
00102         parser.add_option('-p', '--port', metavar='PORT',
00103                           help='motors of specified controllers are connected to PORT')
00104         parser.add_option('-t', '--type', metavar='TYPE', default='simple', choices=('simple','meta'),
00105                           help='type of controller to be loaded (simple|meta) [default: %default]')
00106         parser.add_option('-c', '--command', metavar='COMMAND', default='start', choices=('start','stop','restart'),
00107                           help='command to perform on specified controllers: start, stop or restart [default: %default]')
00108                           
00109         (options, args) = parser.parse_args(rospy.myargv()[1:])
00110         
00111         if len(args) < 1:
00112             parser.error('specify at least one controller name')
00113             
00114         manager_namespace = options.manager
00115         port_namespace = options.port
00116         controller_type = options.type
00117         command = options.command
00118         joint_controllers = args
00119         
00120         if controller_type == 'meta': port_namespace = 'meta'
00121         
00122         start_service_name = '%s/%s/start_controller' % (manager_namespace, port_namespace)
00123         stop_service_name = '%s/%s/stop_controller' % (manager_namespace, port_namespace)
00124         restart_service_name = '%s/%s/restart_controller' % (manager_namespace, port_namespace)
00125         
00126         parent_namespace = 'global' if rospy.get_namespace() == '/' else rospy.get_namespace()
00127         rospy.loginfo('%s controller_spawner: waiting for controller_manager %s to startup in %s namespace...' % (port_namespace, manager_namespace, parent_namespace))
00128         
00129         rospy.wait_for_service(start_service_name)
00130         rospy.wait_for_service(stop_service_name)
00131         rospy.wait_for_service(restart_service_name)
00132         
00133         start_controller = rospy.ServiceProxy(start_service_name, StartController)
00134         stop_controller = rospy.ServiceProxy(stop_service_name, StopController)
00135         restart_controller = rospy.ServiceProxy(restart_service_name, RestartController)
00136         
00137         rospy.loginfo('%s controller_spawner: All services are up, spawning controllers...' % port_namespace)
00138         
00139         if controller_type == 'simple':
00140             for controller_name in joint_controllers:
00141                 manage_controller(controller_name, port_namespace, controller_type, command, [], start_controller, stop_controller, restart_controller)
00142         elif controller_type == 'meta':
00143             controller_name = joint_controllers[0]
00144             dependencies = joint_controllers[1:]
00145             manage_controller(controller_name, port_namespace, controller_type, command, dependencies, start_controller, stop_controller, restart_controller)
00146     except rospy.ROSInterruptException: pass
00147 


dynamixel_controllers
Author(s): Antons Rebguns, Cody Jorgensen, Cara Slutter
autogenerated on Wed Aug 26 2015 11:24:38