def __init__(self):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'failure', 'preempted'])
self._battery_monitor = BatteryMonitor(True)
self._dock_to_charge = DockToChargingStation()
self._undock_from_charge = UndockFromChargingStation()
with self:
smach.StateMachine.add('DOCK_TO_CHARGING_STATION',
self._dock_to_charge,
transitions={
'succeeded': 'CHARGING',
'aborted': 'DOCK_TO_CHARGING_STATION',
'preempted': 'preempted'
})
smach.StateMachine.add('CHARGING',
self._battery_monitor,
transitions={
'invalid':
'UNDOCK_FROM_CHARGING_STATION',
'valid': 'UNDOCK_FROM_CHARGING_STATION'
})
smach.StateMachine.add('UNDOCK_FROM_CHARGING_STATION',
self._undock_from_charge,
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded',
'failure',
'preempted'])
self._battery_monitor = BatteryMonitor(True)
self._dock_to_charge = DockToChargingStation()
self._undock_from_charge = UndockFromChargingStation()
with self:
smach.StateMachine.add('DOCK_TO_CHARGING_STATION',
self._dock_to_charge,
transitions={'succeeded': 'CHARGING',
'aborted': 'DOCK_TO_CHARGING_STATION',
'preempted':'preempted'})
smach.StateMachine.add('CHARGING',
self._battery_monitor,
transitions={
'invalid': 'UNDOCK_FROM_CHARGING_STATION',
'valid': 'UNDOCK_FROM_CHARGING_STATION'})
smach.StateMachine.add('UNDOCK_FROM_CHARGING_STATION',
self._undock_from_charge,
transitions={'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'preempted'})
class DockUndockBehaviour(smach.StateMachine, Loggable):
def __init__(self):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'failure', 'preempted'])
self._battery_monitor = BatteryMonitor(True)
self._dock_to_charge = DockToChargingStation()
self._undock_from_charge = UndockFromChargingStation()
with self:
smach.StateMachine.add('DOCK_TO_CHARGING_STATION',
self._dock_to_charge,
transitions={
'succeeded': 'CHARGING',
'aborted': 'DOCK_TO_CHARGING_STATION',
'preempted': 'preempted'
})
smach.StateMachine.add('CHARGING',
self._battery_monitor,
transitions={
'invalid':
'UNDOCK_FROM_CHARGING_STATION',
'valid': 'UNDOCK_FROM_CHARGING_STATION'
})
smach.StateMachine.add('UNDOCK_FROM_CHARGING_STATION',
self._undock_from_charge,
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'preempted'
})
"""
Set the battery level thresholds.
"""
def set_patroller_thresholds(self, very_low_battery, low_battery,
charged_battery):
self._battery_monitor.set_patroller_thresholds(very_low_battery,
low_battery,
charged_battery)
def __init__(self, going_to_charge):
smach.Concurrence.__init__(self,
outcomes=[
'bumper_pressed', 'battery_low',
'succeeded', 'failure',
'stuck_on_carpet', 'pause_requested'
],
default_outcome='failure',
child_termination_cb=self.child_term_cb,
outcome_cb=self.out_cb,
input_keys=['goal_pose'])
self._battery_monitor = BatteryMonitor(going_to_charge)
self._bumper_monitor = BumperMonitor()
self._recoverable_move_base = RecoverableMoveBase()
self._carpet_monitor = StuckOnCarpetMonitor()
self._nav_pause_monitor = NavPauseMonitor(False)
with self:
smach.Concurrence.add('BATTERY_MONITOR', self._battery_monitor)
smach.Concurrence.add('BUMPER_MONITOR', self._bumper_monitor)
smach.Concurrence.add('STUCK_ON_CARPET_MONITOR',
self._carpet_monitor)
smach.Concurrence.add('NAV_PAUSE_MONITOR', self._nav_pause_monitor)
smach.Concurrence.add('MOVE_BASE_SM', self._recoverable_move_base)
class DockUndockBehaviour(smach.StateMachine, Loggable):
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded',
'failure',
'preempted'])
self._battery_monitor = BatteryMonitor(True)
self._dock_to_charge = DockToChargingStation()
self._undock_from_charge = UndockFromChargingStation()
with self:
smach.StateMachine.add('DOCK_TO_CHARGING_STATION',
self._dock_to_charge,
transitions={'succeeded': 'CHARGING',
'aborted': 'DOCK_TO_CHARGING_STATION',
'preempted':'preempted'})
smach.StateMachine.add('CHARGING',
self._battery_monitor,
transitions={
'invalid': 'UNDOCK_FROM_CHARGING_STATION',
'valid': 'UNDOCK_FROM_CHARGING_STATION'})
smach.StateMachine.add('UNDOCK_FROM_CHARGING_STATION',
self._undock_from_charge,
transitions={'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'preempted'})
"""
Set the battery level thresholds.
"""
def set_patroller_thresholds(self, very_low_battery, low_battery,
charged_battery):
self._battery_monitor.set_patroller_thresholds(very_low_battery,
low_battery,
charged_battery)
def __init__(self,going_to_charge):
smach.Concurrence.__init__(self,
outcomes=['bumper_pressed',
'battery_low',
'succeeded',
'failure',
'stuck_on_carpet',
'pause_requested'],
default_outcome='failure',
child_termination_cb=self.child_term_cb,
outcome_cb=self.out_cb,
input_keys=['goal_pose']
)
self._battery_monitor = BatteryMonitor(going_to_charge)
self._bumper_monitor = BumperMonitor()
self._recoverable_move_base = RecoverableMoveBase()
self._carpet_monitor = StuckOnCarpetMonitor()
self._nav_pause_monitor=NavPauseMonitor(False)
with self:
smach.Concurrence.add('BATTERY_MONITOR', self._battery_monitor)
smach.Concurrence.add('BUMPER_MONITOR', self._bumper_monitor)
smach.Concurrence.add('STUCK_ON_CARPET_MONITOR', self._carpet_monitor)
smach.Concurrence.add('NAV_PAUSE_MONITOR', self._nav_pause_monitor)
smach.Concurrence.add('MOVE_BASE_SM', self._recoverable_move_base)
class MonitoredRecoverableMoveBase(smach.Concurrence, Loggable):
def __init__(self,going_to_charge):
smach.Concurrence.__init__(self,
outcomes=['bumper_pressed',
'battery_low',
'succeeded',
'failure',
'stuck_on_carpet',
'pause_requested'],
default_outcome='failure',
child_termination_cb=self.child_term_cb,
outcome_cb=self.out_cb,
input_keys=['goal_pose']
)
self._battery_monitor = BatteryMonitor(going_to_charge)
self._bumper_monitor = BumperMonitor()
self._recoverable_move_base = RecoverableMoveBase()
self._carpet_monitor = StuckOnCarpetMonitor()
self._nav_pause_monitor=NavPauseMonitor(False)
with self:
smach.Concurrence.add('BATTERY_MONITOR', self._battery_monitor)
smach.Concurrence.add('BUMPER_MONITOR', self._bumper_monitor)
smach.Concurrence.add('STUCK_ON_CARPET_MONITOR', self._carpet_monitor)
smach.Concurrence.add('NAV_PAUSE_MONITOR', self._nav_pause_monitor)
smach.Concurrence.add('MOVE_BASE_SM', self._recoverable_move_base)
def child_term_cb(self, outcome_map):
# decide if this state is done when one or more concurrent inner states
# stop
if ( outcome_map['BUMPER_MONITOR'] == 'invalid' or
outcome_map["BATTERY_MONITOR"] == "invalid" or
outcome_map["STUCK_ON_CARPET_MONITOR"] == "invalid" or
outcome_map["NAV_PAUSE_MONITOR"] == "invalid" or
outcome_map["MOVE_BASE_SM"] == "succeeded" or
outcome_map['MOVE_BASE_SM'] == "failure" ):
return True
return False
def out_cb(self, outcome_map):
# determine what the outcome of this machine is
# rospy.sleep(0.1) without this sleep, sometimes the concurrence container
# terminates before all its children terminate, and an error is printed.
# However, that does not affect the evolution, and I think that with the
# sleep sometimes the container blocks and never terminates
if outcome_map['BUMPER_MONITOR'] == 'invalid':
return 'bumper_pressed'
if outcome_map["BATTERY_MONITOR"] == "invalid":
return "battery_low"
if outcome_map["STUCK_ON_CARPET_MONITOR"] == "invalid":
return "stuck_on_carpet"
if outcome_map["NAV_PAUSE_MONITOR"] == "invalid":
return "pause_requested"
if outcome_map["MOVE_BASE_SM"] == "succeeded":
return "succeeded"
if outcome_map["MOVE_BASE_SM"] == "failure":
return "failure"
"""
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._battery_monitor.set_patroller_thresholds(very_low_battery,
low_battery,
charged_battery)
self._recoverable_move_base.set_patroller_thresholds(max_move_base_recovery_attempts)
class MonitoredRecoverableMoveBase(smach.Concurrence, Loggable):
def __init__(self, going_to_charge):
smach.Concurrence.__init__(self,
outcomes=[
'bumper_pressed', 'battery_low',
'succeeded', 'failure',
'stuck_on_carpet', 'pause_requested'
],
default_outcome='failure',
child_termination_cb=self.child_term_cb,
outcome_cb=self.out_cb,
input_keys=['goal_pose'])
self._battery_monitor = BatteryMonitor(going_to_charge)
self._bumper_monitor = BumperMonitor()
self._recoverable_move_base = RecoverableMoveBase()
self._carpet_monitor = StuckOnCarpetMonitor()
self._nav_pause_monitor = NavPauseMonitor(False)
with self:
smach.Concurrence.add('BATTERY_MONITOR', self._battery_monitor)
smach.Concurrence.add('BUMPER_MONITOR', self._bumper_monitor)
smach.Concurrence.add('STUCK_ON_CARPET_MONITOR',
self._carpet_monitor)
smach.Concurrence.add('NAV_PAUSE_MONITOR', self._nav_pause_monitor)
smach.Concurrence.add('MOVE_BASE_SM', self._recoverable_move_base)
def child_term_cb(self, outcome_map):
# decide if this state is done when one or more concurrent inner states
# stop
if (outcome_map['BUMPER_MONITOR'] == 'invalid'
or outcome_map["BATTERY_MONITOR"] == "invalid"
or outcome_map["STUCK_ON_CARPET_MONITOR"] == "invalid"
or outcome_map["NAV_PAUSE_MONITOR"] == "invalid"
or outcome_map["MOVE_BASE_SM"] == "succeeded"
or outcome_map['MOVE_BASE_SM'] == "failure"):
return True
return False
def out_cb(self, outcome_map):
# determine what the outcome of this machine is
# rospy.sleep(0.1) without this sleep, sometimes the concurrence container
# terminates before all its children terminate, and an error is printed.
# However, that does not affect the evolution, and I think that with the
# sleep sometimes the container blocks and never terminates
if outcome_map['BUMPER_MONITOR'] == 'invalid':
return 'bumper_pressed'
if outcome_map["BATTERY_MONITOR"] == "invalid":
return "battery_low"
if outcome_map["STUCK_ON_CARPET_MONITOR"] == "invalid":
return "stuck_on_carpet"
if outcome_map["NAV_PAUSE_MONITOR"] == "invalid":
return "pause_requested"
if outcome_map["MOVE_BASE_SM"] == "succeeded":
return "succeeded"
if outcome_map["MOVE_BASE_SM"] == "failure":
return "failure"
"""
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._battery_monitor.set_patroller_thresholds(very_low_battery,
low_battery,
charged_battery)
self._recoverable_move_base.set_patroller_thresholds(
max_move_base_recovery_attempts)