def explorationWithVictims():
def _termination_cb(outcome_map):
return True
cc = Concurrence(outcomes=['thermal_alert', 'camera_alert', 'preempted'],
default_outcome='preempted',
outcome_map={
'thermal_alert': {
'VICTIM_MONITOR': 'thermal'
},
'camera_alert': {
'VICTIM_MONITOR': 'camera'
},
'preempted': {
'EXPLORE': 'preempted',
'VICTIM_MONITOR': 'preempted'
}
},
child_termination_cb=_termination_cb)
with cc:
Concurrence.add('VICTIM_MONITOR', MonitorVictimState())
Concurrence.add('EXPLORE', simpleExplorationContainer())
return cc
def ExplorationContainer():
cc = Concurrence(outcomes=['victim_thermal','victim_camera','aborted','preempted','time_out'],
default_outcome='aborted', outcome_map={'victim_thermal':{'VICTIM_MONITOR':'victim_thermal'},
'victim_camera':{'VICTIM_MONITOR':'victim_camera'},'preempted':{'EXPLORE':'preempted','VICTIM_MONITOR':'preempted'},
'aborted':{'EXPLORE':'aborted'}, 'time_out':{'EXPLORE':'time_out'}},
child_termination_cb=_termination_cb)
with cc:
#~ Concurrence.add('TARGET_CONTROLLER', utils.TargetSelectorContainer('explore'))
Concurrence.add('EXPLORE', utils.make_iterator(utils.TargetSelectorContainer('explore'), max_iter=100))
sm_victim_monitor = StateMachine(outcomes=['victim_thermal','victim_camera','preempted'])
sm_victim_monitor.userdata.victim_type = 0
with sm_victim_monitor:
StateMachine.add('VICTIM_MONITORING', MonitorVictimState(
input_keys=['victim_type'], output_keys=['victim_type']),
transitions={'invalid':'VICTIM_DECIDE', 'valid':'VICTIM_MONITORING', 'preempted':'preempted'},
remapping={'victim_type':'victim_type'})
StateMachine.add('VICTIM_DECIDE', DecideVictimState(),
transitions={'thermal':'victim_thermal','camera':'victim_camera'})
Concurrence.add('VICTIM_MONITOR', sm_victim_monitor)
return cc
def main():
rospy.init_node('tinker_mission_follow')
rospy.loginfo(colored('starting follow and guide task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(),
transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
with sequence:
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
Concurrence(default_outcome='aborted',
input_keys=['text'],
output_keys=['text'],
outcomes=['succeeded', 'preempted', 'aborted'],
child_termination_cb=self.getfinish_Cb,
outcome_cb=self.outcome_Cb)
jointLoop = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with jointLoop:
StateMachine.add('NEXT_MOVE',
RandomSelectionFromPoolState(self._movementList),
output_keys={'joint_angles'},
transitions={'succeeded': 'MOVEMENT'}
)
StateMachine.add('MOVEMENT',
JointAngleState(['HeadPitch', 'HeadYaw']),
transitions={'succeeded': 'NEXT_MOVE'}
)
with self:
Concurrence.add('MOVING',
jointLoop,
transitions={'succeeded': 'succeeded'}
)
Concurrence.add('GET_USER_ANSWER',
GetUserAnswer(),
transitions={'succeeded': 'succeeded',
'aborted': 'aborted'},
remapping={'text': 'text'}
)
def main():
rospy.init_node('smach_usecase_executive')
sm_root = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
request = turtlesim.srv.SpawnRequest(0.0, 0.0, 0.0, 'turtle2')
smach.StateMachine.add(
'SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request),
{'succeeded': 'TELEPORT1'})
teleport1 = turtlesim.srv.TeleportAbsoluteRequest(5.0, 1.0, 0.0)
smach.StateMachine.add(
'TELEPORT1',
ServiceState('turtle1/teleport_absolute',
turtlesim.srv.TeleportAbsolute, teleport1),
{'succeeded': 'TELEPORT2'})
teleport2 = turtlesim.srv.TeleportAbsoluteRequest(9.0, 5.0, 0.0)
smach.StateMachine.add(
'TELEPORT2',
ServiceState('turtle2/teleport_absolute',
turtlesim.srv.TeleportAbsolute, teleport2),
{'succeeded': 'SHAPES'})
shapes_concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'BIG': 'succeeded',
'SMALL': 'succeeded'
}
})
smach.StateMachine.add('SHAPES', shapes_concurrence)
with shapes_concurrence:
Concurrence.add(
'BIG',
SimpleActionState('turtle_shape1', ShapeAction,
ShapeGoal(11, 4.0)), {'succeeded': 'SMALL'})
Concurrence.add(
'SMALL',
SimpleActionState('turtle_shape2', ShapeAction,
ShapeGoal(6, 0.5)))
sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
def main():
smach_states = rospy.get_param("smach_states")
# The autonomy_sm handles the full autonomy sequence for the competition run
autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with autonomy_sm:
# TODO: create localization state. might just be a MonitorState
# Sequentially add all the states from the config file to the state machine,
# where state i transitions to state i+1
names = []
for i in range(len(smach_states)):
state = smach_states[i]
name, action_state = create_action_state(state)
# If the state name is already in the state machine, add the new
# one with increasing numbers
if name in names:
name = name+"2"
while name in names:
name = name[:-1]+str(int(name[-1])+1) # increase num
names.append(name)
StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded'])
# StateMachine.add()
# Create the concurrence contained for the fully autonomy sequence. This
# runs the state machine for the competition run. It also concurrently runs
# a state with a timer counting down from 10 minutes and a state that listens
# to the /click/start_button topic. If either of these are triggered, it will
# end autonomy and place us into the teleop state.
# TODO: add 10 minute competition timer state
autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'],
default_outcome='enter_teleop',
child_termination_cb=autonomy_child_term_cb,
outcome_cb=autonomy_out_cb)
with autonomy_concurrence:
# state that runs full autonomy state machine
Concurrence.add('AUTONOMY', autonomy_sm)
# state that listens for toggle message
Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb))
# Top level state machine, containing the autonomy and teleop machines.
top_sm = StateMachine(outcomes=['DONE'])
with top_sm:
StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'})
StateMachine.add('AUTONOMY_MODE', autonomy_concurrence,
transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'})
#StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb),
# transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})
sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH')
sis.start()
top_sm.execute()
rospy.spin()
sis.stop()
def dataFusionHold():
def _termination_cb(outcome_map):
return True
sm = StateMachine(outcomes=['verified', 'not_verified', 'preempted'],
input_keys=['victim_info'],
output_keys=['victim_info'])
with sm:
#~ sm.userdata.victim_info = None
cc = Concurrence(outcomes=['verified', 'time_out', 'preempted'],
default_outcome='time_out',
outcome_map={
'verified': {
'MONITOR_VERIFICATION': 'got_verification'
},
'time_out': {
'MONITOR_VERIFICATION': 'preempted',
'TIMER': 'time_out'
},
'preempted': {
'TIMER': 'preempted',
'MONITOR_VERIFICATION': 'preempted'
}
},
child_termination_cb=_termination_cb,
input_keys=['victim_info'],
output_keys=['victim_info'])
with cc:
Concurrence.add('MONITOR_VERIFICATION',
VictimVerificationState(),
remapping={'victim_info': 'victim_info'})
Concurrence.add('TIMER', DataFusionHold())
StateMachine.add('WAIT_FOR_DF',
cc,
transitions={
'time_out': 'DELETE_CURRENT_HOLE',
'verified': 'verified',
'preempted': 'preempted'
},
remapping={'victim_info': 'victim_info'})
StateMachine.add('DELETE_CURRENT_HOLE',
ValidateHoleState(False),
transitions={
'succeeded': 'not_verified',
'preempted': 'preempted'
})
return sm
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
def main():
rospy.init_node('tinker_mission_navigation')
rospy.loginfo(colored('starting navigation task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
# Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
# Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one'))
# Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'),
# transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'})
# Sequence.add('Obstacle', SpeakState('Obstacle in front of me'))
# Sequence.add('ObstacleDelay', DelayState(10),
# transitions={'succeeded': 'GoToWaypoin2'})
# Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker'))
Sequence.add('Train_human', FollowTrainState())
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
# Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'})
Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three'))
Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'})
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map={'key_points': {
'MONITOR_AR': 'invalid'
}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add(
'MONITOR_AR',
MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'key_points': 'PATROL_KEYPOINTS',
'stop': 'STOP'
})
StateMachine.add('PATROL_KEYPOINTS',
PatrolThroughKeyPoints(
self.keyPointManager).getSM(),
transitions={'succeeded': 'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
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 __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 main():
rospy.init_node('tinker_mission_follow')
rospy.loginfo(colored('starting follow and guide task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button',
MonitorStartButtonState(),
transitions={
'valid': 'Start_Button',
'invalid': '1_Start'
})
StateMachine.add('1_Start',
MonitorKinectBodyState(),
transitions={
'valid': '1_Start',
'invalid': 'Sequence'
})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
follow_concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
Concurrence.add('KeyWordsRecognition',
KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
StateMachine.add('Sequence', sequence, {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state,
'/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
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')
def make_nav_sm(nav_state, tennis_topic):
"""
Make a NAV<x> state.
:param nav_state: the state
nav_state takes in ud called goal_position and waypoints
it has two outcomes: goal, fail, preempted
preempt is called when tennis is found, make sure to implement it
goal_position is a pose (for added laziness in move_base)
:return: the state machine (actually a Concurrence but it doesn't matter)
"""
def i_dont_care_callback(_):
return True # whichever finishes first! (although tennis never finishes unless preempt...)
# noinspection PyTypeChecker
sm = Concurrence(outcomes=["tennis", "goal", "fail", "preempted"],
default_outcome="fail",
input_keys=["goal_position", "waypoints"],
output_keys=["tennis_position"],
outcome_map={
"tennis": {
"TENNIS_MONITOR": "tennis"
},
"fail": {
"NAV": "fail",
"TENNIS_MONITOR": "fail"
},
"goal": {
"NAV": "goal"
},
"preempted": {
"NAV": "preempted",
"TENNIS_MONITOR": "fail"
}
},
child_termination_cb=i_dont_care_callback)
with sm:
Concurrence.add("TENNIS_MONITOR",
TennisBallMonitoringState(tennis_topic),
remapping={"tennis_position": "tennis_position"})
Concurrence.add("NAV", nav_state)
return sm
def main():
rospy.init_node('smach_usecase_executive')
sm_root = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_root:
smach.StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')
smach.StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn, request),
{'succeeded':'TELEPORT1'})
teleport1 = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)
smach.StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, teleport1),
{'succeeded':'TELEPORT2'})
teleport2 = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)
smach.StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, teleport2),
{'succeeded':'SHAPES'})
shapes_concurrence = Concurrence(
outcomes=['succeeded','aborted','preempted'],
default_outcome='aborted',
outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
smach.StateMachine.add('SHAPES', shapes_concurrence)
with shapes_concurrence:
Concurrence.add('BIG',
SimpleActionState('turtle_shape1', ShapeAction, ShapeGoal(11,4.0)),
{'succeeded':'SMALL'})
Concurrence.add('SMALL',
SimpleActionState('turtle_shape2', ShapeAction, ShapeGoal(6,0.5)))
sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node("follow")
rospy.on_shutdown(self.shutdown)
self.state = None
self.sm_follow = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
# 总状态机follow
with self.sm_follow:
self.co_follow = Concurrence(
outcomes=["succeeded", "preempted", "aborted"],
default_outcome="succeeded",
# outcome_cb = self.co_follow_outcome_cb ,
# child_termination_cb=self.co_follow_child_cb
)
with self.co_follow:
Concurrence.add("mo_L", MonitorState("people_position_estimation", PositionMeasurement, self.pos_M_cb))
Concurrence.add("nav", Nav2Waypoint())
# self.sm_nav=StateMachine(outcomes=('succeeded','aborted','preempted'))
# with self.sm_nav:
# # StateMachine.add('wait',MonitorState('sr_data',Int16,self.wait_cb),transitions={'valid':'wait','invalid':'nav_speech','preempted':'nav_speech'})
#
# self.speech_nav=Concurrence(outcomes=['get_in','succeeded'],
# default_outcome='succeeded',
# outcome_cb=self.speech_nav_outcome_cb,
# child_termination_cb=self.speech_nav_child_termination_cb
# )
# with self.speech_nav:
# Concurrence.add('speech',MonitorState('sr_data',Int16,self.nav_speech_cb))
# Concurrence.add('nav', Nav2Waypoint())
# StateMachine.add('nav_speech',self.speech_nav,transitions={'get_in':'Get_in',})
# self.get_in_speech=Concurrence(outcomes=['get_out','succeeded'],
# default_outcome='succeeded',
# outcome_cb=self.speech_get_in_outcome_cb,
# child_termination_cb=self.speech_get_in_child_termination_cb
#
# )
# with self.get_in_speech:
# Concurrence.add('speech',MonitorState('sr_data',Int16,self.get_in_speech_cb))
# Concurrence.add('get_in',Get_in())
# StateMachine.add('Get_in',self.get_in_speech,transitions={'get_out':'Get_out'})
# StateMachine.add('Get_out',Get_out(), transitions={'succeeded':'nav_speech','preempted':'','aborted':'Get_out' })
# Concurrence.add('Nav',self.sm_nav)
StateMachine.add("Nav_top", self.co_follow)
a = self.sm_follow.execute()
def build_concurrence_container(outcomes, default_outcome, outcome_map,
concurrent_states):
"""Create & returns a SMACH Concurrence instance from a dictionary of SMACH states
Args:
outcomes (list of str)
default_outcome (str)
outcome_map (dict of dict)
concurrent_states (dict of class instances)
Returns:
dict: A dict with instance names as keys and class instances as values
"""
concurrence = Concurrence(outcomes=outcomes,
default_outcome=default_outcome,
outcome_map=outcome_map)
with concurrence:
for name, instance in concurrent_states.items():
Concurrence.add(name.upper(), instance)
return concurrence
def __init__(self, distToHuman=0.9, state_machine_name="restaurant"):
smach.Concurrence.__init__(
self,
outcomes=[succeeded, preempted, aborted],
default_outcome=succeeded,
child_termination_cb=child_term_cb,
outcome_cb=out_cb,
input_keys=["in_learn_person"])
rospy.set_param("/params_learn_and_follow_operator_test/distance_to_human", distToHuman)
with self:
setDebugLevel(0)
Concurrence.add('FOLLOW_OPERATOR',
FollowOperator(distToHuman),
remapping={"in_learn_person": "in_learn_person"})
Concurrence.add('LISTEN_COMMANDS',
ListenCommands(state_machine_name))
def build_ball_timeout_state(grabber_frame, distance_limit, timeout):
sm = Concurrence(outcomes=["succeeded", "timed_out"],
default_outcome="timed_out",
output_keys=["ball_location"],
outcome_map={
"succeeded": {
"CLOSEST_BALL": "succeeded"
},
"timed_out": {
"TIMEOUT": "succeeded"
}
},
child_termination_cb=lambda x: True)
with sm:
Concurrence.add("CLOSEST_BALL",
build_ball_retry_state(grabber_frame, distance_limit),
remapping={"position": "ball_location"})
Concurrence.add("TIMEOUT", Delay(timeout))
return sm
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = StateMachine(outcomes=['outcome6'])
# Open the countainer
with sm_top:
StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'})
# Create the sub SMACH state machine
sm_con = Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'FOO': 'outcome2',
'BAR': 'outcome1'
}})
# Open the countainer
with sm_con:
# Add states to the container
Concurrence.add('FOO', Foo())
Concurrence.add('BAR', Bar())
StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
sis = IntrospectionServer('example', sm_top, '/SM_PATH')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def test_concurrence(self):
"""Test concurrent container."""
sm = StateMachine(['done', 'succeeded'])
with sm:
cc = Concurrence(['succeeded', 'done'],
default_outcome='done',
outcome_map={'succeeded': {
'SETTER': 'done'
}})
sm.add('CONCURRENT', cc)
with cc:
Concurrence.add('SETTER', Setter())
Concurrence.add('GETTER', Getter())
outcome = sm.execute()
assert outcome == 'succeeded'
assert 'a' in cc.userdata
assert 'b' in cc.userdata
assert cc.userdata.a == 'A'
assert cc.userdata.b == 'A'
def __init__(self, msg_pool, arg_key='tell_argument'):
input_keys = ['in_location_pose_in_map', 'in_target_object']
if arg_key:
input_keys.append(arg_key)
Concurrence.__init__(self, outcomes=[succeeded, aborted, 'move_failed', 'no_object_found'],
default_outcome=aborted,
input_keys=input_keys,
output_keys=['out_objects_data'],
outcome_map={succeeded: {'TELL_GOING_TO_SEARCH': succeeded,
'MOVE_AND_RECOGNIZE': succeeded},
'no_object_found': {'MOVE_AND_RECOGNIZE': 'no_object_found'},
'move_failed': {'MOVE_AND_RECOGNIZE': 'move_failed'}})
if type(msg_pool) is str:
msg_pool = [msg_pool]
if arg_key is None:
arg_key = 'tell_argument'
self.userdata.tell_argument = None # No argument set!
# Concurrence container to speak and search simultanously
with self:
Concurrence.add('TELL_GOING_TO_SEARCH', SpeakActionFromPoolStateMachine(msg_pool, arg_key=arg_key),
remapping={arg_key: arg_key})
Concurrence.add('MOVE_AND_RECOGNIZE', GoAndRecognizeSM(),
remapping={'in_target_object': 'in_target_object',
'in_location_pose_in_map': 'in_location_pose_in_map',
'out_objects_data': 'out_objects_data'})
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('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 __init__(self,
behavior_pool=None,
textpool=None,
wait_before_speak=None):
input_keys = []
if not textpool:
input_keys = ['text']
use_bpool = True
if not isinstance(behavior_pool, list):
use_bpool = False
Concurrence.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=input_keys,
default_outcome='aborted',
outcome_map={
'succeeded': {
'GESTURE_SPEECH': 'succeeded',
'GESTURE_MOVE': 'succeeded'
}
})
with self:
Concurrence.add(
'GESTURE_MOVE',
ExecuteBehavior(behavior_pool)
if not use_bpool else ExecuteBehaviorFromPoolSM(behavior_pool))
Concurrence.add(
'GESTURE_SPEECH',
SpeechState(wait_before_speak=wait_before_speak,
text=textpool,
blocking=True) if not textpool else
SpeechFromPoolSM(wait_before_speak=wait_before_speak,
pool=textpool,
blocking=True))
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 allign_with_target(target):
"""
Returns a state that alligns with the specified target.
target: string on the form 'GATE' or 'BOUY'
The returned state is responsible for chaning the circeling direction when needed.
"""
# TODO: get target position (x,y) from landmark server
# TODO: create a move_goal
move_goal = None
allignment_attempt = Concurrence(
outcomes=['succeeded', 'preempted', 'wrong_direction'],
outcome_map={
'succeeded': {
'ALLIGNMENT_CHECKER': 'alligned'
},
'wrong_direction': {
'ALLIGNMENT_CHECKER': 'wrong_direction'
}
},
default_outcome=['preempted'],
child_termination_cb=None # TODO: should allways terminate
)
with allignment_attempt:
Concurrence.add(
'CIRCLE_GATE',
SimpleActionState('controller/move', MoveAction,
goal=move_goal) # TODO
)
Concurrence.add('ALLIGNMENT_CHECKER', CBState(allignment_checker))
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map = {'key_points' : {'MONITOR_AR':'invalid'}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add('MONITOR_AR',MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'key_points':'PATROL_KEYPOINTS', 'stop':'STOP'})
StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints(self.keyPointManager).getSM(), transitions={'succeeded':'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
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 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 __init__(self):
Concurrence.__init__(
self,
outcomes=['exit', 'restart', 'aborted', 'preempted'],
default_outcome='restart',
output_keys=['control_infos'],
child_termination_cb=self.child_termination_cb,
outcome_cb=self.outcome_cb)
self.userdata.control_infos = {}
with self:
Concurrence.add(
'CHANGE_CONTROL_MONITOR',
MonitorState("/change_control",
String,
self.change_control_cb,
input_keys=['control_infos'],
output_keys=['control_infos']))
Concurrence.add('TIMER', Timer())
Concurrence.add('CONTROL', Control())
def __init__(self):
Concurrence.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['log_mission'],
output_keys=['log_mission'],
child_termination_cb=self.child_termination_cb,
outcome_map={'succeeded': {
'WAITING': 'finished'
}})
self.register_start_cb(self.start_pause)
self.register_termination_cb(self.termination_pause)
_min_time_to_go_recharge = 15.0
@cb_interface(input_keys=['log_mission'],
outcomes=['insufficient_time', 'recharge', 'preempted'])
def check_waiting_time(ud):
navigation_duration = datetime.datetime.now(
) - ud.log_mission['start_time']
if datetime.timedelta(seconds=ud.log_mission['patrol']['min_duration']) - navigation_duration \
> datetime.timedelta(seconds=_min_time_to_go_recharge):
return 'recharge'
else:
return 'succeeded'
self.recharge = StateMachine(
input_keys=['log_mission'],
output_keys=['log_mission'],
outcomes=['succeeded', 'aborted', 'preempted'])
with self.recharge:
StateMachine.add('CHECK_WAITING_TIME', CBState(check_waiting_time),
{
'recharge': 'RECHARGE',
'insufficient_time': 'succeeded'
})
StateMachine.add('RECHARGE', Recharge())
with self:
Concurrence.add('WAITING', Waiting())
Concurrence.add('CONDITIONAL_RECHARGE', self.recharge)
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'
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 __init__(self):
rospy.init_node("navgation_test")
rospy.on_shutdown(self.shutdown)
self.waypoints=[];
Location= (Point(3.099,-1.796,0),
Point(6.732,-3.260,0),
Point(7.058,-0.119,0),
Point(-0.595,0.069,0),)
quaternions=[];
euler_angles=[0.039,0.056,-3.093,-3.133];
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
for i in range(4):
self.waypoints.append(Pose(Location[i], quaternions[i]))
point_locations = (('Point1', self.waypoints[0]),
('Point2', self.waypoints[1]),
('Point3', self.waypoints[2]),
('Point4', self.waypoints[3]))
self.room_locations = OrderedDict(point_locations)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
rospy.loginfo("Connected to move_base action server")
nav_states={}
self.people_in_sight=0
# Create a navigation task for each room
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.header.stamp = rospy.Time.now()
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(100.0),
server_wait_timeout=rospy.Duration(100.0))
nav_states[room] = move_base_state
sm_nav1= StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with sm_nav1:
StateMachine.add('START',MonitorState("task_comming", xm_Task, self.start_cb),
transitions={'invalid':'WAIT4DOOR_OPEN',
'valid':'START',
'preempted':'WAIT4DOOR_OPEN'})
StateMachine.add('WAIT4DOOR_OPEN',MonitorState("doormsg",door_msg,self.door_is_open_cb),
transitions={'invalid':'INIT',
'valid':'WAIT4DOOR_OPEN',
'preempted':'INIT'})
StateMachine.add("INIT", ServiceState("pan_srv", xm_Pan,True, response_cb=self.init_cb),transitions={"succeeded":"NAV_1","aborted":"NAV_1"})
StateMachine.add("NAV_1",nav_states['Point1'],transitions={'succeeded':'','aborted':'','preempted':''})
sm_nav2=StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_nav2:
StateMachine.add("NAV_2",nav_states['Point2'],transitions={'succeeded':'','aborted':'Check_People','preempted':''})
StateMachine.add("Check_People",MonitorState("tracker/tracks_smoothed",TrackArray,self.check_people_cb),
transitions={'valid':'WAIT',
'invalid':'WAIT',
'preempted':'WAIT'})
StateMachine.add('WAIT',wait(time=5,people_pass=self.people_in_sight),
transitions={'succeeded':'NAV_2_END'})
StateMachine.add("NAV_2_END",nav_states['Point2'],transitions={'succeeded':'','aborted':'','preempted':''})
sm_nav3=StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_nav3:
# StateMachine.add("NAV_3",nav_states['Point3'],transitions={'succeeded':'WAIT_4_CMD','aborted':'NAV_3','preempted':'NAV_3'})
sm_follow=Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
with sm_follow:
Concurrence.add('START_FOLLOW',MonitorState("task_comming", xm_Task, self.follow_stop_cb))
co_follow=Concurrence(outcomes=['succeeded','aborted'],
default_outcome='succeeded',
child_termination_cb=self.co_follow_cb,
outcome_cb=self.co_follow_outcome_cb)
with co_follow:
Concurrence.add('mo_L',MonitorState('people_tf_position',xm_Tracker,
self.pos_M_cb))
Concurrence.add('nav', Nav2Waypoint())
Concurrence.add('FOLLOW',co_follow)
StateMachine.add('WAIT_4_CMD',MonitorState("task_comming", xm_Task, self.follow_start_cb),
transitions={'invalid':'FOLLOW_MODE',
'valid':'FOLLOW_MODE',
'preempted':'FOLLOW_MODE'})
StateMachine.add("FOLLOW_MODE",sm_follow,transitions={'succeeded':'','aborted':''})
sm_nav4=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with sm_nav4:
StateMachine.add("WAIT_4_BACK",MonitorState("task_comming",xm_Task,self.cmd_back_cb),transitions={"invalid":"NAV_4","valid":"WAIT_4_BACK","preempted":""})
StateMachine.add("NAV_4",nav_states['Point4'],transitions={'succeeded':'','aborted':'','preempted':''})
sm_nav_test=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with sm_nav_test:
# StateMachine.add("NAV_1",sm_nav1,transitions={'succeeded':'NAV_2','aborted':'NAV_2','preempted':'NAV_2'})
# StateMachine.add("NAV_2",sm_nav2,transitions={'succeeded':'NAV_3','aborted':'NAV_3','preempted':'NAV_3'})
StateMachine.add("NAV_3",sm_nav3,transitions={'succeeded':'NAV_4','aborted':'NAV_4','preempted':'NAV_4'})
StateMachine.add("NAV_4",sm_nav4,transitions={'succeeded':'','aborted':'','preempted':''})
sm_outcome = sm_nav_test.execute()
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 __init__(self):
rospy.init_node("deliver_food", anonymous=False)
self.initialize_destination()
self.lalala = 100
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = "map"
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState(
"move_base",
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0),
)
nav_states[room] = move_base_state
rospy.loginfo(
room
+ " -> ["
+ str(round(self.room_locations[room].position.x, 2))
+ ", "
+ str(round(self.room_locations[room].position.y, 2))
+ "]"
)
sm_rotate_search = Concurrence(
outcomes=["find", "not_find"],
default_outcome="not_find",
child_termination_cb=self.concurrence_child_termination_callback,
outcome_cb=self.concurrence_outcome_callback,
)
with sm_rotate_search:
Concurrence.add("ROTATE", Rotate360(0.4, 2 * pi))
Concurrence.add("SEARCH", SearchTable())
sm_table1 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table1:
StateMachine.add(
"GOTO_TABLE1",
nav_states["table1"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table2 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table2:
StateMachine.add(
"GOTO_TABLE2",
nav_states["table2"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table3 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table3:
StateMachine.add(
"GOTO_TABLE3",
nav_states["table3"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a"}
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2z", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2z",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE3_2",
nav_states["table3"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE3_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE3_3",
nav_states["table3"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
sm_table4 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table4:
StateMachine.add(
"GOTO_TABLE4",
nav_states["table4"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE4_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE4_2",
nav_states["table4"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.4, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE4_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE4_3",
nav_states["table4"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.4, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
sm_table5 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table5:
StateMachine.add(
"GOTO_TABLE5",
nav_states["table5"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table6 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table6:
StateMachine.add(
"GOTO_TABLE6",
nav_states["table6"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE6_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE6_2",
nav_states["table6"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE6_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE6_3",
nav_states["table6"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
# let's initialize the overall state machine
sm_deliverfood = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_deliverfood:
StateMachine.add("STARTING_TASK", Welcome(), transitions={"succeeded": "COMPUTER_VISION_TASK"})
StateMachine.add(
"COMPUTER_VISION_TASK",
ComputerVision(),
transitions={
"detect1": "TABLE_ONE_TASK",
"detect2": "TABLE_TWO_TASK",
"detect3": "TABLE_THREE_TASK",
"detect4": "TABLE_FOUR_TASK",
"detect5": "TABLE_FIVE_TASK",
"detect6": "TABLE_SIX_TASK",
"preempted": "",
},
)
StateMachine.add(
"TABLE_ONE_TASK",
sm_table1,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_TWO_TASK",
sm_table2,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_THREE_TASK",
sm_table3,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_FOUR_TASK",
sm_table4,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_FIVE_TASK",
sm_table5,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_SIX_TASK",
sm_table6,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer("deliver_food", sm_deliverfood, "/SM_ROOT")
intro_server.start()
# Execute the state machine
sm_outcome = sm_deliverfood.execute()
rospy.on_shutdown(self.shutdown)
def __init__(self):
rospy.init_node('petit_smach_ai', 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]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Action1
self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action1.userdata.mandibles_sleep = 0.1
with self.sm_action1:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPCUBE_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(),
# transitions={'succeeded':'CLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action2
self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action2.userdata.speed = -0.1;
self.sm_action2.userdata.distance = 0.3;
self.sm_action2.userdata.mandibles_sleep = 0.1
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'OPEN_MANDIBLES',
'aborted':'aborted'})
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'MOVE_BACKWARD',
'aborted':'aborted'})
StateMachine.add('MOVE_BACKWARD', MoveForward(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action3
self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action3.userdata.mandibles_sleep = 0.1
self.sm_action3.userdata.speed = -0.1;
self.sm_action3.userdata.distance = 0.2;
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'MOVE_PUSH',
'aborted':'aborted'})
StateMachine.add('MOVE_PUSH', MovePush(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action4
self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action4.userdata.mandibles_sleep = 0.1
self.sm_action4.userdata.speed_x = 0.1
self.sm_action4.userdata.speed_y = 0.1
self.sm_action4.userdata.distance = 0.5;
with self.sm_action4:
StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'SLIDE',
'aborted':'aborted'})
StateMachine.add('SLIDE', Slide(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action5
self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action5.userdata.mandibles_sleep = 0.1
with self.sm_action5:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
'aborted':'ALMOSTCLOSE_MANDIBLES'})
StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action6
self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action6.userdata.mandibles_sleep = 0.1
self.sm_action6.userdata.speed = 0.1;
self.sm_action6.userdata.distance = 0.2;
with self.sm_action6:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'FORWARD',
'aborted':'aborted'})
StateMachine.add('FORWARD', MoveForward(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'HCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action7
self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action7.userdata.mandibles_sleep = 0.1
with self.sm_action7:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
self.sm_actions.userdata.waypoints = self.waypoints
with self.sm_actions:
StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb,
output_keys=['waypoint_out'],
outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']),
transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'},
remapping={'waypoint_out':'patrol_waypoint'})
#StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
# remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'},
remapping={'waypoint_in':'remove_waypoint'})
# State machine with concurrence
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_ACTIONS', self.sm_actions)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# 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:
# @smach.cb_interface()
# def requestPrioCube_cb(userdata, request):
# update_request = UpdatePriority().Request
# update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
# update_request.goal.pose.position.y = 0.800
# update_request.prio = 100
# return update_request
StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'})
StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'})
StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# 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 __init__(self, target_object_key=None, target_frame='/map', tsp_route=TSP_route):
input_keys = ['in_room_name']
if target_object_key is not None:
input_keys.append(target_object_key)
smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
input_keys=input_keys,
output_keys=['out_object_found', 'out_location_inside_room_name'])
if target_object_key is None:
target_object_key = 'in_target_object'
self.userdata.in_target_object = '' # No target object set! Before set to str(None), changed for SearchObjectWCSM
going_to_pool = ["I'm going to the %s to look for objects.", "Maybe I find objects on the %s. I'll have a look.",
"Why don't I search objects on the %s?", "I think there can be objects on the %s."]
object_not_found_pool = ["I can't see any object on the %s.", "It seems that there aren't objects on the %s.",
"I haven't found any object on the %s."]
else:
going_to_pool = ["I'm going to the %s to look if it's there.", "Maybe I find it on the %s. I'll have a look.",
"Why don't I search it on the %s?", "I think it can be on the %s."]
object_not_found_pool = ["I can't see it on the %s.", "It seems that it isn't on the %s.",
"I haven't found it on the %s."]
object_found_pool = ["I can see the %s on the %s.", "Is that the %s on the %s? I think so!",
"The %s is just there on the %s.", "I found the %s on the %s.",
"Can't you see the %s? Is there, on the %s!"]
self.userdata.objects_data = None # To avoid errors the first time the check_if_remaining is called
self.userdata.sensing_route = None # Because at the very beginning there's no route
self.userdata.furniture_route = None # Because at the very beginning there's no route
self.userdata.out_object_found = None # To avoid errors if there's an abort.
with self:
StateMachine.add('CHECK_IF_OBJECTS_REMAINING', CheckRemaining(),
remapping={'in_obj_list': 'objects_data'},
transitions={'empty': 'CHECK_IF_FURNITURE_ROUTE_EMPTY',
'remaining': 'TELL_GO_RECOGNIZE'})
@smach.cb_interface(input_keys=['in_route', 'in_furniture_poses', 'in_orientations', 'in_furniture_name_list'],
output_keys=['out_remaining_poses', 'out_remaining_orientations', 'out_remaining_names'],
outcomes=['remaining', 'empty'])
def check_furniture_route_emptiness(userdata):
if userdata.in_route:
# We get the remaining poses and orientations of the furniture route
# and then we'll recalculate the route through that remaining nodes
remaining_poses = []
remaining_orientations = []
remaining_names = []
for index in userdata.in_route: # furniture route has only the indices of the elements
remaining_poses.append(userdata.in_furniture_poses[index])
remaining_orientations.append(userdata.in_orientations[index])
remaining_names.append(userdata.in_furniture_name_list[index])
userdata.out_remaining_poses = remaining_poses
userdata.out_remaining_orientations = remaining_orientations
userdata.out_remaining_names = remaining_names
return 'remaining'
return 'empty'
StateMachine.add('CHECK_IF_FURNITURE_ROUTE_EMPTY',
CBState(check_furniture_route_emptiness,
input_keys=['in_route', 'in_furniture_poses', 'in_orientations', 'in_furniture_name_list'],
output_keys=['out_remaining_poses', 'out_remaining_orientations', 'out_remaining_names'],
outcomes=[succeeded, 'empty']),
remapping={'in_route': 'furniture_route',
'in_furniture_poses': 'furniture_detect_poses',
'in_orientations': 'furniture_orientation_data',
'out_remaining_poses': 'furniture_detect_poses',
'out_remaining_orientations': 'furniture_orientation_data',
'in_furniture_name_list': 'furniture_name_list',
'out_remaining_names': 'furniture_name_list'},
transitions={'remaining': 'FURNITURE_TSP_ROUTE',
'empty': 'CHECK_IF_SENSING_ROUTE_EMPTY'})
@smach.cb_interface(input_keys=['in_route'], output_keys=['out_remaining_nodes'],
outcomes=['remaining', 'empty'])
def check_sensing_route_emptiness(userdata):
if userdata.in_route:
# We get the remaining nodes of the route
# and then we'll recalculate the route through that remaining nodes
userdata.out_remaining_nodes = userdata.in_route # The route itself has the nodes ordered.
return 'remaining'
return 'empty'
StateMachine.add('CHECK_IF_SENSING_ROUTE_EMPTY',
CBState(check_sensing_route_emptiness,
input_keys=['in_route'],
output_keys=['out_remaining_nodes'],
outcomes=[succeeded, 'empty']),
remapping={'in_route': 'sensing_route',
'out_remaining_nodes': 'sens_locations'},
transitions={'remaining': 'SENSING_LOCATIONS_TSP_ROUTE',
'empty': 'CALCULATE_FURNITURE_SENSING_LOCATIONS'})
StateMachine.add('CALCULATE_FURNITURE_SENSING_LOCATIONS', CalculateSensingLocations(),
remapping={'in_room_name': 'in_room_name', 'out_guards': 'sens_locations'},
transitions={succeeded: 'SENSING_LOCATIONS_TSP_ROUTE', 'no_corner_info': aborted})
StateMachine.add('SENSING_LOCATIONS_TSP_ROUTE', TSPState(HC=HC),
remapping={'in_nodes': 'sens_locations', 'out_route': 'sensing_route'},
transitions={succeeded: 'GET_NEXT_SENSING_LOCATION'})
@smach.cb_interface(input_keys=['in_sensing_route'], output_keys=['out_sensing_route', 'out_next_sensing_pose'],
outcomes=[succeeded, 'empty_route'])
def get_next_loc_from_route(userdata):
# Returns the next sensing location to go and removes it from the route
if not userdata.in_sensing_route: # The sensing route is empty...
return 'empty_route'
next_pos = userdata.in_sensing_route[0]
userdata.out_sensing_route = userdata.in_sensing_route[1:]
pose = Pose()
pose.position.x = next_pos[0]
pose.position.y = next_pos[1]
pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.57)) # FIXME is it possible to avoid a fixed orientation?
userdata.out_next_sensing_pose = pose
return succeeded
StateMachine.add('GET_NEXT_SENSING_LOCATION',
CBState(get_next_loc_from_route,
input_keys=['in_sensing_route'],
output_keys=['out_sensing_route', 'out_next_sensing_pose'],
outcomes=[succeeded, 'empty_route']),
remapping={'out_sensing_route': 'sensing_route',
'in_sensing_route': 'sensing_route',
'out_next_sensing_pose': 'next_sensing_pose'},
transitions={succeeded: 'MOVE_TO_SENSING_LOCATION',
'empty_route': 'CALCULATE_FURNITURE_SENSING_LOCATIONS'})
StateMachine.add('MOVE_TO_SENSING_LOCATION',
MoveActionState("/map", goal_key='next_sensing_pose'),
transitions={succeeded: 'DETECT_FURNITURE_AND_ANNOUNCE',
aborted: 'GET_NEXT_SENSING_LOCATION'}) # FIXME if aborted we get the next pose.
tell_and_search = Concurrence(outcomes=[succeeded, aborted, preempted, 'no_furniture'],
default_outcome=aborted,
output_keys=['furniture_detect_poses',
'furniture_orientation_data', 'furniture_name_list'],
outcome_map={succeeded: {'ANNOUNCE_SEARCHING': succeeded,
'DETECT_FURNITURE_ZONE': succeeded},
'no_furniture': {'DETECT_FURNITURE_ZONE': 'no_furniture'}})
with tell_and_search:
searching_furniture_pool = ["I'm looking for places that can contain objects.",
"I'm searching a place that can have objects.",
"I'm searching the best place to find objects."]
Concurrence.add('ANNOUNCE_SEARCHING',
SpeakActionFromPoolStateMachine(searching_furniture_pool))
Concurrence.add('DETECT_FURNITURE_ZONE',
DetectFurnitureOfZoneSM(distance_treshold=DIST_BETWEEN_FURNITURE, dist_to_furniture=DIST_TO_FURNITURE),
remapping={'out_furniture_pose_list': 'furniture_detect_poses',
'out_furniture_orientation_list': 'furniture_orientation_data',
'out_furniture_name_list': 'furniture_name_list'})
transition_route = 'FURNITURE_TSP_ROUTE' if tsp_route else 'FURNITURE_PROBABILITY_ROUTE'
StateMachine.add('DETECT_FURNITURE_AND_ANNOUNCE', tell_and_search,
transitions={succeeded: transition_route, 'no_furniture': 'GET_NEXT_SENSING_LOCATION'},
remapping={'furniture_detect_poses': 'furniture_detect_poses',
'furniture_name_list': 'furniture_name_list',
'furniture_orientation_data': 'furniture_orientation_data'})
#FIXME filter poses and everything by probability or something like that?
# @smach.cb_interface(input_keys=['in_furniture_detect_data'], output_keys=['out_furniture_node_list'],
# outcomes=[succeeded])
# def check_furniture_data(userdata):
# #FIXME -> May be necessary to filter in some way the already visited furniture...
# #furniture_detect_data is a list of tuples (pos, orientation), so we get only the poses for the TSP
# userdata.out_furniture_node_list = reduce(lambda acc, x: acc + [x[0]], userdata.in_furniture_detect_data, [])
# return succeeded
# StateMachine.add('CHECK_FURNITURE_DATA',
# CBState(check_furniture_data, input_keys=['in_furniture_detect_data'],
# output_keys=['out_object_detection_pose'],
# outcomes=[succeeded]),
# remapping={'in_furniture_detect_data': 'furniture_detect_data',
# 'out_furniture_node_list': 'furniture_node_list'},
# transitions={succeeded: 'TELL_GO_RECOGNIZE'})
StateMachine.add('FURNITURE_TSP_ROUTE', TSPState(HC=HC, indices=True),
remapping={'in_nodes': 'furniture_detect_poses',
'out_route': 'furniture_route'},
transitions={succeeded: 'GET_NEXT_FURNITURE_LOCATION'})
@smach.cb_interface(input_keys=['in_nodes', 'in_furniture_name_list'],
output_keys=['out_route', 'out_furniture_name_list'],
outcomes=[succeeded])
def create_probability_route(userdata):
furn_prob_map = rospy.get_param(FURN_PROB_PARAM)
n = len(userdata.in_nodes)
route = range(n)
# Get a list of tuples (probability, index_of_name)
prob_index = zip(reduce(lambda acc, x: acc + [furn_prob_map[x]], userdata.in_furniture_name_list, []), route)
prob_index.sort(key=lambda x: x[0], reverse=True) # Sort by probability
robot_pose = Pose()
robot_pose.position.x = 0
robot_pose.position.y = 0
robot_pose.position.z = 0
# Get robot's pose in /map coordinates
robot_pose = transform_pose(robot_pose, '/base_link', '/map', timeout=3)
actual = (robot_pose.position.x, robot_pose.position.y)
route[0] = prob_index[0][1]
for i in xrange(1, n):
route[i] = prob_index[i][1]
if prob_index[i][0] == 0: # If probability is 0 we don't look at it.
del route[i]
del userdata.in_furniture_name_list[i]
elif prob_index[i][0] == prob_index[i-1][0]: # If two things have the same probability, get the closer one
di_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i]], actual)
di1_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i-1]], actual)
if di_r < di1_r: # The index at i is nearer than the one at i-1
route[i] = route[i-1]
route[i-1] = prob_index[i][1]
userdata.out_route = route
userdata.out_furniture_list = userdata.in_furniture_name_list
return succeeded
StateMachine.add('FURNITURE_PROBABILITY_ROUTE',
CBState(create_probability_route,
input_keys=['in_nodes', 'in_furniture_name_list'],
output_keys=['out_route', 'out_furniture_name_list'],
outcomes=[succeeded]),
remapping={'in_nodes': 'furniture_detect_poses',
'in_furniture_name_list': 'furniture_name_list',
'out_furniture_name_list': 'furniture_name_list'
'out_route': 'furniture_route'},
transitions={succeeded: 'GET_NEXT_FURNITURE_LOCATION'})
@smach.cb_interface(input_keys=['in_furniture_route', 'in_orient_list', 'in_pose_list', 'in_furniture_name_list'],
output_keys=['out_furniture_route', 'out_object_detection_pose', 'out_furniture_name'],
outcomes=[succeeded, 'empty_route'])
def get_next_loc_from_furniture_route(userdata):
# Returns the next furniture location to go and removes it from the route
if not userdata.in_furniture_route: # The route is empty, so we recalculate the sensing route and go again
return 'empty_route'
node_index = userdata.in_furniture_route[0]
print('node_index: %d, length in_furniture_route: %d' % (node_index, len(userdata.in_furniture_route)))
next_pos = userdata.in_pose_list[node_index]
# Only the index is removed from the list, to preserve the index order of the other elements
userdata.out_furniture_route = userdata.in_furniture_route[1:]
pose = Pose()
pose.position.x = next_pos[0]
pose.position.y = next_pos[1]
pose.orientation = userdata.in_orient_list[node_index]
userdata.out_object_detection_pose = pose
userdata.out_furniture_name = userdata.in_furniture_name_list[node_index]
return succeeded
StateMachine.add('GET_NEXT_FURNITURE_LOCATION',
CBState(get_next_loc_from_furniture_route,
input_keys=['in_furniture_route', 'in_orient_list', 'in_pose_list'],
output_keys=['out_furniture_route', 'out_object_detection_pose', 'out_furniture_name'],
outcomes=[succeeded, 'empty_route']),
remapping={'out_furniture_route': 'furniture_route',
'in_furniture_route': 'furniture_route',
'in_orient_list': 'furniture_orientation_data',
'in_pose_list': 'furniture_detect_poses',
'in_furniture_name_list': 'furniture_name_list',
'out_object_detection_pose': 'object_detection_pose',
'out_furniture_name': 'out_location_inside_room_name'},
transitions={succeeded: 'TELL_GO_RECOGNIZE',
'empty_route': 'SENSING_LOCATIONS_TSP_ROUTE'})
StateMachine.add('TELL_GO_RECOGNIZE', TellGoRecognizeSM(msg_pool=going_to_pool, arg_key='location_name'),
remapping={'location_name': 'out_location_inside_room_name',
'out_objects_data': 'objects_data',
'in_target_object': target_object_key,
'in_location_pose_in_map': 'object_detection_pose'},
transitions={'move_failed': 'GET_NEXT_SENSING_LOCATION',
'no_object_found': 'CHECK_IF_OBJECT_FOUND',
succeeded: 'CHECK_IF_OBJECT_FOUND',
aborted: aborted}) # FIXME aborted should abort everything?
StateMachine.add('CHECK_IF_OBJECT_FOUND',
CheckObjectAndRemoveFromList(target_frame),
remapping={'out_object_found': 'out_object_found',
'in_objects_data': 'objects_data',
'out_objects_data': 'objects_data',
'in_target_object': target_object_key},
transitions={succeeded: 'PREPARE_POOL_ARGS', aborted: 'TELL_NO_OBJECT_FOUND'})
StateMachine.add('TELL_NO_OBJECT_FOUND',
SpeakActionFromPoolStateMachine(object_not_found_pool, arg_key='location_name'),
remapping={'location_name': 'out_location_inside_room_name'},
transitions={succeeded: 'GET_NEXT_SENSING_LOCATION', aborted: 'GET_NEXT_SENSING_LOCATION'})
@smach.cb_interface(input_keys=['in_first_object', 'in_location_name'], output_keys=['out_tell_arg'],
outcomes=[succeeded])
def prepare_userdata(userdata):
userdata.out_tell_arg = (userdata.in_first_object.name, userdata.in_location_name)
return succeeded
StateMachine.add('PREPARE_POOL_ARGS',
CBState(prepare_userdata,
input_keys=['in_first_object', 'in_location_name'],
output_keys=['out_tell_arg'], outcomes=[succeeded]),
remapping={'in_first_object': 'out_object_found',
'out_tell_arg': 'tell_arg',
'in_location_name': 'out_location_inside_room_name'},
transitions={succeeded: 'TELL_OBJECT_RECOGNIZED'})
StateMachine.add('TELL_OBJECT_RECOGNIZED',
SpeakActionFromPoolStateMachine(object_found_pool, arg_key="tell_arg"),
remapping={'tell_arg': 'tell_arg'},
transitions={succeeded: succeeded, aborted: aborted})
def __init__(self):
rospy.init_node('whoiswho_nopick')
rospy.logwarn('start who is who test')
self.smach_bool =False
rospy.on_shutdown(self.shutdown)
# subprocess.call("xterm -e rosrun xm_speech xm_speech_client.py &", shell = True)
# add waypoints into the list
self.waypoints=[]
location= (Point(1.003, -0.217, 0.000), #找人数的点
Point( 5.284, -1.246, 0.000), #出门抓东西的点
Point( 0.000,0.000,0.000)) #结束出门的点
quaternions=(Quaternion(0.000, 0.000, 0.723, 0.691),
Quaternion(0.000, 0.000,0.050, 0.999),
Quaternion(0.000,0.000,0.000, 1))
# euler_angles=[0.000,3.1415,0.000]
# for angle in euler_angles:
# q_angle = quaternion_from_euler(0, 0, angle, axes="sxyz")
# q = Quaternion(*q_angle)
# quaternions.append(q)
for i in range(3):
self.waypoints.append(Pose(location[i], quaternions[i]))
# the locate can specified by ourselves
self.sm_EnterRoom = StateMachine(outcomes = ['succeeded','aborted','error'])
with self.sm_EnterRoom:
# rospy.logerr('sm_EnterRoom add State')
# arm go to the nav_pose in the srdf
# self.sm_EnterRoom.userdata.arm_mode =0
# self.sm_EnterRoom.userdata.arm_ps_1 = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'mode':'arm_mode','arm_ps':'arm_ps_1'})
# wait the door to open
# StateMachine.add('DOOR_DETECT',
# DoorDetect().door_detect_,
# transitions={'invalid':'WAIT','valid':'DOOR_DETECT','preempted':'error'})
# simple delay 5s
# self.sm_EnterRoom.userdata.rec = 5.0
# StateMachine.add('WAIT',
# Wait(),
# transitions={'succeeded':'NAV_1','error':'error'},
# remapping ={'rec':'rec'})
# self.sm_EnterRoom.userdata.point = Point(1.5,0.0,0.0)
# StateMachine.add('SIMPLE_MOVE',
# SimpleMove_move(),
# transitions={'succeeded':'NAV_1','aborted':'NAV_1','error':'error'},
# remapping={'point':'point'})
# navstack to room of crowds
# waypoints are the list of Pose fit the data type need
self.sm_EnterRoom.userdata.start_waypoint = self.waypoints[0]
# self.sm_EnterRoom.userdata.nav_mode =0
StateMachine.add('NAV_1',
NavStack(),
transitions={'succeeded':'SELF_INTRO','aborted':'NAV_1','error':'error'},
remapping = {'pos_xm':'start_waypoint'})
# self-introduce
self.sm_EnterRoom.userdata.sentences_2 = 'I am robot xiaomeng'
StateMachine.add('SELF_INTRO',
Speak(),
remapping ={'sentences':'sentences_2'},
transitions ={'succeeded':'succeeded','aborted':'SELF_INTRO','error':'error'})
# we have already identity the people from 0-4 when the first recongization
self.sm_FaceDetect = StateMachine(outcomes = ['succeeded','aborted','error'],
output_keys = ['people_position','num_list'])
with self.sm_FaceDetect:
self.sm_FaceDetect.userdata.people_position =list()
self.sm_FaceDetect.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
remapping = {'sentences':"sentences"},
transitions = {'succeeded':'GET_POSITION','aborted':'aborted','error':'error'})
# call face_reco service for get all the people position which is a list
self.sm_FaceDetect.userdata.name_id =-1
self.sm_FaceDetect.userdata.num_list = list()
StateMachine.add('GET_POSITION',
FaceReco(),
remapping ={'name_id':'name_id','position':'people_position','num_list':'num_list'},
transitions ={'succeeded':'succeeded',
'again':'GET_POSITION',
'aborted':'GET_POSITION',
'error':'error',
'turn_l':'TURN_L',
'turn_r':'TURN_R',
'train_error':'aborted'})
# if the face-recognize failed, we should make some remedy to make the state continue
self.sm_FaceDetect.userdata.turn_point_1 = Point(0.0,0.0,pi/8)
StateMachine.add('TURN_L',
SimpleMove_move(),
transitions={'succeeded':'SPEAK_2','error':'error'},
remapping ={'point':'turn_point_1'})
self.sm_FaceDetect.userdata.turn_point_2 = Point(0.0,0.0,-pi/8)
StateMachine.add('TURN_R',
SimpleMove_move(),
remapping ={'point':'turn_point_2'},
transitions ={'succeeded':'SPEAK_2','error':'error'})
StateMachine.add('SPEAK_2',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'GET_POSITION','aborted':'aborted','error':'error'})
self.sm_GetTarget = StateMachine(outcomes =['succeeded','aborted','error'],
input_keys =['target'])#the target is a string.....
with self.sm_GetTarget:
# because xm is nav to pose in the nav_pose
self.sm_GetTarget.userdata.nav_ps = self.waypoints[1]
# this smach code donnot to grasp ,so this part is useless
StateMachine.add('NAV_TARGET',
NavStack(),
remapping ={'pos_xm':'nav_ps'},
transitions ={'succeeded':'FIND_OBJECT','aborted':'NAV_TARGET','error':'error'})
self.sm_GetTarget.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
new_vision.FindObject(),
remapping ={'name':'target','object_pos':'object_pos'},
transitions ={'succeeded':'TALK','aborted':'FIND_OBJECT','error':'error'})
self.sm_GetTarget.userdata.sentences = 'I find the target'
StateMachine.add('TALK',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'PICK','aborted':'TALK','error':'error'})
self.sm_GetTarget.userdata.arm_mode_1 =1
StateMachine.add('PICK',
ArmCmd(),
remapping ={'mode':'arm_mode_1','arm_ps':'object_pos'},
transitions ={'succeeded':'succeeded','aborted':'succeeded','error':'error'})
self.sm_GetTarget.userdata.arm_mode_2 = 0
self.sm_GetTarget.userdata.arm_ps_2 = PointStamped()
StateMachine.add('PUT',
PlaceBag(),
transitions ={'succeeded':'succeeded','aborted':'aborted'})
# concurrence for the speech_node
# we want to make the speech-meaning a timeout, so we use the concurrence function
# but in fact we can also use multithread to slove the problem
self.meta_Remember = Concurrence(outcomes=['succeeded','aborted','error'],
output_keys=['name','target'],
default_outcome ='succeeded',
outcome_map={'succeeded':{'NAME_AND_THING':'succeeded'}},
child_termination_cb =self.child_cb)
with self.meta_Remember:
Concurrence.add('GET_EMPTY_NAME',
Hehe()
)
Concurrence.add('NAME_AND_THING',
NameAndThing(),
remapping ={'name':'name','target':'target'}
)
# input one position is a list
# io_keys can wirte and read
# userdata can update real-time
# no need to call the cv service
self.sm_Remember = StateMachine(outcomes =['succeeded','aborted','error'],
input_keys =['person_position'],
output_keys =['name','target']
)
with self.sm_Remember:
# rospy.logerr("sm_Remember add state")
# self.sm_Remember.userdata.nav_mode =1
# rospy.logerr("Is here1?")
StateMachine.add('NAV_GO',
NavStack(),
remapping ={'pos_xm':'person_position'},
transitions ={'succeeded':'TALK','aborted':'NAV_GO','error':'error'}
)
self.sm_Remember.userdata.sentences = "what is your name and what do you want"
StateMachine.add('TALK',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'GET_BOTH','aborted':'TALK','error':'error'})
# StateMachine.add('RUNNODE',
# RunNode(),
# transitions ={'succeeded':'GET_BOTH','aborted':'aborted'})
StateMachine.add('GET_BOTH',
self.meta_Remember,
remapping ={'name':'name','target':'target'},
transitions ={'succeeded':'WAIT','aborted':'GET_BOTH','error':'error'})
self.sm_Remember.userdata.rec =2.0
StateMachine.add('WAIT',
Wait(),
remapping={'rec':'rec'},
transitions ={'succeeded':'succeeded','error':"error"})
self.sm_GiveBack = StateMachine(outcomes =['succeeded','aborted','error'],
input_keys =['name_id','goal_name','goal_target'])# the name is a string
with self.sm_GiveBack:
# rospy.logerr('sm_GiveBack add State')
# self.sm_GiveBack.userdata.nav_ps = self.waypoints[0]
# self.sm_GiveBack.userdata.nav_mode_1 =0
# StateMachine.add('NAV_ROOM',
# NavStack(),
# remapping ={'pos_xm':'nav_ps','mode':'nav_mode_1'},
# transitions ={'succeeded':'FACE_RECO','aborted':'NAV_ROOM','error':'error'})
# self.sm_GiveBack.userdata.name_id =0
# self.sm_GiveBack.userdata.name_list =list()
self.sm_GiveBack.userdata.sentences = "please look at me"
StateMachine.add('SPEAK',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'WAIT','aborted':'aborted','error':'error'})
self.sm_GiveBack.userdata.rec =5.0
StateMachine.add('WAIT',
Wait(),
remapping ={'rec':'rec'},
transitions ={'succeeded':'FACE_RECO','error':'error'})
self.sm_GiveBack.userdata.person_position =PointStamped()
StateMachine.add('FACE_RECO',
FaceReco(),
remapping ={'position':'person_position','name_id':'name_id'},
transitions ={'succeeded':'NAV_GO',
'again':'FACE_RECO',
'aborted':'FACE_RECO',
'error':'error',
'turn_l':'TURN_L',
'turn_r':'TURN_R',
'train_error':'aborted'}
)
self.sm_GiveBack.userdata.turn_point_1 = Point(0.0,0.0,pi/8)
StateMachine.add('TURN_L',
SimpleMove_move(),
transitions={'succeeded':'SPEAK_2','error':'error'},
remapping ={'point':'turn_point_1'})
self.sm_GiveBack.userdata.turn_point_2 = Point(0.0,0.0,-pi/8)
StateMachine.add('TURN_R',
SimpleMove_move(),
remapping ={'point':'turn_point_2'},
transitions ={'succeeded':'SPEAK_2','error':'error'})
StateMachine.add('SPEAK_2',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'FACE_RECO','aborted':'aborted','error':'error'})
#please pay attention!
# self.sm_GiveBack.userdata.nav_mode_2 =1
StateMachine.add('NAV_GO',
NavStack(),
remapping ={'pos_xm':'person_position'},
transitions ={'succeeded':'GET_NAME','aborted':'NAV_GO','error':'error'})
StateMachine.add("GET_NAME",
NameHehe2(),
remapping ={'goal_target':'goal_name','goal_name':'goal_name','goal_target':'goal_target', 'sentences':'sentences_1'},
transitions ={'succeeded':'TALK_1','aborted':'aborted','error':'error'})
self.sm_GiveBack.userdata.sentences_1 =''
StateMachine.add('TALK_1',
Speak(),
remapping ={'sentences':"sentences_1"},
transitions ={'succeeded':'PUT','aborted':"PUT",'error':'error'})
StateMachine.add('PUT' , PlaceBag(),
transitions = {'succeeded':'succeeded',
'aborted':'succeeded'})
# this pose should be specified to make the people get it
# self.sm_GiveBack.userdata.arm_mode =2
# self.sm_GiveBack.userdata.place_ps = PointStamped()
# self.sm_GiveBack.userdata.place_ps.header.frame_id ='base_footprint'
# self.sm_GiveBack.userdata.place_ps.point.x =0.7
# self.sm_GiveBack.userdata.place_ps.point.y =0.0
# self.sm_GiveBack.userdata.place_ps.point.z =0.3
# StateMachine.add('PLACE',
# ArmCmd(),
# remapping ={'mode':'arm_mode','arm_ps':'place_ps'},
# transitions ={'succeeded':'TALK_2','aborted':'PLACE','error':'error'})
# self.sm_GiveBack.userdata.sentences_2 = 'I get the thing you want, am I?'
# StateMachine.add('TALK_2',
# Speak(),
# remapping ={'sentences':'sentences_2'},
# transitions ={'succeeded':'succeeded','aborted':"aborted",'error':'error'})
self.sm_EndTask = StateMachine(outcomes =['succeeded','aborted','error'])
with self.sm_EndTask:
# rospy.logerr('sm_EndTask add State')
self.sm_EndTask.userdata.nav_ps = self.waypoints[2]
# self.sm_EndTask.userdata.nav_mode = 0
StateMachine.add('NAV_BYEBYE',
NavStack(),
remapping ={'pos_xm':'nav_ps'},
transitions ={'succeeded':'succeeded','aborted':'NAV_BYEBYE','error':'error'})
self.sm_EndTask.userdata.point = Point(1.8,0.0,0.0)
# StateMachine.add('SIMPLE_MOVE',
# SimpleMove_move(),
# remapping={'point':'point'},
# transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'})
self.WHOISWHO = StateMachine(outcomes =['succeeded','aborted','error'])
with self.WHOISWHO:
# we can generate some state which contant the list_value
StateMachine.add('ENTERROOM',
self.sm_EnterRoom,
transitions ={'succeeded':'FACEDETECT','aborted':'aborted','error':'error'})
self.WHOISWHO.userdata.people_position =list()
self.WHOISWHO.userdata.num_list = list()
StateMachine.add('FACEDETECT',
self.sm_FaceDetect,
remapping ={'people_position':'people_position','num_list':'num_list'},
transitions = {'succeeded':'GETPERSON','aborted':'aborted','error':'error'}
)
self.WHOISWHO.userdata.person_position = PointStamped()
# if someone is recongized failed, we should make the total list in the same order
# get the last element of the list
# FGM: This state is to get different people pos
StateMachine.add('GETPERSON',
GetValue(),
remapping ={'element_list':'people_position','element':'person_position'},
transitions ={'succeeded':'REMEMBER','aborted':"GETID",'error':'error'}
)
# the cv will remember the face with the increasing ID
self.WHOISWHO.userdata.name =''
self.WHOISWHO.userdata.target= ''
self.WHOISWHO.userdata.name_list =list()
self.WHOISWHO.userdata.target_list =list()
self.WHOISWHO.userdata.goal_name =''
self.WHOISWHO.userdata.goal_target''
# if donnot need to remember, the order of ids should be the same with the positions return
# the first of the position should be the 0
# last be the 4
StateMachine.add('REMEMBER',
self.sm_Remember,
remapping ={'person_position':'person_position','name':'name','target':'target'},
transitions ={'succeeded':'NAMEINLIST','aborted':'aborted','error':'error'}
)
#this state use for joining the name and the target into the name_list and target_list
# insert in the head of the lists
# if the name and target are empty, we should also append them to the lists
StateMachine.add('NAMEINLIST',
NameInList(),
remapping ={'name':'name','target':'target','name_list':'name_list','target_list':'target_list'},
transitions ={'succeeded':'GETPERSON','aborted':'aborted','error':'error'}
)
# ############################################################################
# so far here ,the tasks of remember is completed , the rest is the tasks to return the target to the people
# we should take out the name and the matched target for xm to grasp and give back
# in fact, we will pop the name and target out the list,so the name_id is gradually reduced
# ############################################################################
StateMachine.add('GETTARGET',
GetValue2(),
remapping ={'element_list':'target_list','element':'target'},
transitions ={'succeeded':'CATCHTARGET','aborted':'ENDTASK','error':'error'})
StateMachine.add('CATCHTARGET',
self.sm_GetTarget,
transitions= {'succeeded':'NAV_ROOM','aborted':'NAV_ROOM','error':'error'},
remapping = {'target':'target'})
# ############################################################################
self.WHOISWHO.userdata.nav_ps = self.waypoints[0]
# self.WHOISWHO.userdata.nav_mode_1 =0
StateMachine.add('NAV_ROOM',
NavStack(),
remapping ={'pos_xm':'nav_ps'},
transitions ={'succeeded':'GIVEBACK','aborted':'NAV_ROOM','error':'error'})
StateMachine.add('CHECKFINISH',
CBState(self.checkfinish,outcomes=['finish','continue'],input_keys=['num_list']),
transitions={'finish':'ENDTASK',
'continue':'GETTARGET'},
remapping={'num_list':'num_list'})
# StateMachine.add('CLEAR_MAP',
# ClearMap(),
# transitions ={'succeeded':'GETID','aborted':'GETID'})
# get the last num of the list, ID
# if the name and target list is empty, will skipped this item
self.WHOISWHO.userdata.sentences_1 = 'please change the order'
StateMachine.add('SPEAK_1',
Speak(),
remapping ={'sentences':'sentences_1'},
transitions ={'succeeded':'WAIT_HEHE','aborted':'WAIT_HEHE','error':'error'})
self.WHOISWHO.userdata.rec_hehe =10.0
StateMachine.add('WAIT_HEHE',
Wait(),
remapping ={'rec':'rec_hehe'},
transitions ={'succeeded':'SPEAK_2','error':'error'})
self.WHOISWHO.userdata.sentences_2 = 'I will make the recongization task'
StateMachine.add('SPEAK_2',
Speak(),
transitions ={'succeeded':'GETID','aborted':'GETID','error':'error'},
remapping ={'sentences':'sentences_2'})
self.WHOISWHO.userdata.name_id = 0
StateMachine.add('GETID',
GetId(),
remapping ={'output_id':'name_id','input_list':'name_list','num_list':'num_list'},
transitions ={'succeeded':'GETTARGET','aborted':'ENDTASK','error':'error'}
)
StateMachine.add('GIVEBACK',
self.sm_GiveBack,
remapping ={'name_id':'name_id','name_list':'name_list','target_list':'target_list'},
transitions ={'succeeded':'CHECKFINISH','aborted':'CHECKFINISH','error':'error'}
)
StateMachine.add('ENDTASK',
self.sm_EndTask,
transitions ={'succeeded':'succeeded','aborted':'aborted','error':'error'})
# rospy.logerr('sm_Top execute begin')
intro_server = IntrospectionServer('whoiswho',self.WHOISWHO, 'SM_ROOT')
intro_server.start()
out = self.WHOISWHO.execute()
intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
def __init__(self):
rospy.init_node('carry_smach_move', 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 = (0.0, 3*pi/2, pi/2, 3*pi/2, pi/2, 3*pi/2, 3*pi/2, 0.0, pi, 0.0, pi, 3*pi/2)
# 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()
# 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, -3.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(2.9, 1.0, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(3.0, 1.2, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[5]))
self.waypoints.append(Pose(Point(4.4, -0.6, 0.0), quaternions[6]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[7]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[8]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[9]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[10]))
self.waypoints.append(Pose(Point(-1.6, 0.1, 0.0), quaternions[11]))
# State machine
#self.sm_out_main = StateMachine(outcomes=['succeeded','aborted','preempted'])
#self.sm_out_main.userdata.goal = self.waypoints[2];
#with self.sm_out_main:
# StateMachine.add('GO_OUT_MAIN_ROOM', Nav2Waypoint(),
# transitions={'succeeded':'succeeded',
# 'aborted':'aborted'})
# Concurrent State machine
self.sm_in_main_room = Concurrence(outcomes=['succeeded','aborted','preempted','go_kitchen','go_bedroom','go_sleep'],
default_outcome='succeeded',
child_termination_cb=self.in_main_room_child_termination_cb,
outcome_cb=self.in_main_room_outcome_cb)
self.sm_in_main_room.userdata.goal = self.waypoints[1];
with self.sm_in_main_room:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[1]))
# Concurrent State machine
self.sm_sleep = Concurrence(outcomes=['succeeded','aborted','preempted','go_kitchen','go_bedroom','go_main_room'],
default_outcome='succeeded',
child_termination_cb=self.in_sleep_child_termination_cb,
outcome_cb=self.in_sleep_outcome_cb)
self.sm_sleep.userdata.goal = self.waypoints[0];
with self.sm_sleep:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[0]))
# Concurrent State machine
self.sm_in_kitchen = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_bedroom','go_sleep'],
default_outcome='succeeded',
child_termination_cb=self.in_kitchen_child_termination_cb,
outcome_cb=self.in_kitchen_outcome_cb)
self.sm_in_kitchen.userdata.goal = self.waypoints[6];
with self.sm_in_kitchen:
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[6]))
# Concurrent State machine
self.sm_in_bedroom = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_kitchen','go_sleep'],
default_outcome='succeeded',
child_termination_cb=self.in_bedroom_child_termination_cb,
outcome_cb=self.in_bedroom_outcome_cb)
self.sm_in_bedroom.userdata.goal = self.waypoints[11];
with self.sm_in_bedroom:
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[11]))
# 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('IN_MAIN_ROOM', self.sm_in_main_room, transitions={'succeeded':'IN_MAIN_ROOM',
'go_kitchen':'NAV_M2K_M',
'go_sleep':'IN_SLEEP',
'go_bedroom':'NAV_M2B_M'})
StateMachine.add('IN_KITCHEN', self.sm_in_kitchen, transitions={'succeeded':'succeeded',
'go_main_room':'NAV_K2M_K',
'go_sleep':'NAV_K2S_K',
'go_bedroom':'NAV_K2B_K'})
StateMachine.add('IN_BEDROOM', self.sm_in_bedroom, transitions={'succeeded':'succeeded',
'go_kitchen':'NAV_B2K_B',
'go_sleep':'NAV_B2S_B',
'go_main_room':'NAV_B2M_B'})
StateMachine.add('IN_SLEEP', self.sm_sleep, transitions={'succeeded':'succeeded',
'go_kitchen':'NAV_S2K_M',
'go_main_room':'IN_MAIN_ROOM',
'go_bedroom':'NAV_S2B_M'})
StateMachine.add('NAV_M2K_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_M2K_K',
'aborted':'aborted'})
StateMachine.add('NAV_M2K_K', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'NAV_M2K_END',
'aborted':'aborted'})
StateMachine.add('NAV_M2K_END', Nav2Waypoint(self.waypoints[6]),
transitions={'succeeded':'IN_KITCHEN',
'aborted':'aborted'})
StateMachine.add('NAV_K2M_K', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'NAV_K2M_M',
'aborted':'aborted'})
StateMachine.add('NAV_K2M_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_K2M_END',
'aborted':'aborted'})
StateMachine.add('NAV_K2M_END', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'IN_MAIN_ROOM',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_M2B_C',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_C', Nav2Waypoint(self.waypoints[8]),
transitions={'succeeded':'NAV_M2B_B',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_B', Nav2Waypoint(self.waypoints[10]),
transitions={'succeeded':'NAV_M2B_END',
'aborted':'aborted'})
StateMachine.add('NAV_M2B_END', Nav2Waypoint(self.waypoints[11]),
transitions={'succeeded':'IN_BEDROOM',
'aborted':'aborted'})
StateMachine.add('NAV_K2S_K', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'NAV_K2S_M',
'aborted':'aborted'})
StateMachine.add('NAV_K2S_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_K2S_END',
'aborted':'aborted'})
StateMachine.add('NAV_K2S_END', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'IN_SLEEP',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_K', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'NAV_K2B_C',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_C', Nav2Waypoint(self.waypoints[8]),
transitions={'succeeded':'NAV_K2B_B',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_B', Nav2Waypoint(self.waypoints[10]),
transitions={'succeeded':'NAV_K2B_END',
'aborted':'aborted'})
StateMachine.add('NAV_K2B_END', Nav2Waypoint(self.waypoints[11]),
transitions={'succeeded':'IN_BEDROOM',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_B', Nav2Waypoint(self.waypoints[9]),
transitions={'succeeded':'NAV_B2K_C',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_C', Nav2Waypoint(self.waypoints[7]),
transitions={'succeeded':'NAV_B2K_K',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_K', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'NAV_B2K_END',
'aborted':'aborted'})
StateMachine.add('NAV_B2K_END', Nav2Waypoint(self.waypoints[6]),
transitions={'succeeded':'IN_KITCHEN',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_B', Nav2Waypoint(self.waypoints[9]),
transitions={'succeeded':'NAV_B2S_C',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_C', Nav2Waypoint(self.waypoints[7]),
transitions={'succeeded':'NAV_B2S_M',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_B2S_END',
'aborted':'aborted'})
StateMachine.add('NAV_B2S_END', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'IN_SLEEP',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_B', Nav2Waypoint(self.waypoints[9]),
transitions={'succeeded':'NAV_B2M_C',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_C', Nav2Waypoint(self.waypoints[7]),
transitions={'succeeded':'NAV_B2M_M',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_M', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'NAV_B2M_END',
'aborted':'aborted'})
StateMachine.add('NAV_B2M_END', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'IN_MAIN_ROOM',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_S2B_C',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_C', Nav2Waypoint(self.waypoints[8]),
transitions={'succeeded':'NAV_S2B_B',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_B', Nav2Waypoint(self.waypoints[10]),
transitions={'succeeded':'NAV_S2B_END',
'aborted':'aborted'})
StateMachine.add('NAV_S2B_END', Nav2Waypoint(self.waypoints[11]),
transitions={'succeeded':'IN_BEDROOM',
'aborted':'aborted'})
StateMachine.add('NAV_S2K_M', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'NAV_S2K_K',
'aborted':'aborted'})
StateMachine.add('NAV_S2K_K', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'NAV_S2K_END',
'aborted':'aborted'})
StateMachine.add('NAV_S2K_END', Nav2Waypoint(self.waypoints[6]),
transitions={'succeeded':'IN_KITCHEN',
'aborted':'aborted'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('carry_sm', self.sm_top, '/SM_CARRY_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()
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)
def __init__(self):
rospy.init_node('Shopping')
self.smach_bool = False
rospy.on_shutdown(self.shutdown)
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
StateMachine.add('DOOR_DETECT',
DoorDetect().door_detect_,
transitions={
'invalid': 'WAIT',
'valid': 'DOOR_DETECT',
'preempted': 'aborted'
})
# waits
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'ENTER',
'error': 'error'
},
remapping={'rec': 'wait_len'})
# StateMachine.add('ENTER', LinearDisplacement(), transitions={'succeeded': 'succeeded',
# 'preempted': 'ENTER',
# 'error': 'error'},
# remapping={'displacementVec': 'point'})
self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
StateMachine.add('ENTER',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'ENTER',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
# how to get stop signal?
self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
default_outcome='stop',
outcome_map={
'remeber': {
'STOP': 'remeber'
},
'stop': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.trace_child_cb,
input_keys=['PT_LIST', 'mission'],
output_keys=['PT_LIST', 'mission'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'])
with self.meta_follow:
self.meta_follow.userdata.pos_xm = Pose()
StateMachine.add('FIND',
FindPeople().find_people_,
transitions={
'invalid': 'META_NAV',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={'pos_xm': 'pos_xm'})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP',
CheckStop2(),
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
self.Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target'])
with self.Pick:
self.Pick.userdata.target_pos = PointStamped()
self.Pick.userdata.nav_pos = Pose()
self.Pick.userdata.pick_pos = PointStamped()
self.Pick.userdata.distance = 1.0
self.Pick.userdata.distance2 = 0.9
self.Pick.userdata.target_mode = 1
self.Pick.userdata.objmode = 1
#self.Pick.userdata.target = 'ice_tea'
StateMachine.add('FIND',
FindObject(),
transitions={
'succeeded': 'DISTANCE',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'name': 'target',
'object_pos': 'target_pos',
'object_map_point': 'object_map_point'
})
# StateMachine.add('FIND', self.FindObj,
# transitions={'succeeded': 'DISTANCE',
# 'aborted': 'aborted', 'error': 'error'},
# remapping={'target': 'target',
# 'indice': 'indice',
# 'targets': 'targets',
# 'object_map_point':'object_map_point',
# 'target_pos': 'target_pos'})
# StateMachine.add('FIND', self.FindObj,
# transitions={'succeeded': 'DISTANCE',
# 'aborted': 'aborted', 'error': 'error'},
# remapping = {'name':'target' ,
# 'object_pos':'target_pos',
# 'object_map_point':'object_map_point'})
StateMachine.add('DISTANCE',
CBState(self.PickDistance,
outcomes=['succeeded', 'error'],
input_keys=['name'],
output_keys=['distance']),
transitions={
'succeeded': 'POS_JUS',
'error': 'POS_JUS'
},
remapping={
'distance': 'distance',
'name': 'target'
})
StateMachine.add('POS_JUS',
new_PosJustfy(),
transitions={
'succeeded': 'NAV',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'pose': 'nav_pos',
'distance': 'distance',
'object_pos': 'target_pos'
})
StateMachine.add('NAV',
NavStack(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'NAV',
'error': 'error',
},
remapping={'pos_xm': 'nav_pos'})
StateMachine.add('PICK2',
ArmCmdForTf(),
transitions={
'succeeded': 'succeeded',
'error': 'error',
'aborted': 'aborted'
},
remapping={
'arm_ps': 'object_map_point',
'mode': 'objmode'
})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'PICK2',
'error': 'error'
},
remapping={
'object_pos': 'pick_pos',
'name': 'target',
'object_state': 'object_state'
})
StateMachine.add('PICK',
new_ArmCmd(),
transitions={
'succeeded': 'succeeded',
'error': 'error',
'aborted': 'aborted'
},
remapping={
'arm_ps': 'pick_pos',
'mode': 'objmode',
'object_state': 'object_state'
})
self.sm_FaceDetect = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
output_keys=['people_position', 'num_list'])
with self.sm_FaceDetect:
self.sm_FaceDetect.userdata.people_position = list()
self.sm_FaceDetect.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
remapping={'sentences': "sentences"},
transitions={
'succeeded': 'GET_POSITION',
'aborted': 'aborted',
'error': 'error'
})
# call face_reco service for get all the people position which is a list
self.sm_FaceDetect.userdata.name_id = -1 #
self.sm_FaceDetect.userdata.num_list = list()
self.sm_FaceDetect.userdata.distance = 0.6
StateMachine.add('GET_POSITION',
FaceReco(),
remapping={
'name_id': 'name_id',
'position': 'people_position',
'num_list': 'num_list',
'distance': 'distance'
},
transitions={
'succeeded': 'succeeded',
'again': 'GET_POSITION',
'aborted': 'GET_POSITION',
'error': 'error',
'turn_l': 'MOVEAHEAD',
'turn_r': 'MOVEAHEAD',
'train_error': 'aborted'
})
self.sm_FaceDetect.userdata.point_1 = Point(0.1, 0.0, 0.0)
StateMachine.add(
'MOVEAHEAD',
SimpleMove_move(),
transitions={
'succeeded': 'GET_POSITION',
'error': 'error',
'aborted': 'GET_POSITION'
},
remapping={'point': 'point_1'},
)
self.GetTask = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
output_keys=['task'])
with self.GetTask:
self.GetTask.userdata.people_position = list()
self.GetTask.userdata.num_list = list()
self.GetTask.userdata.person_position = Pose()
StateMachine.add('GET_POSITION',
self.sm_FaceDetect,
transitions={
'succeeded': 'GET_VALUE1',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'people_position': 'people_position',
'num_list': 'num_list'
})
StateMachine.add('GET_VALUE1',
GetValue(),
remapping={
'element_list': 'people_position',
'element': 'person_position'
},
transitions={
'succeeded': 'NAV1',
'aborted': "succeeded",
'error': 'error'
})
StateMachine.add('NAV1',
NavStack(),
transitions={
'succeeded': 'ASK_TASK1',
'aborted': 'NAV1',
'error': 'error'
},
remapping={'pos_xm': 'person_position'})
self.GetTask.userdata.sentence1 = 'what do you want?'
StateMachine.add('ASK_TASK1',
Speak(),
transitions={
'succeeded': 'ANS1',
'aborted': 'GET_VALUE1',
'error': 'error'
},
remapping={'sentence': 'sentence1'})
StateMachine.add('ANS1',
ShoppingGetTask(),
transitions={
'succeeded': 'GET_VALUE1',
'aborted': 'GET_VALUE1'
},
remapping={'task': 'task'})
# StateMachine.add('ASK_TASK2' , Speak() , transitions={'succeeded':'ANS2',
# 'aborted':'aborted',
# 'error':'error'},
# remapping={'sentence':'sentence1'})
# StateMachine.add('ANS2' , ShoppingGetTask() , transitions={'succeeded':'succeeded',
# 'aborted':'aborted'},
# remapping={'task':'task'})
self.DealTask = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['task', 'mission'])
with self.DealTask:
self.DealTask.userdata.nav_pos = Pose()
self.DealTask.userdata.indice = 0
self.DealTask.userdata.indice2 = 3
self.DealTask.userdata.name = ''
StateMachine.add('NXT_TASK',
ShoppingNextTask(),
transitions={
'go': 'NAV',
'back': 'NAV_CASH',
'finish': 'succeeded',
'aborted': "aborted"
},
remapping={
'pose': 'nav_pos',
'name': 'name',
'indice': 'indice'
})
StateMachine.add('NAV',
NavStack(),
transitions={
'succeeded': 'PICK',
'aborted': 'NAV',
'error': 'error'
},
remapping={'pos_xm', 'nav_pos'})
StateMachine.add('PICK',
self.Pick,
transitions={
'succeeded': 'NXT_TASK',
'aborted': 'NXT_TASK',
'error': 'error'
},
remapping={'target': 'name'})
StateMachine.add('NAV_CASH',
NavStack(),
transitions={
'succeeded': 'PLACE',
'aborted': 'NAV_CASH',
'error': 'error'
},
remapping={'pos_xm', 'nav_pos'})
#place need to be upgraded
StateMachine.add('PLACE',
Place2(),
transitions={
'succeeded': 'NXT_TASK',
'aborted': 'NXT_TASK',
})
self.shopping = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.shopping:
self.shopping.userdata.PT_LIST = {}
self.shopping.userdata.mission = {}
self.shopping.userdata.task = list()
self.shopping.userdata.rec = 5.0
# StateMachine.add('ENTERROOM',
# self.sm_EnterRoom,
# transitions={'succeeded':'START','aborted':'aborted'})
StateMachine.add('START',
GetSignal(),
transitions={
'succeeded': 'RUNNODE',
'aborted': 'aborted'
})
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'TRACE',
'error': 'error'
},
remapping={'rec': 'rec'})
StateMachine.add('TRACE',
self.trace,
transitions={
'remeber': 'TRACE',
'stop': 'GET_TASK',
'aborted': 'aborted'
},
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
StateMachine.add('GET_TASK',
self.GetTask,
transitions={
'succeeded': 'DEAL_TASK',
'aborted': 'aborted',
'error': 'error'
},
remapping={'task': 'task'})
StateMachine.add('DEAL_TASK',
self.DealTask,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'task': 'task'})
intro_server = IntrospectionServer('shopping', self.shopping,
'SM_ROOT')
intro_server.start()
out = self.shopping.execute()
intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
def main():
rospy.init_node('smach_usecase_step_06')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'DRAW_SHAPES'})
# Draw some polygons
shapes_cc = Concurrence(
outcomes=['succeeded','aborted','preempted'],
default_outcome='aborted',
outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
StateMachine.add('DRAW_SHAPES',shapes_cc)
with shapes_cc:
# Draw a large polygon with the first turtle
Concurrence.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big))
# Draw a small polygon with the second turtle
draw_monitor_cc = Concurrence(
['succeeded','aborted','preempted'],
'aborted',
child_termination_cb = lambda so: True,
outcome_map = {
'succeeded':{'DRAW':'succeeded'},
'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
'aborted':{'MONITOR':'invalid'}})
Concurrence.add('SMALL',draw_monitor_cc)
with draw_monitor_cc:
Concurrence.add('DRAW',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
def turtle_far_away(ud, msg):
"""Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
return True
return False
Concurrence.add('MONITOR',
MonitorState('/turtle1/pose',turtlesim.msg.Pose,
cond_cb = turtle_far_away))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self):
rospy.init_node('help_me_carry')
rospy.on_shutdown(self.shutdown)
self.smach_bool = False
self.trace = Concurrence(outcomes=['succeeded', 'aborted'],
default_outcome='succeeded',
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': 'FOLLOW', 'aborted': 'aborted', 'preempt': 'succeeded'})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP', CheckStop())
Concurrence.add('RUNNODE', RunNode())
self.xm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.xm_Nav:
# userdata 列表
# xm的位置信息,由GetTarget()得到
self.xm_Nav.userdata.pos_xm = Pose()
# 这个target_mode == 1返回Pose()，如果== 0 返回名字
self.xm_Nav.userdata.target_mode = 1
# 这里的不可能产生aborted
StateMachine.add('GETTAGET_POSE',
GetTarget(),
transitions={'succeeded': 'NAV_GO',
'aborted': 'aborted', 'error': 'error'},
remapping={'target': 'target', 'current_task': 'current_task', 'current_target': 'pos_xm', 'mode': 'target_mode'})
# StateMachine.add('FINDWAY',
# FindWay(),
# transitions = {'succeeded':'NAV_PATH1','aborted':'NAV_GO','error':'NAV_GO'},
# remapping={'way_path1':'way_path1','way_path2':'way_path2'})
# StateMachine.add('NAV_PATH1',
# NavStack(),
# transitions={'succeeded':'NAV_PATH2','aborted':'NAV_PATH1','error':'error'},
# remapping={'pos_xm':'way_path1'})
# StateMachine.add('NAV_PATH2',
# NavStack(),
# transitions = {'succeeded':'NAV_GO','aborted':'NAV_PATH2','error':'error'},
# remapping={'pos_xm':'way_path2'})
# 如果找不到路径继续执行NavStack的execute(),知道找到为止
StateMachine.add('NAV_GO',
NavStack(),
transitions={'succeeded': 'succeeded',
'aborted': 'NAV_GO', 'error': 'error'},
remapping={'pos_xm': 'pos_xm'})
self.xm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target', 'current_task'])
with self.xm_Find:
# self.xm_Find.userdata.degree = 45.0
self.xm_Find.userdata.rec = 2.0
# run the people_tracking node
StateMachine.add('RUNNODE',
RunNode(),
transitions={'succeeded': 'WAIT', 'aborted': 'succeeded'})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GET_PEOPLE_POS', 'error': 'error'},
remapping={'rec': 'rec'})
# the data should be PointStamped()
# 这里必须先运行节点，也就是用RunNode()状态
self.xm_Find.userdata.pos_xm = Pose()
StateMachine.add('GET_PEOPLE_POS',
FindPeople().find_people_,
transitions={
'invalid': 'NAV_PEOPLE', 'valid': 'CLOSEKINECT', 'preempted': 'aborted'},
remapping={'pos_xm': 'pos_xm'})
# this state will use the userdata remapping in the last state
StateMachine.add('NAV_PEOPLE',
NavStack(),
transitions={'succeeded': 'CLOSEKINECT',
'aborted': 'NAV_PEOPLE', 'error': 'error'},
remapping={'pos_xm': 'pos_xm'})
# close the KinectV2
StateMachine.add('CLOSEKINECT',
CloseKinect(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
self.pick_up = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.pick_up:
self.pick_up.userdata.name = 'bag'
self.pick_up.userdata.objmode = -1
self.pick_up.userdata.arm_state = 'after_grasp_bag'
self.pick_up.userdata.mode = 3
self.pick_up.userdata.arm_ps = PointStamped()
self.pick_up.userdata.arm_ps.header.frame_id = 'base_link'
self.pick_up.userdata.arm_ps.point.x = 0.8
self.pick_up.userdata.arm_ps.point.y = 0.0
self.pick_up.userdata.arm_ps.point.z = 0.6
StateMachine.add('RUNNODE',
RunOBJNode(),
transitions={'succeeded': 'GET_POSITION', 'aborted': 'aborted'})
StateMachine.add('GET_POSITION',
FindObject(),
transitions={'succeeded': 'POS_JUSTFY', 'aborted': 'GET_POSITION', 'error': 'PICK_OBJ2'})
self.pick_up.userdata.pose = Pose()
StateMachine.add('POS_JUSTFY',
PosJustfy(),
remapping={
'object_pos': 'object_pos', 'pose': 'pose'},
transitions={'succeeded': 'NAV_TO', 'aborted': 'aborted', 'error': 'error'})
StateMachine.add('NAV_TO',
NavStack(),
transitions={'succeeded': 'RUNNODE_IMG2',
'aborted': 'NAV_TO', 'error': 'error'},
remapping={"pos_xm": 'pose'})
StateMachine.add('RUNNODE_IMG2',
RunNode_img(),
transitions={'succeeded': 'FIND_AGAIN', 'aborted': 'aborted'})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK_OBJ', 'aborted': 'FIND_AGAIN', 'error': 'PICK_OBJ2'},
remapping={'name': 'name', 'object_pos': 'object_pos', 'objmode': 'objmode'})
StateMachine.add('PICK_OBJ',
ArmCmd(),
transitions={'succeeded': 'succeeded',
'aborted': 'aborted', 'error': 'error'},
# 这里的aborted比赛时可以改成到READY_NAV
remapping={'arm_ps': 'object_pos'})
StateMachine.add('PICK_OBJ2',
ArmCmd(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error'})
# self.pick_up.userdata.sentences = 'xiao meng can not find the thing'
# StateMachine.add('SPEAK',
# Speak(),
# transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'})
# StateMachine.add('READY_NAV',
# ChangeArmState(),
# transitions= {'succeeded':'CLOSEKINECT_OBJ','aborted':'aborted'})
# StateMachine.add('CLOSEKINECT_OBJ',
# Close_OBJ(),
# transitions={'succeeded':'succeeded','aborted':'aborted'})
self.sm_Place = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_Place:
# # place_ps please specified due to the scene
# self.sm_Place.userdata.place_ps = PointStamped()
# self.sm_Place.userdata.place_ps.header.frame_id ='base_link'
# self.sm_Place.userdata.place_ps.point.x =0.8
# self.sm_Place.userdata.place_ps.point.y =0.0
# self.sm_Place.userdata.place_ps.point.z =0.6
# self.sm_Place.userdata.objmode = 2
# # without moveit, if is just place it in a open space
# self.sm_Place.userdata.arm_mode_1 =2
StateMachine.add('PLACE',
ArmCmd2(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
self.help_me = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.help_me:
# 这里可以预先定义一个值，识别错误后仍可以继续进行
self.help_me.userdata.target = list()
self.help_me.userdata.action = list()
self.help_me.userdata.current_task = 0
self.help_me.userdata.start_sentences = 'i will follow you'
self.help_me.userdata.help_sentences = 'pleas help carry the groceries into the house, thank you'
self.help_me.userdata.nav_sentences = 'i will go to the car, please follow me, my master'
self.help_me.userdata.target_name = 'door'
self.help_me.userdata.rec = 3.0
self.help_me.userdata.go_point = Point(1, 0.0, 0.0)
self.help_me.userdata.arrive_sentences = 'here is the car,my master'
self.help_me.userdata.pos_car = gpsr_target['car']['pos']
self.help_me.userdata.pos_door = gpsr_target['door']['pos']
self.help_me.userdata.pos_out_door = gpsr_target['out_door']['pos']
self.help_me.userdata.arm_mode_pick = 3
self.help_me.userdata.arm_state_put = 'xm_place_bag'
self.help_me.userdata.arm_ps = PointStamped(
Header(0, 0, ''), Point(0, 0, 0))
StateMachine.add('GET_START', # 开始喽，获取语音信号，如果你说‘follow me’，那action就是follow
GetSignal(),
transitions={'succeeded': 'SPEAK_START', 'aborted': 'GET_START', 'error': 'error'})
StateMachine.add('SPEAK_START',
Speak(),
transitions={'succeeded': 'FOLLOW',
'aborted': 'FOLLOW', 'error': 'error'},
remapping={'sentences': 'start_sentences'})
StateMachine.add('FOLLOW',
self.trace,
transitions={'succeeded': 'SET_POSITION', 'aborted': 'FOLLOW'})
# StateMachine.add('CLOSEKINECT',
# CloseKinect(),
# transitions = {'succeeded':'PICK','aborted':'PICK'})
#####not finished#####
StateMachine.add('SET_POSITION',
SetPose(),
transitions={'succeeded': 'PICK', 'aborted': 'PICK'})
StateMachine.add('PICK',
self.pick_up,
transitions={'succeeded': 'GET_TARGET',
'aborted': 'GET_TARGET', 'error': 'error'},
remapping={'arm_mode': 'arm_mode_pick'})
StateMachine.add('GET_TARGET',
GetSignal(),
transitions={'succeeded': 'NAV_ROOM', 'aborted': 'GET_TARGET', 'error': 'error'})
StateMachine.add('NAV_OUT_DOOR',
NavStack(),
transitions={'succeeded': 'SIMPLE_MOVE1',
'aborted': 'NAV_OUT_DOOR', 'error': 'error'},
remapping={'pos_xm': 'pos_out_door'})
# 在开启导航之前首先让xm冲进房间
# 使用userdata: go_point
StateMachine.add('SIMPLE_MOVE1',
SimpleMove_move(),
remapping={'point': 'go_point'},
transitions={'succeeded': 'NAV_ROOM', 'aborted': 'NAV_ROOM', 'error': 'error'})
StateMachine.add('NAV_ROOM',
self.xm_Nav,
transitions={'succeeded': 'PUT', 'aborted': 'aborted', 'error': 'error'})
StateMachine.add('PUT',
self.sm_Place,
transitions={'succeeded': 'FIND',
'aborted': 'FIND'},
remapping={'arm_state': 'arm_state_put'})
StateMachine.add('FIND',
self.xm_Find,
transitions={'succeeded': 'ASK_HELP', 'aborted': 'aborted', 'error': 'error'})
StateMachine.add('ASK_HELP',
Speak(),
transitions={'succeeded': 'SPEAK_NAV',
'aborted': 'SPEAK_NAV', 'error': 'error'},
remapping={'sentences': 'help_sentences'})
StateMachine.add('SPEAK_NAV',
Speak(),
transitions={'succeeded': 'SET_TARGET',
'aborted': 'NAV_DOOR', 'error': 'error'},
remapping={'sentences': 'nav_sentences'})
StateMachine.add('SET_TARGET',
SetTarget(),
transitions={
'succeeded': 'NAV_DOOR', 'error': 'error'},
remapping={'pos_car': 'pos_car'})
StateMachine.add('NAV_DOOR',
NavStack(),
transitions={'succeeded': 'DOORDETECT',
'aborted': 'NAV_DOOR', 'error': 'error'},
remapping={'pos_xm': 'pos_door'})
# 遇到问题是Doordetect是否要和导航同时运行
# 将导航分为两部分，一个是导航到门，一个是导航到车
StateMachine.add('DOORDETECT',
DoorDetect().door_detect_,
transitions={'invalid': 'WAIT', 'valid': 'DOORDETECT', 'preempted': 'aborted'})
# 在刷新建图后等待一段时间
# 使用userdata: rec
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'SIMPLE_MOVE', 'error': 'error'},
remapping={'rec': 'rec'})
# 在开启导航之前首先让xm冲进房间
# 使用userdata: go_point
StateMachine.add('SIMPLE_MOVE',
SimpleMove_move(),
remapping={'point': 'go_point'},
transitions={'succeeded': 'NAV_CAR', 'aborted': 'NAV_CAR', 'error': 'error'})
#####not finished#####
StateMachine.add('NAV_CAR',
self.xm_Nav,
transitions={'succeeded': 'SPEAK_ARRIVE',
'aborted': 'NAV_CAR', 'error': 'error'},
remapping={'pos_xm': 'pos_car'})
StateMachine.add('SPEAK_ARRIVE',
Speak(),
remapping={'sentences': 'arrive_sentences'},
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error'})
# self.navigation = Concurrence(outcomes=['succeeded','aborted'],
# default_outcome = ['aborted'],
# input_keys = ['car_position'],
# outcome_cb = self.navigation_child_cb,
# child_termination_cb = self.navigation_out_cb)
# with self.navigation:
# Concurrence.add('NAV')
# Concurrence.add('DOORDETECT')
# self.help_me = StateMachine(outcomes=['succeeded','aborted','error'])
try:
out = self.help_me.execute()
except Exception, e:
rospy.logerr('test failed , loser loser, don\'t have dinner')
rospy.logerr(e)
def __init__(self, target_object_key=None, target_frame='/map'):
input_keys = ['in_room_name']
if target_object_key is not None:
input_keys.append(target_object_key)
smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
input_keys=input_keys,
output_keys=['out_object_found', 'out_location_inside_room_name'])
if target_object_key is None:
target_object_key = 'in_target_object'
self.userdata.in_target_object = '' # No target object set! Before set to str(None), changed for SearchObjectWCSM
going_to_pool = ["I'm going to look for objects.", "Maybe I find objects there. I'll have a look.",
"I think I know where objects can be."]
object_not_found_pool = ["I can't see any object here.", "It seems that there aren't objects here.",
"I haven't found any object here."]
else:
going_to_pool = ["I'm going to look if I find it.", "Maybe I find it there. I'll have a look.",
"I think I know where it can be.", "I think it can be there."]
object_not_found_pool = ["I can't see it here.", "It seems that it isn't here.",
"I haven't found it here."]
object_found_pool = ["I can see the %s.", "Is that the %s? I think so!",
"The %s is just there.", "I found the %s.",
"Can't you see the %s? It's there!"]
self.userdata.out_location_inside_room_name = 'tabletop' # Always the same as it's not possible to know the name.
self.userdata.objects_data = None # To avoid errors the first time the check_if_remaining is called
self.userdata.sensing_route = None # Because at the very beginning there's no route
self.userdata.tabletop_route = None # Because at the very beginning there's no route
self.userdata.out_object_found = None # To avoid errors if there's an abort.
with self:
StateMachine.add('CHECK_IF_OBJECTS_REMAINING', CheckRemaining(),
remapping={'in_obj_list': 'objects_data'},
transitions={'empty': 'CHECK_IF_TABLE_ROUTE_EMPTY',
'remaining': 'TELL_GO_RECOGNIZE'})
@smach.cb_interface(input_keys=['in_route', 'in_table_poses', 'in_orientations'],
output_keys=['out_remaining_poses', 'out_remaining_orientations'],
outcomes=['remaining', 'empty'])
def check_ttop_route_emptiness(userdata):
if userdata.in_route:
# We get the remaining poses and orientations of the tabletop route
# and then we'll recalculate the route through that remaining nodes
remaining_poses = []
remaining_orientations = []
for index in userdata.in_route: # Tabletop route has only the indices of the elements
remaining_poses.append(userdata.in_table_poses[index])
remaining_orientations.append(userdata.in_orientations[index])
userdata.out_remaining_poses = remaining_poses
userdata.out_remaining_orientations = remaining_orientations
return 'remaining'
return 'empty'
StateMachine.add('CHECK_IF_TABLE_ROUTE_EMPTY',
CBState(check_ttop_route_emptiness,
input_keys=['in_route', 'in_table_poses', 'in_orientations'],
output_keys=['out_remaining_poses', 'out_remaining_orientations'],
outcomes=[succeeded, 'empty']),
remapping={'in_route': 'tabletop_route',
'in_table_poses': 'tabletop_detect_poses',
'in_orientations': 'tabletop_orientation_data',
'out_remaining_poses': 'tabletop_detect_poses',
'out_remaining_orientations': 'tabletop_orientation_data'},
transitions={'remaining': 'TABLETOPS_TSP_ROUTE',
'empty': 'CHECK_IF_SENSING_ROUTE_EMPTY'})
@smach.cb_interface(input_keys=['in_route'], output_keys=['out_remaining_nodes'],
outcomes=['remaining', 'empty'])
def check_sensing_route_emptiness(userdata):
if userdata.in_route:
# We get the remaining nodes of the route
# and then we'll recalculate the route through that remaining nodes
userdata.out_remaining_nodes = userdata.in_route # The route itself has the nodes ordered.
return 'remaining'
return 'empty'
StateMachine.add('CHECK_IF_SENSING_ROUTE_EMPTY',
CBState(check_sensing_route_emptiness,
input_keys=['in_route'],
output_keys=['out_remaining_nodes'],
outcomes=[succeeded, 'empty']),
remapping={'in_route': 'sensing_route',
'out_remaining_nodes': 'sens_locations'},
transitions={'remaining': 'SENSING_LOCATIONS_TSP_ROUTE',
'empty': 'CALCULATE_TTOP_SENSING_LOCATIONS'})
StateMachine.add('CALCULATE_TTOP_SENSING_LOCATIONS', CalculateSensingLocations(),
remapping={'in_room_name': 'in_room_name', 'out_guards': 'sens_locations'},
transitions={succeeded: 'SENSING_LOCATIONS_TSP_ROUTE', 'no_corner_info': aborted})
StateMachine.add('SENSING_LOCATIONS_TSP_ROUTE', TSPState(HC=HC),
remapping={'in_nodes': 'sens_locations', 'out_route': 'sensing_route'},
transitions={succeeded: 'GET_NEXT_SENSING_LOCATION'})
@smach.cb_interface(input_keys=['in_sensing_route'], output_keys=['out_sensing_route', 'out_next_sensing_pose'],
outcomes=[succeeded, 'empty_route'])
def get_next_loc_from_route(userdata):
# Returns the next sensing location to go and removes it from the route
if not userdata.in_sensing_route: # The sensing route is empty...
return 'empty_route'
next_pos = userdata.in_sensing_route[0]
userdata.out_sensing_route = userdata.in_sensing_route[1:]
pose = Pose()
pose.position.x = next_pos[0]
pose.position.y = next_pos[1]
pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.57)) # FIXME is it possible to avoid a fixed orientation?
userdata.out_next_sensing_pose = pose
return succeeded
StateMachine.add('GET_NEXT_SENSING_LOCATION',
CBState(get_next_loc_from_route,
input_keys=['in_sensing_route'],
output_keys=['out_sensing_route', 'out_next_sensing_pose'],
outcomes=[succeeded, 'empty_route']),
remapping={'out_sensing_route': 'sensing_route',
'in_sensing_route': 'sensing_route',
'out_next_sensing_pose': 'next_sensing_pose'},
transitions={succeeded: 'MOVE_TO_SENSING_LOCATION',
'empty_route': 'CALCULATE_TTOP_SENSING_LOCATIONS'})
StateMachine.add('MOVE_TO_SENSING_LOCATION',
MoveActionState("/map", goal_key='next_sensing_pose'),
transitions={succeeded: 'DETECT_TABLETOPS_AND_ANNOUNCE',
aborted: 'GET_NEXT_SENSING_LOCATION'}) # FIXME if aborted we get the next pose.
tell_and_search = Concurrence(outcomes=[succeeded, aborted, preempted, 'no_tables'],
default_outcome=aborted,
output_keys=['tabletop_detect_poses', 'tabletop_orientation_data'],
outcome_map={succeeded: {'ANNOUNCE_SEARCHING': succeeded,
'DETECT_TABLETOPS_ZONE': succeeded},
'no_tables': {'DETECT_TABLETOPS_ZONE': 'no_tables'}})
with tell_and_search:
searching_tables_pool = ["I'm looking for places that can contain objects.",
"I'm searching a place that can have objects.",
"I'm searching the best place to find objects."]
Concurrence.add('ANNOUNCE_SEARCHING',
SpeakActionFromPoolStateMachine(searching_tables_pool))
Concurrence.add('DETECT_TABLETOPS_ZONE',
DetectTablesOfZoneSM(distance_treshold=DIST_BETWEEN_TABLES, dist_to_table=DIST_TO_TABLE),
remapping={'out_table_pose_list': 'tabletop_detect_poses',
'out_table_orientation_list': 'tabletop_orientation_data'})
StateMachine.add('DETECT_TABLETOPS_AND_ANNOUNCE', tell_and_search,
transitions={succeeded: 'TABLETOPS_TSP_ROUTE', 'no_tables': 'GET_NEXT_SENSING_LOCATION'},
remapping={'tabletop_orientation_data': 'tabletop_orientation_data',
'tabletop_detect_poses': 'tabletop_detect_poses'})
# @smach.cb_interface(input_keys=['in_tabletop_detect_data'], output_keys=['out_table_node_list'],
# outcomes=[succeeded])
# def check_ttop_data(userdata):
# #FIXME -> May be necessary to filter in some way the already visited tables...
# #tabletop_detect_data is a list of tuples (pos, orientation), so we get only the poses for the TSP
# userdata.out_table_node_list = reduce(lambda acc, x: acc + [x[0]], userdata.in_tabletop_detect_data, [])
# return succeeded
# StateMachine.add('CHECK_TABLETOP_DATA',
# CBState(check_ttop_data, input_keys=['in_tabletop_detect_data'],
# output_keys=['out_object_detection_pose'],
# outcomes=[succeeded]),
# remapping={'in_tabletop_detect_data': 'tabletop_detect_data',
# 'out_table_node_list': 'table_node_list'},
# transitions={succeeded: 'TELL_GO_RECOGNIZE'})
StateMachine.add('TABLETOPS_TSP_ROUTE', TSPState(HC=HC, indices=True),
remapping={'in_nodes': 'tabletop_detect_poses',
'out_route': 'tabletop_route'},
transitions={succeeded: 'GET_NEXT_TABLETOP_LOCATION'})
@smach.cb_interface(input_keys=['in_table_route', 'in_orient_list', 'in_pose_list'],
output_keys=['out_table_route', 'out_object_detection_pose'],
outcomes=[succeeded, 'empty_route'])
def get_next_loc_from_table_route(userdata):
# Returns the next tabletop location to go and removes it from the route
if not userdata.in_table_route: # The route is empty, so we recalculate the sensing route and go again
return 'empty_route'
node_index = userdata.in_table_route[0]
print('node_index: %d, length in_table_route: %d' % (node_index, len(userdata.in_table_route)))
next_pos = userdata.in_pose_list[node_index]
# Only the index is removed from the list, to preserve the index order of the other elements
userdata.out_table_route = userdata.in_table_route[1:]
pose = Pose()
pose.position.x = next_pos[0]
pose.position.y = next_pos[1]
pose.orientation = userdata.in_orient_list[node_index]
userdata.out_object_detection_pose = pose
return succeeded
StateMachine.add('GET_NEXT_TABLETOP_LOCATION',
CBState(get_next_loc_from_table_route,
input_keys=['in_table_route', 'in_orient_list', 'in_pose_list'],
output_keys=['out_table_route', 'out_object_detection_pose'],
outcomes=[succeeded, 'empty_route']),
remapping={'out_table_route': 'tabletop_route',
'in_table_route': 'tabletop_route',
'in_orient_list': 'tabletop_orientation_data',
'in_pose_list': 'tabletop_detect_poses',
'out_object_detection_pose': 'object_detection_pose'},
transitions={succeeded: 'TELL_GO_RECOGNIZE',
'empty_route': 'SENSING_LOCATIONS_TSP_ROUTE'})
StateMachine.add('TELL_GO_RECOGNIZE', TellGoRecognizeSM(msg_pool=going_to_pool, arg_key=None),
remapping={'out_objects_data': 'objects_data',
'in_target_object': target_object_key,
'in_location_pose_in_map': 'object_detection_pose'},
transitions={'move_failed': 'GET_NEXT_SENSING_LOCATION',
'no_object_found': 'CHECK_IF_OBJECT_FOUND',
succeeded: 'CHECK_IF_OBJECT_FOUND',
aborted: aborted}) # FIXME aborted should abort everything?
StateMachine.add('CHECK_IF_OBJECT_FOUND',
CheckObjectAndRemoveFromList(target_frame),
remapping={'out_object_found': 'out_object_found',
'in_objects_data': 'objects_data',
'out_objects_data': 'objects_data',
'in_target_object': target_object_key},
transitions={succeeded: 'PREPARE_POOL_ARGS', aborted: 'TELL_NO_OBJECT_FOUND'})
StateMachine.add('TELL_NO_OBJECT_FOUND',
SpeakActionFromPoolStateMachine(object_not_found_pool, arg_key=None),
remapping={},
transitions={succeeded: 'GET_NEXT_SENSING_LOCATION', aborted: 'GET_NEXT_SENSING_LOCATION'})
@smach.cb_interface(input_keys=['in_first_object', 'in_location_name'], output_keys=['out_tell_arg'],
outcomes=[succeeded])
def prepare_userdata(userdata):
userdata.out_tell_arg = userdata.in_first_object.name
return succeeded
StateMachine.add('PREPARE_POOL_ARGS',
CBState(prepare_userdata,
input_keys=['in_first_object', 'in_location_name'],
output_keys=['out_tell_arg'], outcomes=[succeeded]),
remapping={'in_first_object': 'out_object_found',
'out_tell_arg': 'tell_arg'},
transitions={succeeded: 'TELL_OBJECT_RECOGNIZED'})
StateMachine.add('TELL_OBJECT_RECOGNIZED',
SpeakActionFromPoolStateMachine(object_found_pool, arg_key="tell_arg"),
remapping={'tell_arg': 'tell_arg'},
transitions={succeeded: succeeded, aborted: aborted})
def __init__(self):
rospy.init_node('find_treasure', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# How long do we have to get to each waypoint?
self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds
# Initialize the patrol counter
self.patrol_count = 0
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move_base action server")
# 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()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
# -0.163200, 0.044660, 0.193186
self.waypoints.append(Pose(Point(0.9579, 1.8710, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(0.7555, 0.0692, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(-0.72511, 0.4952, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(0.167730, 2.18168, 0.0), quaternions[2]))
position_locations = list()
position_locations.append(('position1', self.waypoints[0]))
position_locations.append(('position2', self.waypoints[1]))
position_locations.append(('position3', self.waypoints[2]))
position_locations.append(('position4', self.waypoints[3]))
self.position_locations = OrderedDict(position_locations)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
rospy.loginfo("Starting Tasks")
# Initialize a number of parameters and variables
# setup_task_environment(self)
RVizUtils.get_instance().init_primary_waypoint_markers(self.waypoints)
''' Create individual state machines for assigning tasks to each position '''
# Initialize the overall state machine
self.sm_find_treasure = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Build the find treasure state machine from the nav states and treasure finding states
with self.sm_find_treasure:
# First State Machine
sm_nav = StateMachine(outcomes=['succeeded','aborted','preempted'])
sm_nav.userdata.waypoints = self.waypoints
sm_nav.userdata.waypoints_primary_count = len(self.waypoints)
rospy.loginfo(sm_nav.userdata.waypoints)
with sm_nav:
# StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'succeeded':'NAV_WAYPOINT','aborted':'','preempted':''},
# remapping={'waypoint_out':'patrol_waypoint',
# 'waypoints_primary_count':'waypoints_primary_count'})
StateMachine.add('NAV_WAYPOINT', NavState(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'',
'preempted':''},
remapping={'waypoint_in':'patrol_waypoint',
'waypoints_primary_count':'waypoints_primary_count'})
# Second State Machine
sm_read_tags = StateMachine(outcomes=['valid','invalid','preempted'])
sm_read_tags.userdata.waypoints = self.waypoints
with sm_read_tags:
StateMachine.add('read_tag', ReadTagState('ar_pose_marker', AlvarMarkers),
transitions={'invalid':'read_tag', 'valid':'read_tag', 'preempted':''})
# Third State Machine
sm_detect_faces = StateMachine(outcomes=['valid','invalid','preempted'])
with sm_detect_faces:
StateMachine.add('detect_face', FaceDetectState("/camera/rgb/image_color", Image),
transitions={'invalid':'detect_face', 'valid':'detect_face', 'preempted':''})
# Forth State Machine
sm_recognize_faces = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_recognize_faces:
StateMachine.add('recognize_face', FaceRecognitionState(), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
# goal = FaceRecognitionGoal()
# goal.order_id = 1
# goal.order_argument = ''
# StateMachine.add('recognize_face', SimpleActionState('face_recognition',
# FaceRecognitionAction, goal=goal), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
sm_con = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],default_outcome='succeeded',
child_termination_cb=self.child_term_cb, outcome_cb=self.out_cb)
with sm_con:
Concurrence.add('SM_NAV', sm_nav)
Concurrence.add('SM_READ_TAGS', sm_read_tags)
# Concurrence.add('SM_DETECT_FACES', sm_detect_faces)
Concurrence.add('SM_RECOGNIZE_FACES', sm_recognize_faces)
sm_estimate_position = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_estimate_position:
StateMachine.add('estimate_pose', PoseEstimation('ar_pose_marker', AlvarMarkers),
transitions={'succeeded':'', 'aborted':'estimate_pose', 'preempted':''})
StateMachine.add('POSE_ESTIMATE', sm_estimate_position, transitions={'succeeded':'CON','aborted':'CON','preempted':'CON'})
StateMachine.add('CON',sm_con, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server sm_find_treasure
intro_server = IntrospectionServer('find_treasure', self.sm_find_treasure, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_find_treasure.execute()
rospy.loginfo('the length now is')
rospy.loginfo(self.waypoints)
intro_server.stop()
def __init__(self):
rospy.init_node('random_patrol', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
#setup_task_environment(self)
# 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]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Set the userdata.waypoints variable to the pre-defined waypoints
self.sm_patrol.userdata.waypoints = self.waypoints
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
transitions={'succeeded':'NAV_WAYPOINT','aborted':'NAV_WAYPOINT'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'',
'aborted':''},
remapping={'waypoint_in':'patrol_waypoint'})
# Create the nav_patrol state machine using a Concurrence container
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_NAV', self.sm_patrol)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
#Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# 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('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# 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 main():
rospy.init_node('smach_usecase_step_05')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'DRAW_SHAPES'})
# Draw some polygons
shapes_cc = Concurrence(
outcomes=['succeeded','aborted','preempted'],
default_outcome='aborted',
outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
StateMachine.add('DRAW_SHAPES',shapes_cc)
with shapes_cc:
# Draw a large polygon with the first turtle
Concurrence.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big))
# Draw a small polygon with the second turtle
Concurrence.add('SMALL',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self):
rospy.init_node('petit_smach_ai', 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]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
# State machine for Action1
self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action1.userdata.speed = 0.1;
self.sm_action1.userdata.distance = 1.0;
with self.sm_action1:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'FORWARD',
'aborted':'aborted'})
StateMachine.add('FORWARD', MoveForward(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action2
self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action2.userdata.speed = -0.1;
self.sm_action2.userdata.distance = 0.5;
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'SIDE',
'aborted':'aborted'})
StateMachine.add('SIDE', MoveSide(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action3
self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action4
self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
with self.sm_action4:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action5
self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
with self.sm_action5:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action6
self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
with self.sm_action6:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
self.sm_actions.userdata.waypoints = self.waypoints
with self.sm_actions:
StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb,
output_keys=['waypoint_out'],
outcomes=['action1','action2','action3','action4','action5','action6','aborted','succeeded','preempted']),
transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
remapping={'waypoint_out':'patrol_waypoint'})
#StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
# remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'},
remapping={'waypoint_in':'remove_waypoint'})
# State machine with concurrence
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_ACTIONS', self.sm_actions)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# 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('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# 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 __init__(self):
rospy.init_node('explain_history_concurrence', anonymous=False)
# 设置关闭机器人函数(stop the robot)
rospy.on_shutdown(self.shutdown)
# 初始化一些参数和变量
setup_task_environment(self)
# 跟踪到达目标位置的成功率
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# 保存上一个或者当前的导航目标点的变量
self.last_nav_state = None
# 指示是否正在充电的标志
self.recharging = False
# 保存导航目标点的列表
nav_states = {}
# 把waypoints变成状态机的状态
for waypoint in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[waypoint]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
# nav_states.append(move_base_state)
nav_states[waypoint] = move_base_state
# 为扩展底座（docking station）创建一个MoveBaseAction state
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(10.0))
# 为written words子任务创建一个状态机
sm_written_words = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_written_words:
StateMachine.add('EXPLAIN_HISTORY',
WrittenWords('written_words', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为rule子任务创建一个状态机
sm_rule = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_rule:
StateMachine.add('EXPLAIN_HISTORY',
Rule('rule', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为life子任务创建一个状态机
sm_life = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_life:
StateMachine.add('EXPLAIN_HISTORY',
Life('life', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为faith子任务创建一个状态机
sm_faith = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_faith:
StateMachine.add('EXPLAIN_HISTORY',
Faith('faith', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 初始化导航的状态机
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 使用transitions将导航的状态添加到状态机
with self.sm_nav:
StateMachine.add('START',
nav_states['explanatory_text'],
transitions={
'succeeded': 'WRITTEN_WORDS',
'aborted': 'WRITTEN_WORDS',
'preempted': 'WRITTEN_WORDS'
})
''' Add the living room subtask(s) '''
StateMachine.add('WRITTEN_WORDS',
nav_states['explanatory_text'],
transitions={
'succeeded': 'WRITTEN_WORDS_TASKS',
'aborted': 'RULE',
'preempted': 'RULE'
})
# 当任务完成时, 继续进行kitchen任务
StateMachine.add('WRITTEN_WORDS_TASKS',
sm_written_words,
transitions={
'succeeded': 'RULE',
'aborted': 'RULE',
'preempted': 'RULE'
})
''' Add the kitchen subtask(s) '''
StateMachine.add('RULE',
nav_states['explain_the_rule'],
transitions={
'succeeded': 'RULE_TASKS',
'aborted': 'LIFE',
'preempted': 'LIFE'
})
# 当任务完成时, 继续进行bathroom任务
StateMachine.add('RULE_TASKS',
sm_rule,
transitions={
'succeeded': 'LIFE',
'aborted': 'LIFE',
'preempted': 'LIFE'
})
''' Add the bathroom subtask(s) '''
StateMachine.add('LIFE',
nav_states['explain_life'],
transitions={
'succeeded': 'LIFE_TASKS',
'aborted': 'FAITH',
'preempted': 'FAITH'
})
# 当任务完成时, 继续进行hallway任务
StateMachine.add('LIFE_TASKS',
sm_life,
transitions={
'succeeded': 'FAITH',
'aborted': 'FAITH',
'preempted': 'FAITH'
})
''' Add the hallway subtask(s) '''
StateMachine.add('FAITH',
nav_states['explain_faith'],
transitions={
'succeeded': 'FAITH_TASKS',
'aborted': '',
'preempted': ''
})
# 当任务完成时, stop
StateMachine.add('FAITH_TASKS',
sm_faith,
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 在sm_nav状态机中注册一个回调函数以启动状态转换（state transitions）
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# 初始化充电的状态机
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION',
nav_docking_station,
transitions={'succeeded': 'RECHARGE_BATTERY'})
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': ''})
# 使用并发容器（Concurrence container）创建nav_patrol状态机
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("battery_level", Float32, self.battery_cb))
# 创建顶层状态机
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 将nav_patrol,sm_recharge和Stop添加到sm_top状态机
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
# 创建并开始SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# 运行状态机
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
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)
def __init__(self):
rospy.init_node('deliver_food', anonymous=False)
self.initialize_destination()
self.lalala = 100
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
rospy.loginfo(room + " -> [" + str(round(self.room_locations[room].position.x,2)) + ", " + str(round(self.room_locations[room].position.y,2)) + "]" )
sm_rotate_search = Concurrence(outcomes=['find', 'not_find'],
default_outcome='not_find',
child_termination_cb=self.concurrence_child_termination_callback,
outcome_cb=self.concurrence_outcome_callback)
with sm_rotate_search:
Concurrence.add('ROTATE', Rotate360() )
Concurrence.add('SEARCH', SearchTable() )
sm_table1 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table1:
StateMachine.add('GOTO_TABLE1', nav_states['table1'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table2 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table2:
StateMachine.add('GOTO_TABLE2', nav_states['table2'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table3 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table3:
StateMachine.add('GOTO_TABLE3', nav_states['table3'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table4 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table4:
StateMachine.add('GOTO_TABLE4', nav_states['table4'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table5 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table5:
StateMachine.add('GOTO_TABLE5', nav_states['table5'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table6 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table6:
StateMachine.add('GOTO_TABLE6', nav_states['table6'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
# let's initialize the overall state machine
sm_deliverfood = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_deliverfood:
StateMachine.add('COMPUTER_VISION_TASK', ComputerVision(), transitions={'detect1':'TABLE_ONE_TASK',
'detect2':'TABLE_TWO_TASK',
'detect3':'TABLE_THREE_TASK',
'detect4':'TABLE_FOUR_TASK',
'detect5':'TABLE_FIVE_TASK',
'detect6':'TABLE_SIX_TASK'})
StateMachine.add('TABLE_ONE_TASK',sm_table1, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_TWO_TASK',sm_table2, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_THREE_TASK',sm_table3, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_FOUR_TASK',sm_table4, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_FIVE_TASK',sm_table5, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_SIX_TASK',sm_table6, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('deliver_food', sm_deliverfood, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_deliverfood.execute()
rospy.on_shutdown(self.shutdown)
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 main():
rospy.init_node('smach_usecase_step_06')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'DRAW_SHAPES'})
# Draw some polygons
shapes_cc = Concurrence(
outcomes=['succeeded','aborted','preempted'],
default_outcome='aborted',
outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
StateMachine.add('DRAW_SHAPES',shapes_cc)
with shapes_cc:
# Draw a large polygon with the first turtle
Concurrence.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big))
# Draw a small polygon with the second turtle
small_shape_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
Concurrence.add('SMALL',small_shape_sm)
with small_shape_sm:
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'DRAW_WITH_MONITOR'})
# Construct a concurrence for the shape action and the monitor
draw_monitor_cc = Concurrence(
['succeeded','aborted','preempted','interrupted'],
'aborted',
child_termination_cb = lambda so: True,
outcome_map = {
'succeeded':{'DRAW':'succeeded'},
'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
'interrupted':{'MONITOR':'invalid'}})
StateMachine.add('DRAW_WITH_MONITOR',
draw_monitor_cc,
{'interrupted':'WAIT_FOR_CLEAR'})
with draw_monitor_cc:
Concurrence.add('DRAW',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
def turtle_far_away(ud, msg):
"""Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
return True
return False
Concurrence.add('MONITOR',
MonitorState('/turtle1/pose',turtlesim.msg.Pose,
cond_cb = turtle_far_away))
StateMachine.add('WAIT_FOR_CLEAR',
MonitorState('/turtle1/pose',turtlesim.msg.Pose,
cond_cb = lambda ud,msg: not turtle_far_away(ud,msg)),
{'valid':'WAIT_FOR_CLEAR','invalid':'TELEPORT2'})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
# Initialize the navigation state machine
self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':''})
#StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb),
#transitions={'succeeded':''})
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb))
# 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('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'})
StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# 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 __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
location_goal = MoveBaseGoal()
result_goal = MoveBaseGoal()
#recognize_goal = VisionGoal()
#flag = 1
class wait(State):
def __init__(self):
State.__init__(self, outcomes=['succeeded', 'aborted'])
def execute(self, userdata):
if flag == 0:
rospy.loginfo('waitting')
return 'aborted'
else:
return 'succeeded'
class MoveItDemo(State):
# spin() simply keeps python from exiting until this node is stopped
def __init__(self):
# Initialize the move_group API
State.__init__(self, outcomes=['succeeded', 'aborted'])
def execute(self, userdata):
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_ik')
#rospy.Subscriber("chatter", Pose, callback)
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.11)
right_arm.set_goal_orientation_tolerance(0.15)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_init')
right_arm.go()
rospy.sleep(2)
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
#global a,b,c,d,e,f,g
target_pose.pose.position.x = 0.3
target_pose.pose.position.y = -0.45
target_pose.pose.position.z = 1.35
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
#Set the start state to the current state
right_arm.set_start_state_to_current_state()
#print a
# Set the goal pose of the end effector to the stored pose
right_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a second
rospy.sleep(5)
#right_arm.set_named_target('right_arm_init')
right_arm.go()
return 'succeeded'
class MoveItDemo1(State):
# spin() simply keeps python from exiting until this node is stopped
def __init__(self):
# Initialize the move_group API
State.__init__(self, outcomes=['succeeded', 'aborted'])
def execute(self, userdata):
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_ik')
#rospy.Subscriber("chatter", Pose, callback)
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
right_arm.set_named_target('right_arm_init')
right_arm.go()
return 'succeeded'
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(20.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(10.0))
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 0.1
pose_target.position.x = 0.7
pose_target.position.y = -0.0
pose_target.position.z = 1.1
result_goal.target_pose.header.frame_id = 'map'
result_goal.target_pose.pose = (Pose(Point(0.5, 1.5, 0.0),
Quaternion(0.0, 0.0, 0.0, 1.0)))
# Initialize the navigation state machine
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('WAITTING',
wait(),
transitions={
'succeeded': 'NAV_STATE_0',
'aborted': 'WAITTING'
})
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
SimpleActionState('move_base',
MoveBaseAction,
goal=location_goal),
transitions={
'succeeded': 'ARM',
'aborted': 'ARM'
})
#StateMachine.add('VISION', SimpleActionState('drv_action', VisionAction, goal=recognize_goal),
#transitions={'succeeded': 'ARM', 'aborted': 'ARM'})
StateMachine.add('ARM',
MoveItDemo(),
transitions={
'succeeded': 'NAV_STATE_2',
'aborted': 'NAV_STATE_2'
})
StateMachine.add('NAV_STATE_2',
nav_states[0],
transitions={
'succeeded': 'ARM1',
'aborted': 'ARM1'
})
StateMachine.add('ARM1',
MoveItDemo1(),
transitions={
'succeeded': '',
'aborted': ''
})
# StateMachine.add('NAV_STATE_2', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']),
# transitions={'succeeded': 'NAV_STATE_0'}, remapping={'target_pose': 'user_data'})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION',
nav_docking_station,
transitions={'succeeded': 'RECHARGE_BATTERY'})
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': ''})
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
# Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb))
Concurrence.add(
'LOCATION_GOAL',
MonitorState("nav_location_goal", Pose,
self.nav_location_goal_cb))
Concurrence.add(
'RECOGNIZE_GOAL',
MonitorState("/comm/msg/control/recognize_goal", String,
self.recognize_goal_cb))
#Concurrence.add('RESULT', MonitorState("/drv_action/result", VisionActionResult, self.result_goal_cb))
# 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('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
rospy.loginfo('=============ce shi=============')
time.sleep(5)
# 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 __init__(self, driver=None):
StateMachine.__init__(self, outcomes=['succeeded',
'preempted', 'aborted'])
self.userdata.text = ''
with self:
StateMachine.add('PREPARE',
HomeOn_SM('Sit'),
transitions={'succeeded': 'INIT'})
#transitions={'succeeded': 'GAME'}) #DEBUGGING LINE
#PREPARE NAO
setup = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with setup:
StateMachine.add('CLEAN_LISTEN_STATE',
StopRecognitionState(),
transitions={'succeeded': 'SET_VOCABULARY'}
)
StateMachine.add('SET_VOCABULARY',
SetSpeechVocabularyState(no_words + yes_words + probably_not_words + dunno_words + maybe_words),
transitions={'succeeded': 'RESET_AKINATOR'}
)
#RESET AKINATOR WEB
StateMachine.add('RESET_AKINATOR',
ServiceState('/reset_akinator_params',
ResetAkinator,
request=ResetAkinatorRequest('Name', 22)),
transitions={'succeeded': 'succeeded'}
)
cc = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='aborted',
outcome_map={'succeeded': {'START_GAME_INTRO': 'succeeded', 'SETUP': 'succeeded'}})
with cc:
PresentationPool = ['CIR_Presentation1', 'CIR_Presentation2']
# INTRODUCTION OF THE GAME
Concurrence.add('START_GAME_INTRO',
ExecuteBehaviorFromPoolSM(behavior_pool=PresentationPool))
# NAO SETUP
Concurrence.add('SETUP', setup)
StateMachine.add('INIT', cc,
transitions={'succeeded': 'GAME', 'aborted': 'LOSE'})
#GAME LOOP
StateMachine.add('GAME',
AkinatorGame(),
transitions={'succeeded': 'CHAR_QUESTION_MAKE', 'aborted': 'LOSE'}
)
#END OF GAME
StateMachine.add('CHAR_QUESTION_MAKE',
CBState(self.compoundQuestion,
input_keys=['text'],
output_keys=['text'],
outcomes=['succeeded', 'aborted']),
transitions={'succeeded': 'CHAR_CORRECT'}
)
StateMachine.add('CHAR_CORRECT',
SpeechGesture(behavior_pool=['CIR_Asking1', 'CIR_Asking2', 'CIR_Asking3', 'CIR_Asking4', 'CIR_Asking5', 'CIR_Asking6']),
transitions={'succeeded': 'FINAL_ANSWER'}
)
StateMachine.add('FINAL_ANSWER',
SpeechRecognitionAndGesture(),
transitions={'succeeded': 'CHECK_WIN_LOSE', 'aborted': 'LOSE'}
)
def check_w_l(userdata):
if (userdata.text == 'yes'):
return 'win'
return 'lose'
StateMachine.add('CHECK_WIN_LOSE',
CBState(check_w_l, input_keys=['text'], outcomes=['win', 'lose']),
transitions={'win': 'WIN', 'lose': 'LOSE'})
StateMachine.add('WIN',
ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Winning1', 'CIR_Winning2', 'CIR_Winning3', 'CIR_Winning4', 'CIR_Winning5']),
transitions={'succeeded': 'DISABLE_STIFF'})
StateMachine.add('LOSE',
#ExecuteBehavior(behavior_name='CIR_Losing1'),
ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Loosing1', 'CIR_Loosing2', 'CIR_Loosing3', 'CIR_Loosing4']),
transitions={'succeeded': 'DISABLE_STIFF'})
StateMachine.add('DISABLE_STIFF',
DisableStiffnessState(),
transitions={'succeeded': 'succeeded'})
def __init__(self):
rospy.init_node('carry_smach_move', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
global recharge_position
global main_room_position
global corridor_position
global kitchen_position
global bed_room_position
self.recharge_position = recharge_position
self.main_room_position = main_room_position
self.corridor_position = corridor_position
self.kitchen_position = kitchen_position
self.bed_room_position = bed_room_position
#StateMachine.add('NAV_S2K_END', Nav2Waypoint(self.waypoints[6]),
# transitions={'succeeded':'IN_KITCHEN',
# 'aborted':'aborted'})
#!!!!!!!!!!!!!!!!!!!!!!!!!!!
# NEW SM !!!!!!!!!!!!!!!!!!!
#!!!!!!!!!!!!!!!!!!!!!!!!!!!
##################
# IN CHARGE ROOM
##################
# Concurrent State machine
self.sm_in_charge_room_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_recharge','go_main_room'],
output_keys=['sm_output'],
default_outcome='succeeded',
child_termination_cb=self.in_charge_room_wait_input_child_termination_cb,
outcome_cb=self.in_charge_room_wait_input_outcome_cb)
with self.sm_in_charge_room_wait_input:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))
# Create state machine
self.sm_in_charge_room = StateMachine(outcomes=['succeeded', 'go_main_room', 'preempted'],
input_keys=['sm_input'],
output_keys=['sm_output'])
# Add
with self.sm_in_charge_room:
StateMachine.add('CHECK_DESTINATION', CheckDestination(),
transitions={'succeeded':'WAIT_INPUT',
'aborted':'WAIT_INPUT',
'go_recharge':'STOP_NAV',
'go_main_room':'WAIT_INPUT',
'go_corridor':'WAIT_INPUT',
'go_kitchen':'WAIT_INPUT',
'go_bed_room':'WAIT_INPUT'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('WAIT_INPUT', self.sm_in_charge_room_wait_input, transitions={'succeeded':'succeeded',
'aborted':'succeeded',
'go_recharge':'STOP_NAV',
'go_main_room':'go_main_room'},
remapping={'sm_output':'sm_output'})
StateMachine.add('TEST', Pause2(),
transitions={'succeeded':'go_main_room',
'aborted':'succeeded'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('STOP_NAV', PauseNav(), transitions={'succeeded':'GO_BACK'})
StateMachine.add('GO_BACK', MoveBack(), transitions={'succeeded':'PAUSE_BACK'})
StateMachine.add('PAUSE_BACK', Pause(), transitions={'succeeded':'STOP_BACK'})
StateMachine.add('STOP_BACK', Stop(), transitions={'succeeded':'PAUSE_STOP'})
StateMachine.add('PAUSE_STOP', Pause(), transitions={'succeeded':'CHARGING'})
StateMachine.add('CHARGING', Charging(), transitions={'succeeded':'GO_FRONT'})
StateMachine.add('GO_FRONT', MoveFront(), transitions={'succeeded':'PAUSE_FRONT'})
StateMachine.add('PAUSE_FRONT', Pause(), transitions={'succeeded':'STOP_FRONT'})
StateMachine.add('STOP_FRONT', Stop(), transitions={'succeeded':'START_NAV'})
StateMachine.add('START_NAV', ResumeNav(), transitions={'succeeded':'WAIT_INPUT'})
##################
# IN MAIN ROOM
##################
# Concurrent State machine
self.sm_in_main_room_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_recharge','go_corridor'],
output_keys=['sm_output'],
default_outcome='succeeded',
child_termination_cb=self.in_main_room_wait_input_child_termination_cb,
outcome_cb=self.in_main_room_wait_input_outcome_cb)
with self.sm_in_main_room_wait_input:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))
# Create state machine
self.sm_in_main_room = StateMachine(outcomes=['succeeded', 'go_charge_room', 'go_corridor', 'preempted'],
input_keys=['sm_input'],
output_keys=['sm_output'])
# Add
with self.sm_in_main_room:
StateMachine.add('CHECK_DESTINATION', CheckDestination(),
transitions={'succeeded':'succeeded',
'aborted':'succeeded',
'go_recharge':'go_charge_room',
'go_main_room':'WAIT_INPUT',
'go_corridor':'go_corridor',
'go_kitchen':'go_corridor',
'go_bed_room':'go_corridor'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('TEST', Pause2(),
transitions={'succeeded':'go_corridor',
'aborted':'succeeded'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('WAIT_INPUT', self.sm_in_main_room_wait_input, transitions={'succeeded':'WAIT_INPUT',
'aborted':'WAIT_INPUT',
'go_recharge':'go_charge_room',
'go_corridor':'go_corridor'},
remapping={'sm_output':'sm_output'})
##################
# IN CORRIDOR
##################
# Concurrent State machine
self.sm_in_corridor_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_kitchen','go_bed_room'],
output_keys=['sm_output'],
default_outcome='succeeded',
child_termination_cb=self.in_corridor_wait_input_child_termination_cb,
outcome_cb=self.in_corridor_wait_input_outcome_cb)
with self.sm_in_corridor_wait_input:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))
# Create state machine
self.sm_in_corridor = StateMachine(outcomes=['succeeded', 'go_main_room', 'go_kitchen', 'go_bed_room', 'preempted'],
input_keys=['sm_input'],
output_keys=['sm_output'])
# Add
with self.sm_in_corridor:
StateMachine.add('CHECK_DESTINATION', CheckDestination(),
transitions={'succeeded':'succeeded',
'aborted':'succeeded',
'go_recharge':'go_main_room',
'go_main_room':'go_main_room',
'go_corridor':'WAIT_INPUT',
'go_kitchen':'go_kitchen',
'go_bed_room':'go_bed_room'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('TEST', Pause2(),
transitions={'succeeded':'go_kitchen',
'aborted':'succeeded'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('WAIT_INPUT', self.sm_in_corridor_wait_input, transitions={'succeeded':'WAIT_INPUT',
'aborted':'WAIT_INPUT',
'go_main_room':'go_main_room',
'go_kitchen':'go_kitchen',
'go_bed_room':'go_bed_room'},
remapping={'sm_output':'sm_output'})
##################
# IN KITCHEN
##################
# Concurrent State machine
self.sm_in_kitchen_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_corridor'],
output_keys=['sm_output'],
default_outcome='succeeded',
child_termination_cb=self.in_kitchen_wait_input_child_termination_cb,
outcome_cb=self.in_kitchen_wait_input_outcome_cb)
with self.sm_in_kitchen_wait_input:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))
# Create state machine
self.sm_in_kitchen = StateMachine(outcomes=['succeeded', 'go_corridor', 'preempted'],
input_keys=['sm_input'],
output_keys=['sm_output'])
# Add
with self.sm_in_kitchen:
StateMachine.add('WAIT_INPUT', self.sm_in_kitchen_wait_input, transitions={'succeeded':'WAIT_INPUT',
'aborted':'WAIT_INPUT',
'go_corridor':'go_corridor'},
remapping={'sm_output':'sm_output'})
StateMachine.add('TEST', Pause2(),
transitions={'succeeded':'go_corridor',
'aborted':'succeeded'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('CHECK_DESTINATION', CheckDestination(),
transitions={'succeeded':'succeeded',
'aborted':'succeeded',
'go_recharge':'go_corridor',
'go_main_room':'go_corridor',
'go_corridor':'go_corridor',
'go_kitchen':'WAIT_INPUT',
'go_bed_room':'go_corridor'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
##################
# IN BED ROOM
##################
# Concurrent State machine
self.sm_in_bed_room_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_corridor'],
output_keys=['sm_output'],
default_outcome='succeeded',
child_termination_cb=self.in_bed_room_wait_input_child_termination_cb,
outcome_cb=self.in_bed_room_wait_input_outcome_cb)
with self.sm_in_bed_room_wait_input:
Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))
# Create state machine
self.sm_in_bed_room = StateMachine(outcomes=['succeeded', 'go_corridor', 'preempted'],
input_keys=['sm_input'],
output_keys=['sm_output'])
# Add
with self.sm_in_bed_room:
StateMachine.add('WAIT_INPUT', self.sm_in_bed_room_wait_input, transitions={'succeeded':'WAIT_INPUT',
'aborted':'WAIT_INPUT',
'go_corridor':'go_corridor'},
remapping={'sm_output':'sm_output'})
StateMachine.add('TEST', Pause2(),
transitions={'succeeded':'go_corridor',
'aborted':'succeeded'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
StateMachine.add('CHECK_DESTINATION', CheckDestination(),
transitions={'succeeded':'succeeded',
'aborted':'succeeded',
'go_recharge':'go_corridor',
'go_main_room':'go_corridor',
'go_corridor':'go_corridor',
'go_kitchen':'go_corridor',
'go_bed_room':'WAIT_INPUT'},
remapping={'waypoint_in':'sm_input',
'waypoint_out':'sm_output'})
##################
# TOP SM
##################
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.goal = self.main_room_position
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('IN_CHARGE_ROOM', self.sm_in_charge_room, transitions={'succeeded':'IN_CHARGE_ROOM',
'go_main_room':'IN_MAIN_ROOM'},
remapping={'sm_input':'goal', 'sm_output':'goal'})
StateMachine.add('IN_MAIN_ROOM', self.sm_in_main_room, transitions={'succeeded':'IN_MAIN_ROOM',
'go_charge_room':'IN_CHARGE_ROOM',
'go_corridor':'IN_CORRIDOR'},
remapping={'sm_input':'goal', 'sm_output':'goal'})
StateMachine.add('IN_CORRIDOR', self.sm_in_corridor, transitions={'succeeded':'IN_CORRIDOR',
'go_main_room':'IN_MAIN_ROOM',
'go_kitchen':'IN_KITCHEN',
'go_bed_room':'IN_BED_ROOM'},
remapping={'sm_input':'goal', 'sm_output':'goal'})
StateMachine.add('IN_KITCHEN', self.sm_in_kitchen, transitions={'succeeded':'IN_KITCHEN',
'go_corridor':'IN_CORRIDOR'},
remapping={'sm_input':'goal', 'sm_output':'goal'})
StateMachine.add('IN_BED_ROOM', self.sm_in_bed_room, transitions={'succeeded':'IN_BED_ROOM',
'go_corridor':'IN_CORRIDOR'},
remapping={'sm_input':'goal', 'sm_output':'goal'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('carry_sm', self.sm_top, '/SM_CARRY_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 main():
rospy.init_node('tinker_mission_manipulation')
trans = tf.TransformListener()
rospy.loginfo("Waiting for tf ...")
rospy.sleep(3)
assert (len(trans.getFrameStrings()) > 0)
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
def kinect_callback(userdata, result):
userdata.objects = []
objects = []
sum_x = 0
for obj in result.objects.objects:
position = obj.pose.pose.pose.position
if position.y > 0.5 or position.y < -0.5:
continue
_levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80]
for l in _levels:
if fabs(l - position.z) < 0.05:
position.z = l + 0.05
if position.z > 1.1:
position.z += 0.05
obj.header.stamp = rospy.Time(0)
kinect_point = PointStamped(header=obj.header, point=position)
odom_point = trans.transformPoint('odom', kinect_point)
sum_x += odom_point.point.x
rospy.loginfo(colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x,
odom_point.point.y, odom_point.point.z)
objects.append(odom_point)
avg_x = sum_x / len(objects)
for from_point in objects:
to_point = copy.deepcopy(from_point)
to_point.point.x = avg_x
to_point.point.z = find_div(from_point.point.z)
userdata.objects.append({'from': from_point, 'to': to_point})
rospy.loginfo(colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x,
to_point.point.y, to_point.point.z)
rospy.loginfo(colored('Total Object: %d','green'), len(objects))
return 'succeeded'
StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect),
transitions={'succeeded': 'Start_Button'})
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition'})
StateMachine.add('S_Kinect_Recognition',
ServiceState(service_name='/kinect_find_objects',
service_spec=FindObjects,
input_keys=['objects'],
output_keys=['objects'],
response_cb=kinect_callback),
transitions={'succeeded': 'Generate_Report'})
StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'),
transitions={'succeeded': 'IT_Objects_Iterator'} )
objects_iterator = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['objects'], output_keys=[],
it=lambda: state.userdata.objects, it_label='target',
exhausted_outcome='succeeded')
with objects_iterator:
fetch_object_sequence = Sequence(outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
input_keys=['target'],
connector_outcome='succeeded')
with fetch_object_sequence:
Sequence.add('Speak', SpeakState('New Object recognized'))
Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'),
transitions={'aborted':'continue'})
concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=['target'])
with concurrence:
Concurrence.add('Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from'))
Concurrence.add('Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x,y: False))
Sequence.add('Move_Fetch_Concurrence', concurrence)
Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE))
Sequence.add('Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from'))
Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to'))
Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to'))
Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'})
Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue'])
# end of objects_iterator
StateMachine.add('IT_Objects_Iterator', objects_iterator,
transitions= {'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset'})
StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()