manager.py

Go to the documentation of this file.

#! /usr/bin/python
#***********************************************************
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# 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 the Willow Garage 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: Wim Meeussen

from __future__ import with_statement

import roslib; roslib.load_manifest('application_manager')
import rospy
import os
import threading
import actionlib
from actionlib_msgs.msg import GoalStatus
import yaml
from application_manager import roslauncher
from application_msgs.msg import *




class Application():
    def __init__(self):
        self.state = 'Stopped' # state of the application (Running, Stopped, Preempted)
        self.client_goals = [] # list of clients that are connected to this application
        

    def configure(self, conf):
        self.lock = threading.Lock()  # lock for interacting with this application

        # check required keys
        required_keys = ['package', 'name', 'description', 'launch_file', 'resources', 'dependencies']
        for k in required_keys:
            if not k in conf:
                rospy.logerr('Application in package %s does not specify option "%s"'%(conf['package'], k))
                return False

        self.resources = conf['resources']
        self.depends = conf['dependencies']
        self.depends_on = []
        self.conflicts = []
        self.name = '/'.join([conf['package'], conf['name']])
        self.description = conf['description']
        try:
            if conf['launch_file'][0] == '/':
                with open(os.path.join(conf['launch_file'])) as f:
                    self.launch_file = f.read()
            else:
                with open(os.path.join(roslib.packages.get_pkg_dir(conf['package']), conf['launch_file'])) as f:
                    self.launch_file = f.read()
        except:
            rospy.logerr('Application %s specified invalid package or launch file'%conf['name'])
            return False

        # optional action interface
        self.action_ns = None
        if 'action_ns' in conf and 'preempt_timeout' in conf:
            self.action_ns = conf['action_ns']
            self.preempt_timeout = rospy.Duration(conf['preempt_timeout'])
            self.launch_timeout = rospy.Duration(30.0)
            self.action_client = None

        # optional daemon parameter
        self.daemonize = False
        if 'daemonize' in conf:
            self.daemonize = conf['daemonize']

        return True



    def start(self, wait):
        self.lock.acquire() # this lock gets released from other thread

        # check if application is already running
        if self.state == 'Running':
            rospy.loginfo('Applicaiton %s already running, not starting it again'%self.name)
            self.lock.release()
            return True

        # start launch file (from this thread, which is main thread)
        self.state = 'Starting'
        self.launcher = roslauncher.ScriptRoslaunch(self.launch_file)
        self.launcher.start()

        # start application in separate thread, so we don't block the manager
        self.application_starter = ApplicationStarter(self)
        self.application_starter.start() # this thread releases the lock

        # wait for start to finish if 'wait' is set to true
        while not rospy.is_shutdown() and wait and self.state != 'Running':
            rospy.sleep(0.1)
        return True



    def stop(self, wait):
        self.lock.acquire() # this lock gets released from other thread

        # check if application is running
        if self.state != 'Running':
            rospy.loginfo("Applicaiton %s is not running, so don't need to stop it"%self.name)
            self.lock.release()
            return True

        # notify all clients that are still connected
        for g in self.client_goals:
            g.set_canceled()
        self.client_goals = []

        # shut down application in separate thread, so we don't block the manager
        self.state = 'Preempting'
        self.application_stopper = ApplicationStopper(self)
        self.application_stopper.start()  # this thread releases the lock

        # wait for stop to finish if 'wait' is set to true
        while not rospy.is_shutdown() and wait and self.state != 'Stopped':
            rospy.sleep(0.1)
        return True


    def done_cb(self, status, result):
        with self.lock:
            if self.state == 'Running':
                rospy.loginfo("Application %s stopped itself. Taking down launchfile"%self.name)
        self.stop(wait=False)


class ApplicationStarter(threading.Thread):
    def __init__(self, application):
        threading.Thread.__init__(self)
        self.application = application


    def run(self):
        rospy.loginfo('Application starter thread created')

        # start action client if application supports this
        if self.application.action_ns:

            rospy.loginfo('Waiting for action client to come up in namespace %s for application %s'%(self.application.action_ns, self.application.name))
            self.application.action_client = actionlib.SimpleActionClient(self.application.action_ns, ApplicationAction)
            if not self.application.action_client.wait_for_server(self.application.launch_timeout):
                rospy.logerr('Action client server for application %s did not connect in time.'%self.application.name)                    
                self.application.launcher.shutdown()
                self.application.launcher = None
                self.application.state = 'Stopped'
                self.application.lock.release() # this lock got acquired in other thread
                return False
            self.application.action_client.send_goal(ApplicationGoal(), done_cb=self.application.done_cb)
            start_time = rospy.Time.now()
            while self.application.action_client.get_state() != GoalStatus.ACTIVE:
                rospy.sleep(0.1)
                if rospy.Time.now() > start_time + self.application.launch_timeout:
                    rospy.logerr('Action client server for application %s did not accept start goal in time.'%self.application.name)
                    self.application.launcher.shutdown()
                    self.application.launcher = None
                    self.application.state = 'Stopped'
                    self.application.lock.release() # this lock got acquired in other thread
                    return False

        self.application.state = 'Running'
        rospy.loginfo('Started application %s'%self.application.name)
        self.application.lock.release() # this lock got acquired in other thread
        return True




class ApplicationStopper(threading.Thread):
    def __init__(self, application):
        threading.Thread.__init__(self)
        self.application = application

    def run(self):
        rospy.loginfo('Application stopper thread created')
        # preempt application if this is supported
        if self.application.action_ns:
            rospy.loginfo("Sending preempt signal to application %s"%self.application.name)
            self.application.action_client.cancel_goal()
            self.application.action_client.wait_for_result(self.application.preempt_timeout)
            self.application.action_client = None

        # kill launch file
        self.application.launcher.shutdown()
        self.application.launcher = None
        self.application.state = 'Stopped'
        rospy.loginfo('Stopped application %s'%self.application.name)
        self.application.lock.release() # this lock got acquired in other thread


class ApplicationQueueItem():
    def __init__(self, app, action, wait):
        self.app = app
        self.action = action
        self.wait = wait
        self.result = None


class ApplicationQueue():
    def __init__(self):
        self.items = []
        self.items_lock = threading.Lock()

    def pull_application(self):
        with self.items_lock:
            if len(self.items) == 0:
                return None
            else:
                item = self.items[0]
                del self.items[0]
                return item

    def push_application(self, app, action, wait):
        with self.items_lock:
            self.items.append(ApplicationQueueItem(app, action, wait))
        return True

    def stop_application(self, app, wait):
        return self.push_application(app, 'stop', wait)

    def start_application(self, app, wait):
        return self.push_application(app, 'start', wait)







class ApplicationManager():
    def __init__(self, queue):
        self.lock = threading.RLock()
        self.queue = queue
        self.applications = self.get_available_applications()
        
        # interface of application manager
        self.run_interface = actionlib.ActionServer('application_manager/run_application', RunApplicationAction, self.run_cb, self.cancel_cb)
        self.kill_interface = actionlib.SimpleActionServer('application_manager/kill_application', KillApplicationAction, self.kill_cb)
        self.stop_interface = actionlib.SimpleActionServer('application_manager/stop_application', StopApplicationAction, self.stop_cb)
        self.list_interface = actionlib.SimpleActionServer('application_manager/list_applications', ListApplicationsAction, self.list_cb)


    def get_available_applications(self):
        with self.lock:
            # create applications
            path = roslib.packages.get_pkg_dir('application_manager')
            apps_yaml = yaml.load(open(path+'/applications.yaml'))
            available_applications = {}
            for app in apps_yaml:
                application = Application()
                if not 'app' in app:
                    rospy.logerr('Invalid entry in applications.yaml file: %s'%str(app))
                elif application.configure(app['app']):
                    available_applications[application.name] = application
                    rospy.loginfo("Available application: %s"%application.name)                    

            # create depends-on list
            for name, app in available_applications.iteritems():
                for d in app.depends:
                    available_applications[d].depends_on.append(name)
                    
            # create conflicts list
            all_resources = {}
            for name, app in available_applications.iteritems():
                all_resources[name] = []
                self.update_resources(name, all_resources[name], available_applications)
            for name, app in available_applications.iteritems():
                for r in all_resources[name]:
                    for name2 in available_applications:
                        if r in all_resources[name2] and not name2 in app.conflicts:
                            if name != name2 and not name2 in app.depends and not name2 in app.depends_on:
                                app.conflicts.append(name2)

            return available_applications


    def update_resources(self, app, resource_list, applications):
        for r in applications[app].resources:
            if not r in resource_list:
                resource_list.append(r)
        for d in applications[app].depends:
            self.update_resources(d, resource_list, applications)



    def start_application(self, application_name, wait=False):
        print 'STARTING %s'%application_name
        # check if application exists
        if not application_name in self.applications:
            rospy.logerr('Cannot start application %s, no application exists with that name'%application_name)
            return False
        app = self.applications[application_name]

        # stop apps that conflict with this app
        for a in app.conflicts:
            if self.applications[a].state != 'Stopped':
                self.stop_application(self.applications[a].name, wait=True)

        # start apps that this app depends on
        for a in app.depends:
            if self.applications[a].state != 'Running':
                self.start_application(self.applications[a].name, wait=True)

        # start application
        self.queue.start_application(app, wait)
        return True


    def stop_application(self, application_name, wait=False):
        # check if application exists
        if not application_name in self.applications:
            rospy.logerr('Cannot stop application %s, no application exists with that name'%application_name)
            return False

        # stop apps that depend on this app
        app = self.applications[application_name]
        for a in app.depends_on:
            if self.applications[a].state != 'Stopped':
                self.stop_application(self.applications[a].name, wait=True)

        # stop application
        self.queue.stop_application(app, wait)
        return True


    def run_cb(self, goal):
        with self.lock:
            application_name = goal.get_goal().application_name
            if self.start_application(application_name):
                self.applications[application_name].client_goals.append(goal)
                goal.set_accepted()
                rospy.loginfo('Client connected to application %s'%application_name)
            else:
                goal.set_rejected()
            

    def cancel_cb(self, goal):
        with self.lock:
            # find matching application
            application = None
            for name, app in self.applications.iteritems():
                if goal in app.client_goals:
                    application = app
            if not application:
                rospy.loginfo('Canceling goal that does not match up with an application any more')
                return
            
            # remove goal from client goal list 
            application.client_goals.remove(goal)
            goal.set_canceled()

            # stop app if needed
            if application.daemonize or len(application.client_goals) > 0:
                rospy.loginfo('Disconnected client from application %s, but keep applcation running'%application.name)
            else:
                self.stop_application(application.name)

                
    def stop_cb(self, goal):
        with self.lock:
            if self.stop_application(goal.application_name):
                self.stop_interface.set_succeeded(KillApplicationResult())
            else:
                self.stop_interface.set_aborted(KillApplicationResult())


    def kill_cb(self, goal):
        with self.lock:
            if self.stop_application(goal.application_name):
                self.kill_interface.set_succeeded(KillApplicationResult())
            else:
                self.kill_interface.set_aborted(KillApplicationResult())


    def list_cb(self, goal):
        with self.lock:
            res = ListApplicationsResult()
            for name, app in self.applications.iteritems():
                res.application_name.append(name)
                res.application_status.append(app.state)
            self.list_interface.set_succeeded(res)


        


def main():
    rospy.init_node('application_manager_node')
    queue = ApplicationQueue()
    ApplicationManager(queue)
    rospy.loginfo("Application manager running")

    while not rospy.is_shutdown(): 
       rospy.sleep(0.1)
       item = queue.pull_application()
       if item:
           if item.action == 'start':
               item.result = item.app.start(item.wait)
           if item.action == 'stop':
               item.result = item.app.stop(item.wait)


if __name__ == '__main__':
    main()