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