class SMACHAI(): def __init__(self): rospy.init_node('smach_home_status', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) ##################################### # JO IS AWAKE ##################################### # State machine for Jo-awake-go-sleep self.sm_jo_awake_sleep = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_awake_sleep: StateMachine.add('LOOK_WAKE', MonitorState("/JO/sleep", Empty, self.empty_cb), transitions={'valid':'GOING_SLEEP', 'preempted':'preempted', 'invalid':'GOING_SLEEP'}) StateMachine.add('GOING_SLEEP', JoGoingSleep(), transitions={'succeeded':'LOOK_IN_BED'}) StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={'valid':'IN_BED', 'preempted':'preempted', 'invalid':'IN_BED'}) StateMachine.add('IN_BED', JoInBed(), transitions={'succeeded':'succeeded'}) # State machine for Jo-awake-bothgo-sleep self.sm_jo_awake_bothsleep = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_awake_bothsleep: StateMachine.add('LOOK_WAKE', MonitorState("/BOTH/sleep", Empty, self.empty_cb), transitions={'valid':'GOING_SLEEP', 'preempted':'preempted', 'invalid':'GOING_SLEEP'}) StateMachine.add('GOING_SLEEP', BothGoingSleep(), transitions={'succeeded':'LOOK_IN_BED'}) StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={'valid':'IN_BED', 'preempted':'preempted', 'invalid':'IN_BED'}) StateMachine.add('IN_BED', BothInBed(), transitions={'succeeded':'succeeded'}) # State machine for Jo-awake-go-out self.sm_jo_awake_out = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_awake_out: StateMachine.add('LOOK_OUT', MonitorState("/JO/go_out", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'}) StateMachine.add('PAUSE', Pause(), transitions={'succeeded':'succeeded'}) # State machine for Jo-awake self.sm_jo_awake = Concurrence(outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'], default_outcome='succeeded', child_termination_cb=self.jo_awake_child_termination_cb, outcome_cb=self.jo_awake_outcome_cb) with self.sm_jo_awake: Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep) Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep) Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out) ##################################### # JO IS SLEEPING ##################################### # State machine for Jo-sleep-waking self.sm_jo_sleep_waking = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_sleep_waking: StateMachine.add('WAIT_WAKE_UP', MonitorState("/JO/wake_up", Empty, self.empty_cb), transitions={'valid':'WAKING_UP', 'preempted':'preempted', 'invalid':'WAKING_UP'}) StateMachine.add('WAKING_UP', JoWakingUp(), transitions={'succeeded':'succeeded'}) # State machine for Jo-sleep-bothwaking self.sm_jo_sleep_bothwaking = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_sleep_bothwaking: StateMachine.add('WAIT_WAKE_UP', MonitorState("/BOTH/wake_up", Empty, self.empty_cb), transitions={'valid':'WAKING_UP', 'preempted':'preempted', 'invalid':'WAKING_UP'}) StateMachine.add('WAKING_UP', BothWakingUp(), transitions={'succeeded':'succeeded'}) # State machine for Jo-awake self.sm_jo_sleep = Concurrence(outcomes=['succeeded','aborted','preempted', 'wake_up'], default_outcome='succeeded', child_termination_cb=self.jo_sleep_child_termination_cb, outcome_cb=self.jo_sleep_outcome_cb) with self.sm_jo_sleep: Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking) Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking) ##################################### # JO IS OUT TODO ##################################### # State machine for Jo-out-back self.sm_jo_out_back = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_out_back: StateMachine.add('WAIT_BACK_HOME', MonitorState("/JO/back_home", Empty, self.empty_cb), transitions={'valid':'WAIT_MYO', 'preempted':'preempted', 'invalid':'WAIT_MYO'}) StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={'valid':'COMING_BACK', 'preempted':'preempted', 'invalid':'COMING_BACK'}) StateMachine.add('COMING_BACK', JoBackHome(), transitions={'succeeded':'succeeded'}) # State machine for Jo-out-bothback self.sm_jo_out_bothback = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo_out_bothback: StateMachine.add('WAIT_BACK_HOME', MonitorState("/BOTH/back_home", Empty, self.empty_cb), transitions={'valid':'WAIT_MYO', 'preempted':'preempted', 'invalid':'WAIT_MYO'}) StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={'valid':'COMING_BACK', 'preempted':'preempted', 'invalid':'COMING_BACK'}) StateMachine.add('COMING_BACK', BothBackHome(), transitions={'succeeded':'succeeded'}) # State machine for Jo-out self.sm_jo_out = Concurrence(outcomes=['succeeded','aborted','preempted', 'back_home'], default_outcome='succeeded', child_termination_cb=self.jo_out_child_termination_cb, outcome_cb=self.jo_out_outcome_cb) with self.sm_jo_out: Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back) Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback) ##################################### # TOP LVL JO SM ##################################### # State machine for JO self.sm_jo = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_jo: StateMachine.add('AWAKE', self.sm_jo_awake, transitions={'succeeded':'succeeded', 'stop':'aborted', 'go_sleep':'SLEEP', 'go_out':'OUT'}) StateMachine.add('SLEEP', self.sm_jo_sleep, transitions={'succeeded':'succeeded', 'wake_up':'AWAKE'}) StateMachine.add('OUT', self.sm_jo_out, transitions={'succeeded':'succeeded', 'back_home':'AWAKE'}) ##################################### # TOP LVL CAROLE SM TODO ##################################### # State machine for CAROLE self.sm_carole = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_carole: StateMachine.add('WAIT3', MonitorState("/TEST/wait3", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'}) StateMachine.add('PAUSE', Pause(), transitions={'succeeded':'WAIT3', 'aborted':'aborted'}) ##################################### # TOP LVL EAT SM TODO ##################################### # State machine for EAT self.sm_eat = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_eat: StateMachine.add('WAIT2', MonitorState("/TEST/wait2", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'}) StateMachine.add('PAUSE', Pause(), transitions={'succeeded':'WAIT2', 'aborted':'aborted'}) ##################################### # TOP LVL SHOWER SM ##################################### # State machine for SHOWER self.sm_shower = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_shower: StateMachine.add('WAIT_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb), transitions={'valid':'PREPARING_SHOWER', 'preempted':'preempted', 'invalid':'PREPARING_SHOWER'}) StateMachine.add('PREPARING_SHOWER', PreparingShower(), transitions={'succeeded':'GO_SHOWER', 'aborted':'WAIT1'}) StateMachine.add('GO_SHOWER', GoShower(), transitions={'succeeded':'STOP_SHOWER', 'aborted':'aborted'}) StateMachine.add('STOP_SHOWER', StopShower(), transitions={'succeeded':'WAIT1', 'aborted':'aborted'}) ##################################### # TOP LVL SM ##################################### # Create the top level state machine self.sm_top = Concurrence(outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: Concurrence.add('SM_JO', self.sm_jo) Concurrence.add('SM_CAROLE', self.sm_carole) Concurrence.add('SM_EAT', self.sm_eat) Concurrence.add('SM_SHOWER', self.sm_shower) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def empty_cb(self, userdata, msg): return False def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_JO'] == 'succeeded': return True if outcome_map['SM_CAROLE'] == 'succeeded': return True if outcome_map['SM_EAT'] == 'succeeded': return True if outcome_map['SM_SHOWER'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_JO'] == 'succeeded': return 'succeeded' elif outcome_map['SM_CAROLE'] == 'succeeded': return 'succeeded' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_EAT'] == 'succeeded': return 'succeeded' elif outcome_map['SM_SHOWER'] == 'succeeded': return 'succeeded' else: return 'stop' # Gets called when ANY child state terminates def jo_awake_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_GO_TO_SLEEP'] == 'succeeded': return True if outcome_map['SM_BOTH_GO_TO_SLEEP'] == 'succeeded': return True if outcome_map['SM_GO_OUT'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def jo_awake_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_GO_TO_SLEEP'] == 'succeeded': return 'go_sleep' elif outcome_map['SM_BOTH_GO_TO_SLEEP'] == 'succeeded': return 'go_sleep' elif outcome_map['SM_GO_OUT'] == 'succeeded': return 'go_out' else: return 'stop' # Gets called when ANY child state terminates def jo_sleep_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_WAKE_UP'] == 'succeeded': return True if outcome_map['SM_BOTH_WAKE_UP'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def jo_sleep_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_WAKE_UP'] == 'succeeded': return 'wake_up' elif outcome_map['SM_BOTH_WAKE_UP'] == 'succeeded': return 'wake_up' else: return 'stop' # Gets called when ANY child state terminates def jo_out_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_BACK_HOME'] == 'succeeded': return True if outcome_map['SM_BOTH_BACK_HOME'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def jo_out_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_BACK_HOME'] == 'succeeded': return 'back_home' elif outcome_map['SM_BOTH_BACK_HOME'] == 'succeeded': return 'back_home' else: return 'stop' def shutdown(self): rospy.loginfo("Stopping the home automation...") self.sm_top.request_preempt() rospy.sleep(1)
class SMACHAI(): def __init__(self): rospy.init_node('smach_home_status', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) ##################################### # JO IS AWAKE ##################################### # State machine for Jo-awake-go-sleep self.sm_jo_awake_sleep = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_awake_sleep: StateMachine.add('LOOK_WAKE', MonitorState("/JO/sleep", Empty, self.empty_cb), transitions={ 'valid': 'GOING_SLEEP', 'preempted': 'preempted', 'invalid': 'GOING_SLEEP' }) StateMachine.add('GOING_SLEEP', JoGoingSleep(), transitions={'succeeded': 'LOOK_IN_BED'}) StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={ 'valid': 'IN_BED', 'preempted': 'preempted', 'invalid': 'IN_BED' }) StateMachine.add('IN_BED', JoInBed(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake-bothgo-sleep self.sm_jo_awake_bothsleep = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_awake_bothsleep: StateMachine.add('LOOK_WAKE', MonitorState("/BOTH/sleep", Empty, self.empty_cb), transitions={ 'valid': 'GOING_SLEEP', 'preempted': 'preempted', 'invalid': 'GOING_SLEEP' }) StateMachine.add('GOING_SLEEP', BothGoingSleep(), transitions={'succeeded': 'LOOK_IN_BED'}) StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={ 'valid': 'IN_BED', 'preempted': 'preempted', 'invalid': 'IN_BED' }) StateMachine.add('IN_BED', BothInBed(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake-go-out self.sm_jo_awake_out = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_awake_out: StateMachine.add('LOOK_OUT', MonitorState("/JO/go_out", Empty, self.empty_cb), transitions={ 'valid': 'PAUSE', 'preempted': 'preempted', 'invalid': 'PAUSE' }) StateMachine.add('PAUSE', Pause(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake self.sm_jo_awake = Concurrence( outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'], default_outcome='succeeded', child_termination_cb=self.jo_awake_child_termination_cb, outcome_cb=self.jo_awake_outcome_cb) with self.sm_jo_awake: Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep) Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep) Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out) ##################################### # JO IS SLEEPING ##################################### # State machine for Jo-sleep-waking self.sm_jo_sleep_waking = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_sleep_waking: StateMachine.add('WAIT_WAKE_UP', MonitorState("/JO/wake_up", Empty, self.empty_cb), transitions={ 'valid': 'WAKING_UP', 'preempted': 'preempted', 'invalid': 'WAKING_UP' }) StateMachine.add('WAKING_UP', JoWakingUp(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-sleep-bothwaking self.sm_jo_sleep_bothwaking = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_sleep_bothwaking: StateMachine.add('WAIT_WAKE_UP', MonitorState("/BOTH/wake_up", Empty, self.empty_cb), transitions={ 'valid': 'WAKING_UP', 'preempted': 'preempted', 'invalid': 'WAKING_UP' }) StateMachine.add('WAKING_UP', BothWakingUp(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake self.sm_jo_sleep = Concurrence( outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'], default_outcome='succeeded', child_termination_cb=self.jo_sleep_child_termination_cb, outcome_cb=self.jo_sleep_outcome_cb) with self.sm_jo_sleep: Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking) Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking) ##################################### # JO IS OUT TODO ##################################### # State machine for Jo-out-back self.sm_jo_out_back = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_out_back: StateMachine.add('WAIT_BACK_HOME', MonitorState("/JO/back_home", Empty, self.empty_cb), transitions={ 'valid': 'WAIT_MYO', 'preempted': 'preempted', 'invalid': 'WAIT_MYO' }) StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={ 'valid': 'COMING_BACK', 'preempted': 'preempted', 'invalid': 'COMING_BACK' }) StateMachine.add('COMING_BACK', JoBackHome(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-out-bothback self.sm_jo_out_bothback = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_out_bothback: StateMachine.add('WAIT_BACK_HOME', MonitorState("/BOTH/back_home", Empty, self.empty_cb), transitions={ 'valid': 'WAIT_MYO', 'preempted': 'preempted', 'invalid': 'WAIT_MYO' }) StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={ 'valid': 'COMING_BACK', 'preempted': 'preempted', 'invalid': 'COMING_BACK' }) StateMachine.add('COMING_BACK', BothBackHome(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-out self.sm_jo_out = Concurrence( outcomes=['succeeded', 'aborted', 'preempted', 'back_home'], default_outcome='succeeded', child_termination_cb=self.jo_out_child_termination_cb, outcome_cb=self.jo_out_outcome_cb) with self.sm_jo_out: Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back) Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback) ##################################### # TOP LVL JO SM ##################################### # State machine for JO self.sm_jo = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo: StateMachine.add('AWAKE', self.sm_jo_awake, transitions={ 'succeeded': 'succeeded', 'stop': 'aborted', 'go_sleep': 'SLEEP', 'go_out': 'OUT' }) StateMachine.add('SLEEP', self.sm_jo_sleep, transitions={ 'succeeded': 'succeeded', 'wake_up': 'AWAKE' }) StateMachine.add('OUT', self.sm_jo_out, transitions={ 'succeeded': 'succeeded', 'back_home': 'AWAKE' }) ##################################### # TOP LVL CAROLE SM TODO ##################################### # State machine for CAROLE self.sm_carole = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_carole: StateMachine.add('WAIT3', MonitorState("/TEST/wait3", Empty, self.empty_cb), transitions={ 'valid': 'PAUSE', 'preempted': 'preempted', 'invalid': 'PAUSE' }) StateMachine.add('PAUSE', Pause(), transitions={ 'succeeded': 'WAIT3', 'aborted': 'aborted' }) ##################################### # TOP LVL EAT SM TODO ##################################### # State machine for EAT self.sm_eat = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_eat: StateMachine.add('WAIT2', MonitorState("/TEST/wait2", Empty, self.empty_cb), transitions={ 'valid': 'PAUSE', 'preempted': 'preempted', 'invalid': 'PAUSE' }) StateMachine.add('PAUSE', Pause(), transitions={ 'succeeded': 'WAIT2', 'aborted': 'aborted' }) ##################################### # TOP LVL SHOWER SM ##################################### # State machine for SHOWER self.sm_shower = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_shower: StateMachine.add('WAIT_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb), transitions={ 'valid': 'PREPARING_SHOWER', 'preempted': 'preempted', 'invalid': 'PREPARING_SHOWER' }) StateMachine.add('PREPARING_SHOWER', PreparingShower(), transitions={ 'succeeded': 'GO_SHOWER', 'aborted': 'WAIT1' }) StateMachine.add('GO_SHOWER', GoShower(), transitions={ 'succeeded': 'STOP_SHOWER', 'aborted': 'aborted' }) StateMachine.add('STOP_SHOWER', StopShower(), transitions={ 'succeeded': 'WAIT1', 'aborted': 'aborted' }) ##################################### # TOP LVL SM ##################################### # Create the top level state machine self.sm_top = Concurrence( outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: Concurrence.add('SM_JO', self.sm_jo) Concurrence.add('SM_CAROLE', self.sm_carole) Concurrence.add('SM_EAT', self.sm_eat) Concurrence.add('SM_SHOWER', self.sm_shower) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def empty_cb(self, userdata, msg): return False def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_JO'] == 'succeeded': return True if outcome_map['SM_CAROLE'] == 'succeeded': return True if outcome_map['SM_EAT'] == 'succeeded': return True if outcome_map['SM_SHOWER'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_JO'] == 'succeeded': return 'succeeded' elif outcome_map['SM_CAROLE'] == 'succeeded': return 'succeeded' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_EAT'] == 'succeeded': return 'succeeded' elif outcome_map['SM_SHOWER'] == 'succeeded': return 'succeeded' else: return 'stop' # Gets called when ANY child state terminates def jo_awake_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_GO_TO_SLEEP'] == 'succeeded': return True if outcome_map['SM_BOTH_GO_TO_SLEEP'] == 'succeeded': return True if outcome_map['SM_GO_OUT'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def jo_awake_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_GO_TO_SLEEP'] == 'succeeded': return 'go_sleep' elif outcome_map['SM_BOTH_GO_TO_SLEEP'] == 'succeeded': return 'go_sleep' elif outcome_map['SM_GO_OUT'] == 'succeeded': return 'go_out' else: return 'stop' # Gets called when ANY child state terminates def jo_sleep_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_WAKE_UP'] == 'succeeded': return True if outcome_map['SM_BOTH_WAKE_UP'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def jo_sleep_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_WAKE_UP'] == 'succeeded': return 'wake_up' elif outcome_map['SM_BOTH_WAKE_UP'] == 'succeeded': return 'wake_up' else: return 'stop' # Gets called when ANY child state terminates def jo_out_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_BACK_HOME'] == 'succeeded': return True if outcome_map['SM_BOTH_BACK_HOME'] == 'succeeded': return True else: return False # Gets called when ALL child states are terminated def jo_out_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['SM_BACK_HOME'] == 'succeeded': return 'back_home' elif outcome_map['SM_BOTH_BACK_HOME'] == 'succeeded': return 'back_home' else: return 'stop' def shutdown(self): rospy.loginfo("Stopping the home automation...") self.sm_top.request_preempt() rospy.sleep(1)
class SMACHAI(): def __init__(self): rospy.init_node('HOME_automation_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi / 2, pi, 3 * pi / 2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append( Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append( Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append( Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # State machine for light entry self.sm_light_entry = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_light_entry.userdata.day_mode = 1 with self.sm_light_entry: StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb), transitions={ 'valid': 'LOOK_ENTRY', 'invalid': 'LIGHT_UP' }) StateMachine.add('LIGHT_UP', LightEntry(), transitions={'succeeded': 'succeeded'}) # State machine for dark entry self.sm_dark_entry = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_dark_entry.userdata.day_mode = 1 with self.sm_dark_entry: StateMachine.add('LOOK_ENTRY_OFF', MonitorState("/HOME/entry_noOne", Empty, self.empty_cb), transitions={ 'valid': 'LOOK_ENTRY_OFF', 'invalid': 'LIGHT_DOWN' }) StateMachine.add('LIGHT_DOWN', DarkEntry(), transitions={'succeeded': 'succeeded'}) # State machine for day mode self.sm_day_mode = Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'go_shower', 'go_sleep', 'go_eat', 'go_out' ], default_outcome='succeeded', child_termination_cb=self.daymode_child_termination_cb, outcome_cb=self.daymode_outcome_cb) self.sm_day_mode.userdata.day_mode = 1 with self.sm_day_mode: Concurrence.add( 'LOOK_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb)) Concurrence.add( 'LOOK_LEAVING', MonitorState("/HOME/leaving_home", Empty, self.empty_cb)) Concurrence.add( 'LOOK_SLEEP', MonitorState("/HOME/go_sleep", Empty, self.empty_cb)) Concurrence.add('LOOK_EAT', MonitorState("/HOME/go_eat", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTRY', self.sm_light_entry) Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry) # State machine for leaving home self.sm_leaving_home = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_leaving_home: StateMachine.add('LEAV', Pause(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # State machine for going to sleep self.sm_going_sleep = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_going_sleep: StateMachine.add('GOING_SLEEP', GoingSleep(), transitions={'succeeded': 'succeeded'}) # State machine for day mode self.sm_wait_bed = Concurrence( outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=self.useless_child_termination_cb, outcome_cb=self.useless_outcome_cb) with self.sm_wait_bed: Concurrence.add( 'LOOK_INBED', MonitorState("/METAWATCH/button2", Empty, self.empty_cb)) Concurrence.add('TIMEOUT', TimeoutToBed()) # State machine for in bed self.sm_in_bed = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_in_bed: StateMachine.add('IN_BED', InBed(), transitions={'succeeded': 'succeeded'}) # State machine for light entry self.sm_lightn_entry = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_lightn_entry.userdata.day_mode = 0 with self.sm_lightn_entry: StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb), transitions={ 'valid': 'LOOK_ENTRY', 'invalid': 'LIGHT_UP' }) StateMachine.add('LIGHT_UP', LightEntry(), transitions={'succeeded': 'succeeded'}) # State machine for night mode self.sm_night_mode = Concurrence( outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'], default_outcome='succeeded', child_termination_cb=self.nightmode_child_termination_cb, outcome_cb=self.nightmode_outcome_cb) self.sm_night_mode.userdata.day_mode = 0 with self.sm_night_mode: Concurrence.add( 'LOOK_WAKE', MonitorState("/HOME/wake_up", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTRY', self.sm_lightn_entry) Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry) # State machine for night mode #self.sm_night_mode = StateMachine(outcomes=['succeeded','aborted','preempted']) #self.sm_night_mode.userdata.day_mode = 0; #with self.sm_night_mode: # StateMachine.add('NIGHT_MOD', Pause(), # transitions={'succeeded':'succeeded', # 'aborted':'aborted'}) # State machine for waking up self.sm_waking_up = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_waking_up.userdata.day_mode = 0 with self.sm_waking_up: StateMachine.add('WAKING_UP', WakingUp(), transitions={'succeeded': 'succeeded'}) # State machine for waking up self.sm_going_eat = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_going_eat.userdata.day_mode = 1 with self.sm_going_eat: StateMachine.add('EATTTTTTTT', Pause(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # State machine for home self.sm_home = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'going_out']) with self.sm_home: StateMachine.add('DAY_MODE', self.sm_day_mode, transitions={ 'succeeded': 'DAY_MODE', 'go_shower': 'PREPARING_SHOWER', 'go_sleep': 'GOING_SLEEP', 'go_eat': 'GOING_EAT', 'go_out': 'LEAVING_HOME', 'aborted': 'aborted' }) StateMachine.add('PREPARING_SHOWER', PreparingShower(), transitions={ 'succeeded': 'GO_SHOWER', 'aborted': 'aborted' }) StateMachine.add('GO_SHOWER', GoShower(), transitions={ 'succeeded': 'STOP_SHOWER', 'aborted': 'aborted' }) StateMachine.add('STOP_SHOWER', StopShower(), transitions={ 'succeeded': 'DAY_MODE', 'aborted': 'aborted' }) StateMachine.add('LEAVING_HOME', self.sm_leaving_home, transitions={ 'succeeded': 'going_out', 'aborted': 'aborted' }) StateMachine.add('GOING_SLEEP', self.sm_going_sleep, transitions={ 'succeeded': 'WAIT_TO_BED', 'aborted': 'aborted' }) StateMachine.add('WAIT_TO_BED', self.sm_wait_bed, transitions={ 'succeeded': 'IN_BED', 'preempted': 'IN_BED', 'aborted': 'aborted' }) StateMachine.add('IN_BED', self.sm_in_bed, transitions={ 'succeeded': 'NIGHT_MODE', 'aborted': 'aborted' }) StateMachine.add('NIGHT_MODE', self.sm_night_mode, transitions={ 'succeeded': 'NIGHT_MODE', 'wake_up': 'WAKING_UP', 'aborted': 'aborted' }) StateMachine.add('WAKING_UP', self.sm_waking_up, transitions={ 'succeeded': 'DAY_MODE', 'aborted': 'aborted' }) StateMachine.add('GOING_EAT', self.sm_going_eat, transitions={ 'succeeded': 'DAY_MODE', 'aborted': 'aborted' }) # State machine for waking up self.sm_guarding = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_guarding: StateMachine.add('GUARD', Pause(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # State machine with concurrence self.sm_incoming_home = Concurrence( outcomes=['succeeded', 'aborted'], default_outcome='succeeded', child_termination_cb=self.incoming_child_termination_cb, outcome_cb=self.incoming_outcome_cb) # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine with self.sm_incoming_home: Concurrence.add( 'LOOK_CONNECTION', MonitorState("/METAWATCH/connected", Empty, self.empty_cb)) Concurrence.add( 'LOOK_ENTERING', MonitorState("/HOME/entry_door_open", Empty, self.empty_cb)) # State machine for away self.sm_away = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'entering_home']) with self.sm_away: StateMachine.add('GUARDING_MODE', self.sm_guarding, transitions={ 'succeeded': 'INCOMING_HOME', 'aborted': 'aborted' }) StateMachine.add('INCOMING_HOME', self.sm_incoming_home, transitions={ 'succeeded': 'entering_home', 'aborted': 'aborted' }) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('AT_HOME', self.sm_home, transitions={ 'succeeded': 'AT_HOME', 'going_out': 'AWAY' }) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('AWAY', self.sm_away, transitions={ 'succeeded': 'AWAY', 'entering_home': 'AT_HOME' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def empty_cb(self, userdata, msg): #rospy.loginfo("Empty message received.") return False # Gets called when ANY child state terminates def useless_child_termination_cb(self, outcome_map): rospy.loginfo("useless_child_termination_cb.") return True # Gets called when ALL child states are terminated def useless_outcome_cb(self, outcome_map): rospy.loginfo("useless_outcome_cb.") return 'succeeded' # Gets called when ANY child state terminates def daymode_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def daymode_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") # If the battery is below threshold, return the 'recharge' outcome if outcome_map['LOOK_SHOWER'] == 'invalid': rospy.loginfo("Going to shower ") return 'go_shower' if outcome_map['LOOK_LEAVING'] == 'invalid': rospy.loginfo("Leaving home") return 'go_out' if outcome_map['LOOK_SLEEP'] == 'invalid': rospy.loginfo("Going to sleep") return 'go_sleep' if outcome_map['LOOK_EAT'] == 'invalid': rospy.loginfo("Going to eat") return 'go_eat' if outcome_map['LOOK_ENTRY'] == 'succeeded': rospy.loginfo("Restart looking entry") return 'succeeded' if outcome_map['LOOK_ENTRY_OFF'] == 'succeeded': rospy.loginfo("No one in the entry") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def nightmode_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def nightmode_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") # If the battery is below threshold, return the 'recharge' outcome if outcome_map['LOOK_WAKE'] == 'invalid': rospy.loginfo("Wake up dude !") return 'wake_up' if outcome_map['LOOK_ENTRY'] == 'succeeded': rospy.loginfo("Restart looking entry") return 'succeeded' if outcome_map['LOOK_ENTRY_OFF'] == 'succeeded': rospy.loginfo("No one in the entry") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def incoming_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['LOOK_CONNECTION'] == 'succeeded': rospy.loginfo("MW connected. Welcome back.") return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['LOOK_ENTERING'] == 'succeeded': rospy.loginfo("Someone entering...") return True else: return False # Gets called when ALL child states are terminated def incoming_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['LOOK_CONNECTION'] == 'succeeded': rospy.loginfo("MW connected. Welcome back ") return 'succeeded' if outcome_map['LOOK_ENTERING'] == 'succeeded': rospy.loginfo("Someone entering..") return 'succeeded' else: return 'aborted' def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if (waypoint_type == 1): return 'action1' if (waypoint_type == 2): return 'action2' if (waypoint_type == 3): return 'action3' if (waypoint_type == 4): return 'action4' if (waypoint_type == 5): return 'action5' if (waypoint_type == 6): return 'action6' return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping home automation...") self.sm_day_mode.request_preempt() rospy.sleep(1)
class SMACHAI(): def __init__(self): rospy.init_node('HOME_automation_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # State machine for light entry self.sm_light_entry = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_light_entry.userdata.day_mode = 1; with self.sm_light_entry: StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb), transitions={'valid':'LOOK_ENTRY', 'invalid':'LIGHT_UP'}) StateMachine.add('LIGHT_UP', LightEntry(), transitions={'succeeded':'succeeded'}) # State machine for dark entry self.sm_dark_entry = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_dark_entry.userdata.day_mode = 1; with self.sm_dark_entry: StateMachine.add('LOOK_ENTRY_OFF', MonitorState("/HOME/entry_noOne", Empty, self.empty_cb), transitions={'valid':'LOOK_ENTRY_OFF', 'invalid':'LIGHT_DOWN'}) StateMachine.add('LIGHT_DOWN', DarkEntry(), transitions={'succeeded':'succeeded'}) # State machine for day mode self.sm_day_mode = Concurrence(outcomes=['succeeded','aborted','preempted','go_shower','go_sleep','go_eat','go_out'], default_outcome='succeeded', child_termination_cb=self.daymode_child_termination_cb, outcome_cb=self.daymode_outcome_cb) self.sm_day_mode.userdata.day_mode = 1; with self.sm_day_mode: Concurrence.add('LOOK_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb)) Concurrence.add('LOOK_LEAVING', MonitorState("/HOME/leaving_home", Empty, self.empty_cb)) Concurrence.add('LOOK_SLEEP', MonitorState("/HOME/go_sleep", Empty, self.empty_cb)) Concurrence.add('LOOK_EAT', MonitorState("/HOME/go_eat", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTRY', self.sm_light_entry) Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry) # State machine for leaving home self.sm_leaving_home = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_leaving_home: StateMachine.add('LEAV', Pause(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for going to sleep self.sm_going_sleep = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_going_sleep: StateMachine.add('GOING_SLEEP', GoingSleep(), transitions={'succeeded':'succeeded'}) # State machine for day mode self.sm_wait_bed = Concurrence(outcomes=['succeeded','aborted','preempted'], default_outcome='succeeded', child_termination_cb=self.useless_child_termination_cb, outcome_cb=self.useless_outcome_cb) with self.sm_wait_bed: Concurrence.add('LOOK_INBED', MonitorState("/METAWATCH/button2", Empty, self.empty_cb)) Concurrence.add('TIMEOUT', TimeoutToBed()) # State machine for in bed self.sm_in_bed = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_in_bed: StateMachine.add('IN_BED', InBed(), transitions={'succeeded':'succeeded'}) # State machine for light entry self.sm_lightn_entry = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_lightn_entry.userdata.day_mode = 0; with self.sm_lightn_entry: StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb), transitions={'valid':'LOOK_ENTRY', 'invalid':'LIGHT_UP'}) StateMachine.add('LIGHT_UP', LightEntry(), transitions={'succeeded':'succeeded'}) # State machine for night mode self.sm_night_mode = Concurrence(outcomes=['succeeded','aborted','preempted','wake_up'], default_outcome='succeeded', child_termination_cb=self.nightmode_child_termination_cb, outcome_cb=self.nightmode_outcome_cb) self.sm_night_mode.userdata.day_mode = 0; with self.sm_night_mode: Concurrence.add('LOOK_WAKE', MonitorState("/HOME/wake_up", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTRY', self.sm_lightn_entry) Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry) # State machine for night mode #self.sm_night_mode = StateMachine(outcomes=['succeeded','aborted','preempted']) #self.sm_night_mode.userdata.day_mode = 0; #with self.sm_night_mode: # StateMachine.add('NIGHT_MOD', Pause(), # transitions={'succeeded':'succeeded', # 'aborted':'aborted'}) # State machine for waking up self.sm_waking_up = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_waking_up.userdata.day_mode = 0; with self.sm_waking_up: StateMachine.add('WAKING_UP', WakingUp(), transitions={'succeeded':'succeeded'}) # State machine for waking up self.sm_going_eat = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_going_eat.userdata.day_mode = 1; with self.sm_going_eat: StateMachine.add('EATTTTTTTT', Pause(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for home self.sm_home = StateMachine(outcomes=['succeeded','aborted','preempted','going_out']) with self.sm_home: StateMachine.add('DAY_MODE', self.sm_day_mode, transitions={'succeeded':'DAY_MODE', 'go_shower':'PREPARING_SHOWER', 'go_sleep':'GOING_SLEEP', 'go_eat':'GOING_EAT', 'go_out':'LEAVING_HOME', 'aborted':'aborted'}) StateMachine.add('PREPARING_SHOWER', PreparingShower(), transitions={'succeeded':'GO_SHOWER', 'aborted':'aborted'}) StateMachine.add('GO_SHOWER', GoShower(), transitions={'succeeded':'STOP_SHOWER', 'aborted':'aborted'}) StateMachine.add('STOP_SHOWER', StopShower(), transitions={'succeeded':'DAY_MODE', 'aborted':'aborted'}) StateMachine.add('LEAVING_HOME', self.sm_leaving_home, transitions={'succeeded':'going_out', 'aborted':'aborted'}) StateMachine.add('GOING_SLEEP', self.sm_going_sleep, transitions={'succeeded':'WAIT_TO_BED', 'aborted':'aborted'}) StateMachine.add('WAIT_TO_BED', self.sm_wait_bed, transitions={'succeeded':'IN_BED', 'preempted':'IN_BED', 'aborted':'aborted'}) StateMachine.add('IN_BED', self.sm_in_bed, transitions={'succeeded':'NIGHT_MODE', 'aborted':'aborted'}) StateMachine.add('NIGHT_MODE', self.sm_night_mode, transitions={'succeeded':'NIGHT_MODE', 'wake_up':'WAKING_UP', 'aborted':'aborted'}) StateMachine.add('WAKING_UP', self.sm_waking_up, transitions={'succeeded':'DAY_MODE', 'aborted':'aborted'}) StateMachine.add('GOING_EAT', self.sm_going_eat, transitions={'succeeded':'DAY_MODE', 'aborted':'aborted'}) # State machine for waking up self.sm_guarding = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_guarding: StateMachine.add('GUARD', Pause(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine with concurrence self.sm_incoming_home = Concurrence(outcomes=['succeeded', 'aborted'], default_outcome='succeeded', child_termination_cb=self.incoming_child_termination_cb, outcome_cb=self.incoming_outcome_cb) # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine with self.sm_incoming_home: Concurrence.add('LOOK_CONNECTION', MonitorState("/METAWATCH/connected", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTERING', MonitorState("/HOME/entry_door_open", Empty, self.empty_cb)) # State machine for away self.sm_away = StateMachine(outcomes=['succeeded','aborted','preempted','entering_home']) with self.sm_away: StateMachine.add('GUARDING_MODE', self.sm_guarding, transitions={'succeeded':'INCOMING_HOME', 'aborted':'aborted'}) StateMachine.add('INCOMING_HOME', self.sm_incoming_home, transitions={'succeeded':'entering_home', 'aborted':'aborted'}) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('AT_HOME', self.sm_home, transitions={'succeeded':'AT_HOME', 'going_out':'AWAY'}) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('AWAY', self.sm_away, transitions={'succeeded':'AWAY', 'entering_home':'AT_HOME'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def empty_cb(self, userdata, msg): #rospy.loginfo("Empty message received.") return False # Gets called when ANY child state terminates def useless_child_termination_cb(self, outcome_map): rospy.loginfo("useless_child_termination_cb.") return True # Gets called when ALL child states are terminated def useless_outcome_cb(self, outcome_map): rospy.loginfo("useless_outcome_cb.") return 'succeeded' # Gets called when ANY child state terminates def daymode_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def daymode_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") # If the battery is below threshold, return the 'recharge' outcome if outcome_map['LOOK_SHOWER'] == 'invalid': rospy.loginfo("Going to shower ") return 'go_shower' if outcome_map['LOOK_LEAVING'] == 'invalid': rospy.loginfo("Leaving home") return 'go_out' if outcome_map['LOOK_SLEEP'] == 'invalid': rospy.loginfo("Going to sleep") return 'go_sleep' if outcome_map['LOOK_EAT'] == 'invalid': rospy.loginfo("Going to eat") return 'go_eat' if outcome_map['LOOK_ENTRY'] == 'succeeded': rospy.loginfo("Restart looking entry") return 'succeeded' if outcome_map['LOOK_ENTRY_OFF'] == 'succeeded': rospy.loginfo("No one in the entry") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def nightmode_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def nightmode_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") # If the battery is below threshold, return the 'recharge' outcome if outcome_map['LOOK_WAKE'] == 'invalid': rospy.loginfo("Wake up dude !") return 'wake_up' if outcome_map['LOOK_ENTRY'] == 'succeeded': rospy.loginfo("Restart looking entry") return 'succeeded' if outcome_map['LOOK_ENTRY_OFF'] == 'succeeded': rospy.loginfo("No one in the entry") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def incoming_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['LOOK_CONNECTION'] == 'succeeded': rospy.loginfo("MW connected. Welcome back.") return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['LOOK_ENTERING'] == 'succeeded': rospy.loginfo("Someone entering...") return True else: return False # Gets called when ALL child states are terminated def incoming_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['LOOK_CONNECTION'] == 'succeeded': rospy.loginfo("MW connected. Welcome back ") return 'succeeded' if outcome_map['LOOK_ENTERING'] == 'succeeded': rospy.loginfo("Someone entering..") return 'succeeded' else: return 'aborted' def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if(waypoint_type == 1): return 'action1' if(waypoint_type == 2): return 'action2' if(waypoint_type == 3): return 'action3' if(waypoint_type == 4): return 'action4' if(waypoint_type == 5): return 'action5' if(waypoint_type == 6): return 'action6' return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping home automation...") self.sm_day_mode.request_preempt() rospy.sleep(1)