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)
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)
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)