def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failure']) self._bump_monitored_dk_undk = MonitoredDockUndockBehaviour() self._recover_bumper = RecoverBumper() self._nav_resume_monitor = NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_DOCK', self._bump_monitored_dk_undk, transitions={ 'succeeded': 'succeeded', 'failure': 'failure', 'bumper_pressed': 'RECOVER_BUMPER', 'pause_requested': 'NAV_RESUME_MONITOR' }) smach.StateMachine.add('RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_DOCK'}) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={ 'invalid': 'MONITORED_DOCK', 'valid': 'MONITORED_DOCK', 'preempted': 'MONITORED_DOCK' })
class HighLevelMoveBase(smach.StateMachine, Loggable): def __init__(self, going_to_charge): smach.StateMachine.__init__( self, outcomes=['succeeded', 'move_base_failure', 'battery_low'], input_keys=['goal_pose']) self._monitored_recoverable_move_base = MonitoredRecoverableMoveBase( going_to_charge) self._recover_bumper = RecoverBumper() self._recover_carpet = RecoverStuckOnCarpet() self._nav_resume_monitor = NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_MOVE_BASE', self._monitored_recoverable_move_base, transitions={ 'bumper_pressed': 'RECOVER_BUMPER', 'battery_low': 'battery_low', 'succeeded': 'succeeded', 'failure': 'move_base_failure', 'stuck_on_carpet': 'RECOVER_STUCK_ON_CARPET', 'pause_requested': 'NAV_RESUME_MONITOR' }) smach.StateMachine.add( 'RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_MOVE_BASE'}) smach.StateMachine.add('RECOVER_STUCK_ON_CARPET', self._recover_carpet, transitions={ 'succeeded': 'MONITORED_MOVE_BASE', 'failure': 'RECOVER_STUCK_ON_CARPET' }) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={ 'invalid': 'MONITORED_MOVE_BASE', 'valid': 'MONITORED_MOVE_BASE', 'preempted': 'MONITORED_MOVE_BASE' }) """ Set the battery level thresholds. """ def set_patroller_thresholds(self, very_low_battery, low_battery, charged_battery, max_bumper_recovery_attempts, max_move_base_recovery_attempts): self._monitored_recoverable_move_base.set_patroller_thresholds( very_low_battery, low_battery, charged_battery, max_bumper_recovery_attempts, max_move_base_recovery_attempts) self._recover_bumper.set_patroller_thresholds( max_bumper_recovery_attempts)
def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failure']) self._bump_monitored_dk_undk = MonitoredDockUndockBehaviour() self._recover_bumper = RecoverBumper() self._nav_resume_monitor=NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_DOCK', self._bump_monitored_dk_undk, transitions={'succeeded': 'succeeded', 'failure': 'failure', 'bumper_pressed': 'RECOVER_BUMPER', 'pause_requested':'NAV_RESUME_MONITOR' } ) smach.StateMachine.add('RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_DOCK' }) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={'invalid': 'MONITORED_DOCK', 'valid': 'MONITORED_DOCK', 'preempted':'MONITORED_DOCK'})
def __init__(self,going_to_charge): smach.StateMachine.__init__(self, outcomes=['succeeded', 'move_base_failure', 'battery_low'], input_keys=['goal_pose', 'goal_pan_tilt'], output_keys=['goal_pan_tilt']) self._monitored_recoverable_move_base = MonitoredRecoverableMoveBase(going_to_charge) self._recover_bumper = RecoverBumper() self._recover_carpet = RecoverStuckOnCarpet() self._nav_resume_monitor=NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_MOVE_BASE', self._monitored_recoverable_move_base, transitions={'bumper_pressed': 'RECOVER_BUMPER', 'battery_low': 'battery_low', 'succeeded': 'succeeded', 'failure': 'move_base_failure', 'stuck_on_carpet':'RECOVER_STUCK_ON_CARPET', 'pause_requested':'NAV_RESUME_MONITOR'}) smach.StateMachine.add('RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_MOVE_BASE'}) smach.StateMachine.add('RECOVER_STUCK_ON_CARPET', self._recover_carpet, transitions={'succeeded': 'MONITORED_MOVE_BASE', 'failure': 'RECOVER_STUCK_ON_CARPET'}) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={'invalid': 'MONITORED_MOVE_BASE', 'valid': 'MONITORED_MOVE_BASE', 'preempted':'MONITORED_MOVE_BASE'})
class HighLevelMoveBase(smach.StateMachine, Loggable): def __init__(self,going_to_charge): smach.StateMachine.__init__(self, outcomes=['succeeded', 'move_base_failure', 'battery_low'], input_keys=['goal_pose', 'goal_pan_tilt'], output_keys=['goal_pan_tilt']) self._monitored_recoverable_move_base = MonitoredRecoverableMoveBase(going_to_charge) self._recover_bumper = RecoverBumper() self._recover_carpet = RecoverStuckOnCarpet() self._nav_resume_monitor=NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_MOVE_BASE', self._monitored_recoverable_move_base, transitions={'bumper_pressed': 'RECOVER_BUMPER', 'battery_low': 'battery_low', 'succeeded': 'succeeded', 'failure': 'move_base_failure', 'stuck_on_carpet':'RECOVER_STUCK_ON_CARPET', 'pause_requested':'NAV_RESUME_MONITOR'}) smach.StateMachine.add('RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_MOVE_BASE'}) smach.StateMachine.add('RECOVER_STUCK_ON_CARPET', self._recover_carpet, transitions={'succeeded': 'MONITORED_MOVE_BASE', 'failure': 'RECOVER_STUCK_ON_CARPET'}) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={'invalid': 'MONITORED_MOVE_BASE', 'valid': 'MONITORED_MOVE_BASE', 'preempted':'MONITORED_MOVE_BASE'}) """ Set the battery level thresholds. """ def set_patroller_thresholds(self, very_low_battery, low_battery, charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): self._monitored_recoverable_move_base.set_patroller_thresholds(very_low_battery, low_battery, charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts) self._recover_bumper.set_patroller_thresholds(max_bumper_recovery_attempts)
def __init__(self, going_to_charge): smach.StateMachine.__init__( self, outcomes=['succeeded', 'move_base_failure', 'battery_low'], input_keys=['goal_pose']) self._monitored_recoverable_move_base = MonitoredRecoverableMoveBase( going_to_charge) self._recover_bumper = RecoverBumper() self._recover_carpet = RecoverStuckOnCarpet() self._nav_resume_monitor = NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_MOVE_BASE', self._monitored_recoverable_move_base, transitions={ 'bumper_pressed': 'RECOVER_BUMPER', 'battery_low': 'battery_low', 'succeeded': 'succeeded', 'failure': 'move_base_failure', 'stuck_on_carpet': 'RECOVER_STUCK_ON_CARPET', 'pause_requested': 'NAV_RESUME_MONITOR' }) smach.StateMachine.add( 'RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_MOVE_BASE'}) smach.StateMachine.add('RECOVER_STUCK_ON_CARPET', self._recover_carpet, transitions={ 'succeeded': 'MONITORED_MOVE_BASE', 'failure': 'RECOVER_STUCK_ON_CARPET' }) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={ 'invalid': 'MONITORED_MOVE_BASE', 'valid': 'MONITORED_MOVE_BASE', 'preempted': 'MONITORED_MOVE_BASE' })
class HighLevelDockUndockBehaviour(smach.StateMachine, Loggable): def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failure']) self._bump_monitored_dk_undk = MonitoredDockUndockBehaviour() self._recover_bumper = RecoverBumper() self._nav_resume_monitor = NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_DOCK', self._bump_monitored_dk_undk, transitions={ 'succeeded': 'succeeded', 'failure': 'failure', 'bumper_pressed': 'RECOVER_BUMPER', 'pause_requested': 'NAV_RESUME_MONITOR' }) smach.StateMachine.add('RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_DOCK'}) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={ 'invalid': 'MONITORED_DOCK', 'valid': 'MONITORED_DOCK', 'preempted': 'MONITORED_DOCK' }) """ Set the patrolelr level thresholds. """ def set_patroller_thresholds(self, very_low_battery, low_battery, charged_battery, max_bumper_recovery_attempts, max_move_base_recovery_attempts): self._bump_monitored_dk_undk.set_patroller_thresholds( very_low_battery, low_battery, charged_battery) self._recover_bumper.set_patroller_thresholds( max_bumper_recovery_attempts)
class HighLevelDockUndockBehaviour(smach.StateMachine, Loggable): def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failure']) self._bump_monitored_dk_undk = MonitoredDockUndockBehaviour() self._recover_bumper = RecoverBumper() self._nav_resume_monitor=NavPauseMonitor(True) with self: smach.StateMachine.add('MONITORED_DOCK', self._bump_monitored_dk_undk, transitions={'succeeded': 'succeeded', 'failure': 'failure', 'bumper_pressed': 'RECOVER_BUMPER', 'pause_requested':'NAV_RESUME_MONITOR' } ) smach.StateMachine.add('RECOVER_BUMPER', self._recover_bumper, transitions={'succeeded': 'MONITORED_DOCK' }) smach.StateMachine.add('NAV_RESUME_MONITOR', self._nav_resume_monitor, transitions={'invalid': 'MONITORED_DOCK', 'valid': 'MONITORED_DOCK', 'preempted':'MONITORED_DOCK'}) """ Set the patrolelr level thresholds. """ def set_patroller_thresholds(self, very_low_battery, low_battery, charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): self._bump_monitored_dk_undk.set_patroller_thresholds(very_low_battery, low_battery, charged_battery) self._recover_bumper.set_patroller_thresholds(max_bumper_recovery_attempts)