executive.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



import roslib; roslib.load_manifest('continuous_ops_executive')
import rospy
import actionlib
import smach
import smach_ros
from pr2_msgs.msg import PowerState
from std_msgs.msg import String, Float64, UInt8
from actionlib_msgs.msg import GoalStatus
from pr2_plugs_msgs.msg import RechargeAction, RechargeGoal, RechargeCommand
from continuous_ops_msgs.msg import TaskAction, TaskGoal, ChargeStatus
from continuous_ops_msgs.srv import *
from random import choice
from threading import Thread

# Define Battery Monitor state
class HighBatteryMonitor(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['stuffed', 'not_plugged_in', 'preempted'])
        self._charge_data = ChargeStatus()
        self.sub = rospy.Subscriber('charge_status', ChargeStatus, self.charge_cb)
        self.srv = rospy.ServiceProxy('task_manager/get_scheduled_tasks', GetTaskList)

    def execute(self, userdata):
        rospy.loginfo('Monitoring battery level and task schedule')
        while not rospy.is_shutdown():
            # we're still using battery power
            if self._charge_data.charge_status == ChargeStatus.UNPLUGGED:
                rospy.logerr('Robot is not plugged in correctly!')
                return 'not_plugged_in'

            # low battery level
            task_list = self.srv().tasks
            if (self._charge_data.battery_status == ChargeStatus.STUFFED) and len(task_list) > 0:
                return 'stuffed'
            # preemption
            if self.preempt_requested():
                self.service_preempt()
                return 'preempted'
            rospy.sleep(0.1)

    def charge_cb(self, msg):
        self._charge_data = msg

# Montior state for low battery and task schedule
class LowBatteryMonitor(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['bored', 'hungry','preempted'])
        self._charge_data = ChargeStatus()
        self.sub = rospy.Subscriber('charge_status', ChargeStatus, self.charge_cb)
        self.srv = rospy.ServiceProxy('task_manager/get_scheduled_tasks', GetTaskList)

    def execute(self, userdata):
        rospy.loginfo('Monitoring battery level and task schedule')
        while not rospy.is_shutdown():
            # low battery level
            if self._charge_data.battery_status == ChargeStatus.HUNGRY:
                rospy.loginfo("hungry")
                return 'hungry'
            # no tasks and medium battery level
            resp = self.srv()
            if (not self._charge_data.battery_status == ChargeStatus.HUNGRY) and (self._charge_data.charge_status == ChargeStatus.UNPLUGGED) and len(resp.tasks) == 0:
                rospy.loginfo("Continuous ops bored")
                return 'bored'
            # preemption
            if self.preempt_requested():
                self.service_preempt()
                rospy.loginfo("Continuous ops was preempted")
                return 'preempted'
            rospy.sleep(0.1)

    def charge_cb(self, msg):
        self._charge_data = msg



# Launch a task
class TaskLaunchState(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['succeeded', 'aborted', 'preempted'],
                             output_keys=['task'])
        self.list_srv    = rospy.ServiceProxy('task_manager/get_scheduled_tasks', GetTaskList)
        self.start_srv   = rospy.ServiceProxy('task_manager/start_task', ChangeTask)


    def execute(self, userdata):
        # service the first available task
        task_list = self.list_srv().tasks
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'

        if len(task_list) == 0:
            rospy.logerr("No scheduled tasks to run")
            rospy.sleep(5)
            return 'aborted'

        task_list = self.start_srv(ChangeTaskRequest(task_list[0].run_id)).tasks
        if len(task_list) == 0:
            rospy.logerr("No scheduled tasks to run")
            rospy.sleep(5)
            return 'aborted'

        if task_list[0].state == 'Running':
            userdata.task = task_list[0]
            return 'succeeded'
        return 'aborted'



# Kill a task
class TaskKillState(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['succeeded', 'preempted'],
                             input_keys=['task'])
        self.stop_srv = rospy.ServiceProxy('task_manager/stop_task', ChangeTask)
        self.remove_srv = rospy.ServiceProxy('task_manager/remove_task', ChangeTask)

    def execute(self, userdata):
        self.stop_srv(ChangeTaskRequest(userdata.task.run_id))
        
        if not self.preempt_requested():
            self.remove_srv(ChangeTaskRequest(userdata.task.run_id))
            return 'succeeded'
        else:
            self.service_preempt()
            return 'preempted'





# Run the task action
class TaskRunState(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['succeeded', 'aborted', 'preempted'],
                             input_keys=['task'],
                             output_keys=['task'])
        self.connect_timeout = rospy.Duration(30)
        self.preempt_timeout = rospy.Duration(30)        
        self.succeeded = []
        self.aborted = []
        self.preempted = []
        self.srv = rospy.Service('continuous_ops_executive/cancel_running_task', ChangeTask, self.cancel_srv)

    def execute(self, userdata):
        self.cancel_run_id = ''
        print "service should be up"

        # wait for action client to connect with server
        rospy.loginfo('Connecting to action server %s'%userdata.task.action_ns)
        self.ac = actionlib.SimpleActionClient(userdata.task.action_ns, TaskAction)
        start_time = rospy.Time.now()
        while not rospy.is_shutdown() and not self.ac.wait_for_server(rospy.Duration(0.2)):
            if self.cancel_run_id == userdata.task.run_id:
                self.cancel_run_id = ''
                self.aborted.append(userdata.task.name)
                rospy.logerr('Canceling task before it is connected to action server')
                return 'aborted'
            if self.preempt_requested():
                self.preempted.append(userdata.task.name)
                return 'preempted'
            if rospy.Time.now() > start_time + self.connect_timeout:
                self.aborted.append(userdata.task.name)
                rospy.logerr('Timed out waiting for action server of task')
                return 'aborted'

        # run the task
        rospy.loginfo('Sending goal to action server %s'%userdata.task.action_ns)
        self.preempting = False
        self.ac.send_goal(TaskGoal())
        start_time = rospy.Time.now()
        while not rospy.is_shutdown():
            # reasons to preempt
            if self.cancel_run_id == userdata.task.run_id:
                self.preempt_task('Current task canceled from external source')
            if self.preempt_requested():
                self.preempt_task('Current task being preempted by state machine')
            if userdata.task.timeout != rospy.Duration(0) and rospy.Time.now() > start_time + userdata.task.timeout:
                self.preempt_task('Current task exceeded its allocated time slot')

            # reasons to abort
            if self.preempting and rospy.Time.now() > self.preempting_start + self.preempt_timeout:
                rospy.logerr('Task did not preempt within timeout. Aborting')
                return 'aborted'
    
            # Check actual state 
            state = self.ac.get_state()
            if state == GoalStatus.SUCCEEDED:
                self.succeeded.append(userdata.task.name)
                rospy.loginfo('Task succeeded')
                return 'succeeded'
            elif state == GoalStatus.PREEMPTED:
                self.preempted.append(userdata.task.name)
                rospy.logwarn('Task preempted')
                return 'preempted'
            elif state in [GoalStatus.RECALLED,
                           GoalStatus.REJECTED, 
                           GoalStatus.ABORTED, 
                           GoalStatus.LOST]:
                self.aborted.append(userdata.task.name)
                rospy.logerr('Task aborted')
                return 'aborted'
            rospy.sleep(0.2)

    def preempt_task(self, msg):
        if not self.preempting:
            self.ac.cancel_all_goals()
            self.preempting = True
            self.preempting_start = rospy.Time.now()
            rospy.logwarn('%s'%msg)


    def cancel_srv(self, req):
        self.cancel_run_id = req.run_id
        return ChangeTaskResponse()




class TimeLogger(Thread):
    def __init__(self):
        Thread.__init__(self)
        self.start_time = rospy.Time.now()

    def run(self):
        print 'Running'
        pub = rospy.Publisher('continuous_ops_duration', Float64)
        while not rospy.is_shutdown():
            rospy.sleep(2.0)
            pub.publish((rospy.Time.now() - self.start_time).to_sec())


def main():
    rospy.init_node('continuous_ops_executive')

    time_logger = TimeLogger()
    time_logger.start()
    
    # wait for task manager
    rospy.loginfo('Detecting task manager...')
    rospy.wait_for_service('task_manager/get_scheduled_tasks')
    rospy.wait_for_service('task_manager/start_task')
    rospy.wait_for_service('task_manager/stop_task')
    rospy.wait_for_service('task_manager/remove_task')
    rospy.loginfo('Connected to task manager')

    # Create the top level SMACH state machine
    sm_top = smach.StateMachine(outcomes=['aborted', 'preempted'])
    with sm_top:

        # -----------------
        # Task with monitor
        # -----------------
        def child_term_cb(outcome_map):
            # if any of the states exit for another reason, preempt the other state
            rospy.loginfo('One concurrent state finished, preempting other state')
            return True
        def out_cb(outcome_map):
            if outcome_map['LOW_BATTERY_MONITOR'] == 'hungry':
                return 'hungry'
            if outcome_map['LOW_BATTERY_MONITOR'] == 'bored':
                return 'bored'
            return 'continue'
        sm_task_monitor = smach.Concurrence(outcomes=['continue', 'hungry', 'bored'],
                                            default_outcome='continue',
                                            child_termination_cb = child_term_cb,
                                            outcome_cb = out_cb)
        with sm_task_monitor:
            sm_task = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
            with sm_task:
                # Launch a task
                smach.StateMachine.add('LAUNCH_TASK', TaskLaunchState(),
                                       transitions={'succeeded':'RUN_TASK',
                                                    'aborted':'LAUNCH_TASK',
                                                    'preempted':'preempted'})
                # Run a task
                smach.StateMachine.add('RUN_TASK', TaskRunState(),
                                       transitions={'succeeded':'KILL_TASK',
                                                    'aborted':'KILL_TASK',
                                                    'preempted':'KILL_TASK'})
                # Kill a task
                smach.StateMachine.add('KILL_TASK', TaskKillState(),
                                       transitions={'succeeded':'LAUNCH_TASK',
                                                    'preempted':'preempted'})

            # Tasks to execute
            smach.Concurrence.add('TASK', sm_task)

            # Low battery monitor
            smach.Concurrence.add('LOW_BATTERY_MONITOR', LowBatteryMonitor())

        # Add to top level sm
        smach.StateMachine.add('TASK_WITH_MONITOR', sm_task_monitor,
                               transitions={'continue':'TASK_WITH_MONITOR',
                                            'bored':'RECHARGE',
                                            'hungry':'RECHARGE'})



        # --------
        # Recharge
        # --------
        sm_recharge = smach.StateMachine(outcomes=['aborted', 'preempted', 'stuffed'])
        
        with sm_recharge:
            # choose random recharging location
            def location_cb(userdata, goal):
                goal.command.plug_id = choice(['wim',  'james'])
                rospy.loginfo("Recharging at location %s"%goal.command.plug_id)
                return goal
            plugin = RechargeGoal(RechargeCommand(RechargeCommand.PLUG_IN, ''))
            smach.StateMachine.add('PLUG_IN', smach_ros.SimpleActionState('recharge',
                                                                          RechargeAction,
                                                                          goal=plugin,
                                                                          goal_cb=location_cb),
                                   transitions={'aborted':'PLUG_IN',
                                                'succeeded':'HIGH_BATTERY_MONITOR',
                                                'preempted':'preempted'})

            # wait until battery is charged
            smach.StateMachine.add('HIGH_BATTERY_MONITOR', HighBatteryMonitor(),
                                   transitions={'preempted':'preempted',
                                                'stuffed':'UNPLUG',
                                                'not_plugged_in':'UNPLUG_RECOVER'})

            # unplug
            unplug = RechargeGoal(RechargeCommand(RechargeCommand.UNPLUG, ''))
            smach.StateMachine.add('UNPLUG', smach_ros.SimpleActionState('recharge',
                                                                         RechargeAction,
                                                                         goal=unplug),
                                   transitions={'succeeded':'stuffed',
                                                'aborted':'UNPLUG',
                                                'preempted':'preempted'})

            # unplug
            unplug = RechargeGoal(RechargeCommand(RechargeCommand.UNPLUG, ''))
            smach.StateMachine.add('UNPLUG_RECOVER', smach_ros.SimpleActionState('recharge',
                                                                                 RechargeAction,
                                                                                 goal=unplug),
                                   transitions={'succeeded':'PLUG_IN',
                                                'aborted':'UNPLUG_RECOVER',
                                                'preempted':'preempted'})

        smach.StateMachine.add('RECHARGE', sm_recharge,
                               transitions={'aborted':'aborted',
                                            'stuffed':'TASK_WITH_MONITOR'})

                                   
    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('continuous_ops_executive_server', sm_top, '/CONTINUOUS_OPERATION_EXECUTIVE')
    sis.start()

    # Execute SMACH plan
    rospy.sleep(4)
    outcome = sm_top.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()


if __name__ == '__main__':
    main()