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'])
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'
})
