dynamixel_controllers: controller_manager.py Source File

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, Antons Rebguns, Cody Jorgensen, Cara Slutter
00007 #               2010-2011, Antons Rebguns
00008 # All rights reserved.
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions
00012 # are met:
00013 #
00014 #  * Redistributions of source code must retain the above copyright
00015 #    notice, this list of conditions and the following disclaimer.
00016 #  * Redistributions in binary form must reproduce the above
00017 #    copyright notice, this list of conditions and the following
00018 #    disclaimer in the documentation and/or other materials provided
00019 #    with the distribution.
00020 #  * Neither the name of University of Arizona nor the names of its
00021 #    contributors may be used to endorse or promote products derived
00022 #    from this software without specific prior written permission.
00023 #
00024 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 # POSSIBILITY OF SUCH DAMAGE.
00036 
00037 
00038 __author__ = 'Antons Rebguns, Cody Jorgensen, Cara Slutter'
00039 __copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns, Cody Jorgensen, Cara Slutter'
00040 
00041 __license__ = 'BSD'
00042 __maintainer__ = 'Antons Rebguns'
00043 __email__ = 'anton@email.arizona.edu'
00044 
00045 
00046 from threading import Thread, Lock
00047 
00048 import sys
00049 
00050 import rospy
00051 
00052 from dynamixel_driver.dynamixel_serial_proxy import SerialProxy
00053 
00054 from diagnostic_msgs.msg import DiagnosticArray
00055 from diagnostic_msgs.msg import DiagnosticStatus
00056 from diagnostic_msgs.msg import KeyValue
00057 
00058 from dynamixel_controllers.srv import StartController
00059 from dynamixel_controllers.srv import StartControllerResponse
00060 from dynamixel_controllers.srv import StopController
00061 from dynamixel_controllers.srv import StopControllerResponse
00062 from dynamixel_controllers.srv import RestartController
00063 from dynamixel_controllers.srv import RestartControllerResponse
00064 
00065 class ControllerManager:
00066     def __init__(self):
00067         rospy.init_node('dynamixel_controller_manager', anonymous=True)
00068         rospy.on_shutdown(self.on_shutdown)
00069         
00070         self.waiting_meta_controllers = []
00071         self.controllers = {}
00072         self.serial_proxies = {}
00073         self.diagnostics_rate = rospy.get_param('~diagnostics_rate', 1)
00074         
00075         self.start_controller_lock = Lock()
00076         self.stop_controller_lock = Lock()
00077 
00078         manager_namespace = rospy.get_param('~namespace')
00079         serial_ports = rospy.get_param('~serial_ports')
00080         
00081         for port_namespace,port_config in serial_ports.items():
00082             port_name = port_config['port_name']
00083             baud_rate = port_config['baud_rate']
00084             readback_echo = port_config['readback_echo'] if 'readback_echo' in port_config else False
00085             min_motor_id = port_config['min_motor_id'] if 'min_motor_id' in port_config else 0
00086             max_motor_id = port_config['max_motor_id'] if 'max_motor_id' in port_config else 253
00087             update_rate = port_config['update_rate'] if 'update_rate' in port_config else 5
00088             error_level_temp = 75
00089             warn_level_temp = 70
00090             
00091             if 'diagnostics' in port_config:
00092                 if 'error_level_temp' in port_config['diagnostics']:
00093                     error_level_temp = port_config['diagnostics']['error_level_temp']
00094                 if 'warn_level_temp' in port_config['diagnostics']:
00095                     warn_level_temp = port_config['diagnostics']['warn_level_temp']
00096                     
00097             serial_proxy = SerialProxy(port_name,
00098                                        port_namespace,
00099                                        baud_rate,
00100                                        min_motor_id,
00101                                        max_motor_id,
00102                                        update_rate,
00103                                        self.diagnostics_rate,
00104                                        error_level_temp,
00105                                        warn_level_temp,
00106                                        readback_echo)
00107             serial_proxy.connect()
00108             
00109             # will create a set of services for each serial port under common manager namesapce
00110             # e.g. /dynamixel_manager/robot_arm_port/start_controller
00111             #      /dynamixel_manager/robot_head_port/start_controller
00112             # where 'dynamixel_manager' is manager's namespace
00113             #       'robot_arm_port' and 'robot_head_port' are human readable names for serial ports
00114             rospy.Service('%s/%s/start_controller' % (manager_namespace, port_namespace), StartController, self.start_controller)
00115             rospy.Service('%s/%s/stop_controller' % (manager_namespace, port_namespace), StopController, self.stop_controller)
00116             rospy.Service('%s/%s/restart_controller' % (manager_namespace, port_namespace), RestartController, self.restart_controller)
00117             
00118             self.serial_proxies[port_namespace] = serial_proxy
00119             
00120         # services for 'meta' controllers, e.g. joint trajectory controller
00121         # these controllers don't have their own serial port, instead they rely
00122         # on regular controllers for serial connection. The advantage of meta
00123         # controller is that it can pack commands for multiple motors on multiple
00124         # serial ports.
00125         # NOTE: all serial ports that meta controller needs should be managed by
00126         # the same controler manager.
00127         rospy.Service('%s/meta/start_controller' % manager_namespace, StartController, self.start_controller)
00128         rospy.Service('%s/meta/stop_controller' % manager_namespace, StopController, self.stop_controller)
00129         rospy.Service('%s/meta/restart_controller' % manager_namespace, RestartController, self.restart_controller)
00130         
00131         self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00132         if self.diagnostics_rate > 0: Thread(target=self.diagnostics_processor).start()
00133 
00134     def on_shutdown(self):
00135         for serial_proxy in self.serial_proxies.values():
00136             serial_proxy.disconnect()
00137 
00138     def diagnostics_processor(self):
00139         diag_msg = DiagnosticArray()
00140         
00141         rate = rospy.Rate(self.diagnostics_rate)
00142         while not rospy.is_shutdown():
00143             diag_msg.status = []
00144             diag_msg.header.stamp = rospy.Time.now()
00145             
00146             for controller in self.controllers.values():
00147                 try:
00148                     joint_state = controller.joint_state
00149                     temps = joint_state.motor_temps
00150                     max_temp = max(temps)
00151                     
00152                     status = DiagnosticStatus()
00153                     status.name = 'Joint Controller (%s)' % controller.joint_name
00154                     status.hardware_id = 'Robotis Dynamixel %s on port %s' % (str(joint_state.motor_ids), controller.port_namespace)
00155                     status.values.append(KeyValue('Goal', str(joint_state.goal_pos)))
00156                     status.values.append(KeyValue('Position', str(joint_state.current_pos)))
00157                     status.values.append(KeyValue('Error', str(joint_state.error)))
00158                     status.values.append(KeyValue('Velocity', str(joint_state.velocity)))
00159                     status.values.append(KeyValue('Load', str(joint_state.load)))
00160                     status.values.append(KeyValue('Moving', str(joint_state.is_moving)))
00161                     status.values.append(KeyValue('Temperature', str(max_temp)))
00162                     status.level = DiagnosticStatus.OK
00163                     status.message = 'OK'
00164                         
00165                     diag_msg.status.append(status)
00166                 except:
00167                     pass
00168                     
00169             self.diagnostics_pub.publish(diag_msg)
00170             rate.sleep()
00171 
00172     def check_deps(self):
00173         controllers_still_waiting = []
00174         
00175         for i,(controller_name,deps,kls) in enumerate(self.waiting_meta_controllers):
00176             if not set(deps).issubset(self.controllers.keys()):
00177                 controllers_still_waiting.append(self.waiting_meta_controllers[i])
00178                 rospy.logwarn('[%s] not all dependencies started, still waiting for %s...' % (controller_name, str(list(set(deps).difference(self.controllers.keys())))))
00179             else:
00180                 dependencies = [self.controllers[dep_name] for dep_name in deps]
00181                 controller = kls(controller_name, dependencies)
00182                 
00183                 if controller.initialize():
00184                     controller.start()
00185                     self.controllers[controller_name] = controller
00186                     
00187         self.waiting_meta_controllers = controllers_still_waiting[:]
00188 
00189     def start_controller(self, req):
00190         port_name = req.port_name
00191         package_path = req.package_path
00192         module_name = req.module_name
00193         class_name = req.class_name
00194         controller_name = req.controller_name
00195         
00196         self.start_controller_lock.acquire()
00197         
00198         if controller_name in self.controllers:
00199             self.start_controller_lock.release()
00200             return StartControllerResponse(False, 'Controller [%s] already started. If you want to restart it, call restart.' % controller_name)
00201             
00202         try:
00203             if module_name not in sys.modules:
00204                 # import if module not previously imported
00205                 package_module = __import__(package_path, globals(), locals(), [module_name], -1)
00206             else:
00207                 # reload module if previously imported
00208                 package_module = reload(sys.modules[package_path])
00209             controller_module = getattr(package_module, module_name)
00210         except ImportError, ie:
00211             self.start_controller_lock.release()
00212             return StartControllerResponse(False, 'Cannot find controller module. Unable to start controller %s\n%s' % (module_name, str(ie)))
00213         except SyntaxError, se:
00214             self.start_controller_lock.release()
00215             return StartControllerResponse(False, 'Syntax error in controller module. Unable to start controller %s\n%s' % (module_name, str(se)))
00216         except Exception, e:
00217             self.start_controller_lock.release()
00218             return StartControllerResponse(False, 'Unknown error has occured. Unable to start controller %s\n%s' % (module_name, str(e)))
00219         
00220         kls = getattr(controller_module, class_name)
00221         
00222         if port_name == 'meta':
00223             self.waiting_meta_controllers.append((controller_name,req.dependencies,kls))
00224             self.check_deps()
00225             self.start_controller_lock.release()
00226             return StartControllerResponse(True, '')
00227             
00228         if port_name != 'meta' and (port_name not in self.serial_proxies):
00229             self.start_controller_lock.release()
00230             return StartControllerResponse(False, 'Specified port [%s] not found, available ports are %s. Unable to start controller %s' % (port_name, str(self.serial_proxies.keys()), controller_name))
00231             
00232         controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name)
00233         
00234         if controller.initialize():
00235             controller.start()
00236             self.controllers[controller_name] = controller
00237             
00238             self.check_deps()
00239             self.start_controller_lock.release()
00240             
00241             return StartControllerResponse(True, 'Controller %s successfully started.' % controller_name)
00242         else:
00243             self.start_controller_lock.release()
00244             return StartControllerResponse(False, 'Initialization failed. Unable to start controller %s' % controller_name)
00245 
00246     def stop_controller(self, req):
00247         controller_name = req.controller_name
00248         self.stop_controller_lock.acquire()
00249         
00250         if controller_name in self.controllers:
00251             self.controllers[controller_name].stop()
00252             del self.controllers[controller_name]
00253             self.stop_controller_lock.release()
00254             return StopControllerResponse(True, 'controller %s successfully stopped.' % controller_name)
00255         else:
00256             self.self.stop_controller_lock.release()
00257             return StopControllerResponse(False, 'controller %s was not running.' % controller_name)
00258 
00259     def restart_controller(self, req):
00260         response1 = self.stop_controller(StopController(req.controller_name))
00261         response2 = self.start_controller(req)
00262         return RestartControllerResponse(response1.success and response2.success, '%s\n%s' % (response1.reason, response2.reason))
00263 
00264 if __name__ == '__main__':
00265     try:
00266         manager = ControllerManager()
00267         rospy.spin()
00268     except rospy.ROSInterruptException: pass
00269