task_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('continuous_ops_task_manager')
import rospy
import os
import threading
import yaml
import copy
import roslauncher
from continuous_ops_msgs.srv import *
from continuous_ops_msgs.msg import *


def tasks_to_msg(tasks, ids=None):
    task_msgs = []
    if ids:
        loop = ids
    else:
        loop = tasks.keys()
    for t in loop:
        task_msgs.append(tasks[t].to_msg())
    return task_msgs



class OneTask():
    def __init__(self, pkg, file, conf):
        path = roslib.packages.get_pkg_dir(pkg)
        self.name = conf['name']
        self.description = conf['description']
        self.task_id = ('_'.join([pkg, file])).replace('.','_')
        self.run_id = ''
        self.launch_file = os.path.join(path, conf['launch_file'])
        self.image_file = os.path.join(path, conf['image_file'])
        self.state = ''
        self.action_ns = conf['action_ns']
        self.timeout = rospy.Duration(conf['timeout'])

    def to_msg(self):
        msg = Task()
        msg.name = self.name
        msg.description = self.description
        msg.task_id = self.task_id
        msg.run_id = self.run_id
        msg.launch_file = self.launch_file
        msg.image_file = self.image_file
        msg.state = self.state
        msg.action_ns = self.action_ns
        msg.timeout = self.timeout
        return msg




class TaskManager():
    def __init__(self):
        self.lock = threading.RLock()

        # Get list of available tasks
        self.pub_available = rospy.Publisher('available_tasks', Tasks, latch=True)
        self.pub_available_tasks()

        # List of scheduled tasks, store in dictionary with key run_id
        self.start_task_needed = False
        self.schedule_order = []
        self.scheduled_tasks = {}
        self.launch = None
        self.pub_scheduled = rospy.Publisher('scheduled_tasks', Tasks, latch=True)
        self.get_scheduled_tasks()

        # Advertise services
        self.srv_get_available_tasks = rospy.Service('get_available_tasks', GetTaskList, self.get_available_tasks_srv)
        self.srv_get_scheduled_tasks = rospy.Service('get_scheduled_tasks', GetTaskList, self.get_scheduled_tasks_srv)
        self.srv_add_task = rospy.Service('add_task', AddTask, self.add_task_srv)
        self.srv_remove_task = rospy.Service('remove_task', ChangeTask, self.remove_task_srv)
        self.srv_start_task = rospy.Service('start_task', ChangeTask, self.start_task_srv)
        self.srv_stop_task = rospy.Service('stop_task', ChangeTask, self.stop_task_srv)
        self.srv_reorder_tasks = rospy.Service('reorder_tasks', ReorderTaskList, self.reorder_tasks_srv)

    def get_available_tasks(self):
        with self.lock:
            self.available_tasks = {}
            for pkg in roslib.rospack.rospack_depends_on_1('continuous_ops_task_manager'):
                path = roslib.packages.get_pkg_dir(pkg)
                files = os.listdir(path)
                for f in files:
                    if f.partition('.')[2] == 'task':
                        conf = yaml.load(open(os.path.join(path, f)))
                        task = OneTask(pkg, f, conf)
                        self.available_tasks[task.task_id] = task
            return tasks_to_msg(self.available_tasks)

    def pub_available_tasks(self):
        self.pub_available.publish(self.get_available_tasks())

    def get_scheduled_tasks(self):
        with self.lock:
            msg = tasks_to_msg(self.scheduled_tasks, self.schedule_order)
            return msg

    def pub_scheduled_tasks(self):
        self.pub_scheduled.publish(self.get_scheduled_tasks())

    def start_task(self, run_id):
        with self.lock:
            if not run_id in self.scheduled_tasks:
                rospy.logerr('Cannot start task with run_id %s, no task exists with that run_id'%run_id)
                return False
            if self.launch:
                rospy.logerr('Cannot start task with run_id %s, another task is still running'%run_id)
                return False
            if self.schedule_order[0] != run_id:
                rospy.logerr('Cannot start task with run_id %s, this is not the first task in the schedule'%run_id)
                return False
            with open(self.scheduled_tasks[run_id].launch_file) as f:
                rospy.loginfo('Starting task with run_id %s'%run_id)
                self.launch = roslauncher.ScriptRoslaunch(f.read())
                self.launch.start()
                self.scheduled_tasks[run_id].state = 'Running'
                self.pub_scheduled_tasks()
                return True
            return False

    def stop_task(self, run_id):
        with self.lock:
            if not run_id in self.scheduled_tasks:
                rospy.logerr('Cannot stop task with run_id %s, no task exists with that run_id'%run_id)                
                return False
            if not self.scheduled_tasks[run_id].state == 'Running':
                rospy.logerr('Cannot stop task with run_id %s, task is not running'%run_id)                                
                return False
            rospy.loginfo('Stopped task with run_id %s'%run_id)
            self.launch.shutdown()
            self.launch = None
            self.scheduled_tasks[run_id].state = 'Stopped'
            self.pub_scheduled_tasks()
            return True
            
    def add_task(self, task_id):
        with self.lock:
            if not task_id in self.available_tasks:
                rospy.logerr('Cannot add task with task_id %s, because no task exists with that task_id'%task_id)
                return ''
            run_id = ('_'.join([task_id,str(rospy.Time.now())])).replace('.','_')
            rospy.loginfo('Adding new task with run_id %s'%run_id)
            task = copy.deepcopy(self.available_tasks[task_id])
            task.state = 'Queued'
            task.run_id = run_id
            self.scheduled_tasks[task.run_id] = task
            self.schedule_order.append(task.run_id)
            self.pub_scheduled_tasks()
            return task.run_id

    def remove_task(self, run_id):
        with self.lock:
            if not run_id in self.scheduled_tasks:
                rospy.logerr('Cannot remove task with run_id %s, no task exists with that run_id'%run_id)                
                return False
            if self.scheduled_tasks[run_id].state == 'Running':
                rospy.logerr('Cannot remove task with run_id %s, this task is currently running'%run_id)
                return False
            rospy.loginfo('Removed task with run_id %s'%run_id)
            del self.scheduled_tasks[run_id]
            self.schedule_order.remove(run_id)
            self.pub_scheduled_tasks()
            return True

    def reorder_tasks(self, run_id_list):
        if len(run_id_list) != len(self.schedule_order):
            rospy.logerr('Cannot reorder tasks. Desired list has different length than scheduled list.')
            return False
        for t in self.schedule_order:
            if not t in run_id_list:
                rospy.logerr('Cannot reorder tasks. Task %s is not specified in desired list'%t)
                return False
        if len(run_id_list) == 0:
            return True
        if self.scheduled_tasks[self.schedule_order[0]].state == 'Running' and self.schedule_order[0] != run_id_list[0]:
            rospy.logerr('Cannot reorder a running task with run_id %s'%self.schedule_order[0])
            return False
        rospy.loginfo('Reordering scheduled tasks')
        self.schedule_order = run_id_list
        self.pub_scheduled_tasks()
        return True


    # Service callbacks
    def get_available_tasks_srv(self, req):
        msg = self.get_available_tasks()
        self.pub_available_tasks()
        return GetTaskListResponse(msg)

    def get_scheduled_tasks_srv(self, req):
        return GetTaskListResponse(self.get_scheduled_tasks())

    def add_task_srv(self, req):
        return AddTaskResponse(self.add_task(req.task_id), self.get_scheduled_tasks())

    def remove_task_srv(self, req):
        self.remove_task(req.run_id)
        return ChangeTaskResponse(self.get_scheduled_tasks())
        
    def start_task_srv(self, req):
        self.start_task_run_id = req.run_id
        self.start_task_needed = True
        while self.start_task_needed:
            rospy.sleep(0.1)
        return ChangeTaskResponse(self.get_scheduled_tasks())

    def stop_task_srv(self, req):
        self.stop_task(req.run_id)
        return ChangeTaskResponse(self.get_scheduled_tasks())

    def reorder_tasks_srv(self, req):
        self.reorder_tasks(req.run_id_list)
        return ReorderTaskListResponse(self.get_scheduled_tasks())



def main():
    rospy.init_node('task_manager_node')
    tm = TaskManager()

    # do some testing
#     tm.add_task('continuous_ops_test_task_wim_task')
#     tm.add_task('continuous_ops_test_task_wim_task')
#     tm.add_task('continuous_ops_test_task_eitan_task')

    while not rospy.is_shutdown():
        if tm.start_task_needed:
            print 'STARTING THREAD'
            tm.start_task(tm.start_task_run_id)
            tm.start_task_needed = False
        rospy.sleep(0.1)

if __name__ == '__main__':
    main()