class sm_Follow():
def __init__(self):
rospy.init_node('sm_Follow')
self.sm_FollowMe = Concurrence(
outcomes=['succeeded', 'aborted', 'error'],
default_outcome='succeeded',
outcome_map={'succeeded': {
'STOP': 'succeeded'
}},
child_termination_cb=self.child_cb)
# if one state complete, the Concurrence will give a preempted signal, and will stop the current state in the preempt outcomes
with self.sm_FollowMe:
self.meta_Follow = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.meta_Follow:
self.meta_Follow.userdata.pos_xm = Pose()
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'FIND_PEOPLE',
'aborted': 'aborted'
})
StateMachine.add('FIND_PEOPLE',
FindPeople().find_people_,
remapping={'pos_xm': 'pos_xm'},
transitions={
'invalid': 'MOVE',
'valid': 'FIND_PEOPLE',
'preempted': 'aborted'
})
StateMachine.add('MOVE',
NavStack(),
remapping={'pos_xm': 'pos_xm'},
transitions={
'succeeded': 'FIND_PEOPLE',
'aborted': 'MOVE',
'error': 'error'
})
self.meta_Stop = StateMachine(outcomes=['succeeded', 'aborted'])
with self.meta_Stop:
StateMachine.add('STOP',
StopFollow(),
transitions={
'succeeded': 'succeeded',
'aborted': 'STOP'
})
Concurrence.add('FOLLOW', self.meta_Follow)
Concurrence.add('STOP', self.meta_Stop)
self.sm_FollowMe.execute()
def child_cb(self, outcome_map):
if outcome_map['STOP'] == 'succeeded':
rospy.logerr('f**k')
subprocess.call("/home/xiong/Recognition/kinect2/terminate_people",
shell=True)
return True
class SmachHaha():
def __init__(self):
rospy.init_node('smach_haha')
rospy.on_shutdown(self.shutdown)
self.smach_top = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
outcome_map={'succeeded': {
'STATE_1': 'succeeded'
}},
child_termination_cb=self.child_termination_cb)
with self.smach_top:
self.sm_state2 = StateMachine(outcomes=['succeeded'])
with self.sm_state2:
StateMachine.add('FIRST',
State2(),
transitions={'succeeded': 'SECOND'})
StateMachine.add('SECOND',
State1(),
transitions={'succeeded': 'FIRST'})
Concurrence.add('STATE_1', State1())
Concurrence.add('STATE_2', self.sm_state2)
haha = self.smach_top.execute()
def child_termination_cb(self, outcome_map):
if outcome_map['STATE_1'] == 'succeeded':
rospy.logerr('f**k you')
return True
def people_cb(self, UserData, msg):
return False
def shutdown(self):
rospy.logerr('smach test over')
class sm_Follow():
def __init__(self):
rospy.init_node('sm_Follow')
self.trace = Concurrence(
outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
# outcome_cb = self.trace_out_cb,
child_termination_cb=self.trace_child_cb)
with self.trace:
self.meta_follow = StateMachine(['succeeded', 'aborted'])
with self.meta_follow:
StateMachine.add('FOLLOW',
SimpleFollow(),
transitions={
'succeeded': 'FINDPEOPLE',
'aborted': 'aborted'
})
Concurrence.add('RUNNODE', RunNode())
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
out = self.trace.execute()
print out
def trace_child_cb(self, outcome_map):
if outcome_map['STOP'] == 'stop':
rospy.logwarn('get the stop signal, stop tracing ........')
subprocess.call('xterm -e rosnode kill people_tracking &',
shell=True)
return True
elif outcome_map['STOP'] == 'aborted':
rospy.logerr('the stop state meet error!')
return True
if outcome_map['FOLLOW']:
rospy.logerr('the follow state meet error!')
return True
return False
# rospy.logerr('byebye')
out = self.xm_follow.execute()
if out == 'succeeded':
rospy.logwarn('test succeeded')
def test_outcome_cb(self):
"""Test concurrent container that doesnt preempt siblings."""
cc = Concurrence(['succeeded', 'done'],
default_outcome='done',
child_termination_cb=lambda so: False,
outcome_cb=lambda so: list(set(so.values()))[0])
with cc:
Concurrence.add('SETTER', Setter())
Concurrence.add('GETTER', Getter())
outcome = cc.execute()
assert outcome == 'done'
assert 'a' in cc.userdata
assert 'b' in cc.userdata
assert cc.userdata.a == 'A'
assert cc.userdata.b == 'A'
def test_preempt(self):
"""Test concurrent container that preempts siblings."""
cc = Concurrence(['succeeded', 'done'],
default_outcome='done',
child_termination_cb=lambda so: True,
outcome_map={
'succeeded': {
'SETTER': 'done',
'GETTER': 'preempted'
}
})
with cc:
Concurrence.add('SETTER', Setter())
Concurrence.add('GETTER', Getter())
outcome = cc.execute()
assert outcome == 'succeeded'
assert 'a' in cc.userdata
assert 'b' in cc.userdata
assert cc.userdata.a == 'A'
assert cc.userdata.b == 'A'
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)
