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)
