controller_manager.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#####
#####
# This script is copied from https://github.com/arebgun/dynamixel_motor/pull/28
#####
#####
#
# Software License Agreement (BSD License)
#
# Copyright (c) 2010, Antons Rebguns, Cody Jorgensen, Cara Slutter
#               2010-2011, Antons Rebguns
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of University of Arizona nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.


__author__ = 'Antons Rebguns, Cody Jorgensen, Cara Slutter'
__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns, Cody Jorgensen, Cara Slutter'

__license__ = 'BSD'
__maintainer__ = 'Antons Rebguns'
__email__ = 'anton@email.arizona.edu'


from threading import Thread, Lock

import sys
from optparse import OptionParser

import rospy

#from dynamixel_driver.dynamixel_serial_proxy import SerialProxy
#from dynamixel_driver.dynamixel_dummy_proxy import DummyProxy

from threading import Thread, Lock

from dynamixel_driver.dynamixel_serial_proxy import SerialProxy

import rospy

class DummyDynamixelIO(object):
    """ Provides low level IO with the Dynamixel servos through pyserial. Has the
    ability to write instruction packets, request and read register value
    packets, send and receive a response to a ping packet, and send a SYNC WRITE
    multi-servo instruction packet.
    """

    def __init__(self, port, baudrate, max_motor_id, readback_echo=False):
        """ Constructor takes serial port and baudrate as arguments. """
        self.serial_mutex = Lock()
        self.port_name = port
        self.readback_echo = readback_echo

        self.goal = [2048]*(max_motor_id+1)
        self.position = [2048]*(max_motor_id+1)
        self.speed = [0]*(max_motor_id+1)

    def ping(self, servo_id):
        self.goal[servo_id] = 2048
        self.position[servo_id] = 2048
        self.speed[servo_id] = 0
        return True;

    def get_model_number(self, servo_id):
        """ Reads the servo's model number (e.g. 12 for AX-12+). """
        return 12

    def get_angle_limits(self, servo_id):
        cwLimit = 0
        ccwLimit = 4096
        return {'min':cwLimit, 'max':ccwLimit}

    def get_voltage(self, servo_id):
        return 12

    def get_voltage_limits(self, servo_id):
        # return the data in a dictionary
        min_voltage = 8
        max_voltage = 12
        return {'min':min_voltage, 'max':max_voltage}

    def get_position(self, servo_id):
        return self.position[servo_id]

    def get_speed(self, servo_id):
        return self.speed[servo_id]

    def get_current(self, servo_id):
        return 0

    def get_firmware_version(self, servo_id):
        return None

    def get_return_delay_time(self, servo_id):
        """ Reads the servo's return delay time. """
        return 5

    def get_feedback(self, servo_id):
        """
        Returns the id, goal, position, error, speed, load, voltage, temperature
        and moving values from the specified servo.
        """

        error = 0
        if self.speed[servo_id] != 0 :
            speed = self.speed[servo_id]
            self.goal[servo_id] += speed
        else:
            speed = (self.goal[servo_id] -  self.position[servo_id])*0.1
        self.position[servo_id] += speed
        return { 'timestamp': rospy.Time.now().to_sec(),
                 'id': servo_id,
                 'goal': self.goal[servo_id],
                 'position': self.position[servo_id] % 4096,
                 'error': error,
                 'speed': speed,
                 'load': 0}

    def set_multi_speed(self, valueTuples):
        for servo_id,val in valueTuples:
            self.speed[servo_id] = val
        return True

    def set_multi_position(self, valueTuples):
        for servo_id,val in valueTuples:
            self.goal[servo_id] = val
            self.speed[servo_id] = 0
        return True


class DummyProxy(SerialProxy):
    def connect(self):
        self.dxl_io = DummyDynamixelIO(self.port_name, self.baud_rate, self.max_motor_id) # self.readback_echo: release deb version does not have readback_echo
        self._SerialProxy__find_motors()
        self.running = True
        if self.update_rate > 0: Thread(target=self._SerialProxy__update_motor_states).start()
        if self.diagnostics_rate > 0: Thread(target=self._SerialProxy__publish_diagnostic_information).start()
##


from diagnostic_msgs.msg import DiagnosticArray
from diagnostic_msgs.msg import DiagnosticStatus
from diagnostic_msgs.msg import KeyValue

from dynamixel_controllers.srv import StartController
from dynamixel_controllers.srv import StartControllerResponse
from dynamixel_controllers.srv import StopController
from dynamixel_controllers.srv import StopControllerResponse
from dynamixel_controllers.srv import RestartController
from dynamixel_controllers.srv import RestartControllerResponse

class ControllerManager:
    def __init__(self):
        rospy.init_node('dynamixel_controller_manager', anonymous=True)
        rospy.on_shutdown(self.on_shutdown)
        
        self.waiting_meta_controllers = []
        self.controllers = {}
        self.serial_proxies = {}
        self.diagnostics_rate = rospy.get_param('~diagnostics_rate', 1)
        
        self.start_controller_lock = Lock()
        self.stop_controller_lock = Lock()

        manager_namespace = rospy.get_param('~namespace')
        serial_ports = rospy.get_param('~serial_ports')
        
        for port_namespace,port_config in serial_ports.items():
            port_name = port_config['port_name']
            baud_rate = port_config['baud_rate']
            readback_echo = port_config['readback_echo'] if 'readback_echo' in port_config else False
            min_motor_id = port_config['min_motor_id'] if 'min_motor_id' in port_config else 0
            max_motor_id = port_config['max_motor_id'] if 'max_motor_id' in port_config else 253
            update_rate = port_config['update_rate'] if 'update_rate' in port_config else 5
            error_level_temp = 75
            warn_level_temp = 70
            
            if 'diagnostics' in port_config:
                if 'error_level_temp' in port_config['diagnostics']:
                    error_level_temp = port_config['diagnostics']['error_level_temp']
                if 'warn_level_temp' in port_config['diagnostics']:
                    warn_level_temp = port_config['diagnostics']['warn_level_temp']
                    
            serial_proxy = SerialProxy(port_name=port_name,
                                       port_namespace=port_namespace,
                                       baud_rate=baud_rate,
                                       min_motor_id=min_motor_id,
                                       max_motor_id=max_motor_id,
                                       update_rate=update_rate,
                                       diagnostics_rate=self.diagnostics_rate,
                                       error_level_temp=error_level_temp,
                                       warn_level_temp=warn_level_temp)
                                       # readback_echo=readback_echo ## this will not work on deb version
            serial_proxy.connect()
            
            # will create a set of services for each serial port under common manager namesapce
            # e.g. /dynamixel_manager/robot_arm_port/start_controller
            #      /dynamixel_manager/robot_head_port/start_controller
            # where 'dynamixel_manager' is manager's namespace
            #       'robot_arm_port' and 'robot_head_port' are human readable names for serial ports
            rospy.Service('%s/%s/start_controller' % (manager_namespace, port_namespace), StartController, self.start_controller)
            rospy.Service('%s/%s/stop_controller' % (manager_namespace, port_namespace), StopController, self.stop_controller)
            rospy.Service('%s/%s/restart_controller' % (manager_namespace, port_namespace), RestartController, self.restart_controller)
            
            self.serial_proxies[port_namespace] = serial_proxy
            
        # services for 'meta' controllers, e.g. joint trajectory controller
        # these controllers don't have their own serial port, instead they rely
        # on regular controllers for serial connection. The advantage of meta
        # controller is that it can pack commands for multiple motors on multiple
        # serial ports.
        # NOTE: all serial ports that meta controller needs should be managed by
        # the same controler manager.
        rospy.Service('%s/meta/start_controller' % manager_namespace, StartController, self.start_controller)
        rospy.Service('%s/meta/stop_controller' % manager_namespace, StopController, self.stop_controller)
        rospy.Service('%s/meta/restart_controller' % manager_namespace, RestartController, self.restart_controller)
        
        self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
        if self.diagnostics_rate > 0: Thread(target=self.diagnostics_processor).start()

    def on_shutdown(self):
        for serial_proxy in self.serial_proxies.values():
            serial_proxy.disconnect()

    def diagnostics_processor(self):
        diag_msg = DiagnosticArray()
        
        rate = rospy.Rate(self.diagnostics_rate)
        while not rospy.is_shutdown():
            diag_msg.status = []
            diag_msg.header.stamp = rospy.Time.now()
            
            for controller in self.controllers.values():
                try:
                    joint_state = controller.joint_state
                    temps = joint_state.motor_temps
                    max_temp = max(temps)
                    
                    status = DiagnosticStatus()
                    status.name = 'Joint Controller (%s)' % controller.joint_name
                    status.hardware_id = 'Robotis Dynamixel %s on port %s' % (str(joint_state.motor_ids), controller.port_namespace)
                    status.values.append(KeyValue('Goal', str(joint_state.goal_pos)))
                    status.values.append(KeyValue('Position', str(joint_state.current_pos)))
                    status.values.append(KeyValue('Error', str(joint_state.error)))
                    status.values.append(KeyValue('Velocity', str(joint_state.velocity)))
                    status.values.append(KeyValue('Load', str(joint_state.load)))
                    status.values.append(KeyValue('Moving', str(joint_state.is_moving)))
                    status.values.append(KeyValue('Temperature', str(max_temp)))
                    status.level = DiagnosticStatus.OK
                    status.message = 'OK'
                        
                    diag_msg.status.append(status)
                except:
                    pass
                    
            self.diagnostics_pub.publish(diag_msg)
            rate.sleep()

    def check_deps(self):
        controllers_still_waiting = []
        
        for i,(controller_name,deps,kls) in enumerate(self.waiting_meta_controllers):
            if not set(deps).issubset(self.controllers.keys()):
                controllers_still_waiting.append(self.waiting_meta_controllers[i])
                rospy.logwarn('[%s] not all dependencies started, still waiting for %s...' % (controller_name, str(list(set(deps).difference(self.controllers.keys())))))
            else:
                dependencies = [self.controllers[dep_name] for dep_name in deps]
                controller = kls(controller_name, dependencies)
                
                if controller.initialize():
                    controller.start()
                    self.controllers[controller_name] = controller
                    
        self.waiting_meta_controllers = controllers_still_waiting[:]

    def start_controller(self, req):
        port_name = req.port_name
        package_path = req.package_path
        module_name = req.module_name
        class_name = req.class_name
        controller_name = req.controller_name
        
        self.start_controller_lock.acquire()
        
        if controller_name in self.controllers:
            self.start_controller_lock.release()
            return StartControllerResponse(False, 'Controller [%s] already started. If you want to restart it, call restart.' % controller_name)
            
        try:
            if module_name not in sys.modules:
                # import if module not previously imported
                package_module = __import__(package_path, globals(), locals(), [module_name], -1)
            else:
                # reload module if previously imported
                package_module = reload(sys.modules[package_path])
            controller_module = getattr(package_module, module_name)
        except ImportError, ie:
            self.start_controller_lock.release()
            return StartControllerResponse(False, 'Cannot find controller module. Unable to start controller %s\n%s' % (module_name, str(ie)))
        except SyntaxError, se:
            self.start_controller_lock.release()
            return StartControllerResponse(False, 'Syntax error in controller module. Unable to start controller %s\n%s' % (module_name, str(se)))
        except Exception, e:
            self.start_controller_lock.release()
            return StartControllerResponse(False, 'Unknown error has occured. Unable to start controller %s\n%s' % (module_name, str(e)))
        
        kls = getattr(controller_module, class_name)
        
        if port_name == 'meta':
            self.waiting_meta_controllers.append((controller_name,req.dependencies,kls))
            self.check_deps()
            self.start_controller_lock.release()
            return StartControllerResponse(True, '')
            
        if port_name != 'meta' and (port_name not in self.serial_proxies):
            self.start_controller_lock.release()
            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))
            
        controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name)
        
        if controller.initialize():
            controller.start()
            self.controllers[controller_name] = controller
            
            self.check_deps()
            self.start_controller_lock.release()
            
            return StartControllerResponse(True, 'Controller %s successfully started.' % controller_name)
        else:
            self.start_controller_lock.release()
            return StartControllerResponse(False, 'Initialization failed. Unable to start controller %s' % controller_name)

    def stop_controller(self, req):
        controller_name = req.controller_name
        self.stop_controller_lock.acquire()
        
        if controller_name in self.controllers:
            self.controllers[controller_name].stop()
            del self.controllers[controller_name]
            self.stop_controller_lock.release()
            return StopControllerResponse(True, 'controller %s successfully stopped.' % controller_name)
        else:
            self.self.stop_controller_lock.release()
            return StopControllerResponse(False, 'controller %s was not running.' % controller_name)

    def restart_controller(self, req):
        response1 = self.stop_controller(StopController(req.controller_name))
        response2 = self.start_controller(req)
        return RestartControllerResponse(response1.success and response2.success, '%s\n%s' % (response1.reason, response2.reason))

if __name__ == '__main__':
    try:
        usage_msg = 'Usage: %prog [options]'
        parser = OptionParser(usage=usage_msg)
        parser.add_option('-d', '--dummy', default=False,
                          help='running manager in dummy mode, which did not connect to actual haradware device')
        (options, args) = parser.parse_args(rospy.myargv()[1:])
        if options.dummy:
            SerialProxy = DummyProxy
        manager = ControllerManager()
        rospy.spin()
    except rospy.ROSInterruptException: pass