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 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 __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 __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
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 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 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 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 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 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 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 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 main():
# The autonomy_sm handles the full autonomy sequence for the competition run
autonomy_sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with autonomy_sm:
# Add states for setup (these run once each)
for i in range(len(setup_sequence)):
name, action_type = get_name_and_type(setup_sequence[i])
func_to_run = None
if action_type == 'localize':
func_to_run = localization_cb
elif action_type == 'move':
# action instead of function
func_to_run = 'action'
else:
raise NotImplementedError('Only localize is supported rn')
# if this is the last setup state
if i == len(loop_sequence) - 1:
# tie the last setup state to the first loop state
first_loop_name = get_name_and_type(loop_sequence[0])
if func_to_run == 'action':
action_state = create_action_state(name, action_type)
StateMachine.add(
name,
action_state,
transitions={'succeeded': first_loop_name})
else:
StateMachine.add(name, CBState(func_to_run),
{'succeeded': first_loop_name})
else:
# add_auto adds this to the state machine and automatically
# ties the next and previous states together
if func_to_run == 'action':
action_state = create_action_state(name, action_type)
StateMachine.add_auto(name,
action_state,
connector_outcomes=['succeeded'])
else:
StateMachine.add_auto(name,
CBState(func_to_run),
connector_outcomes=['succeeded'])
names = [] # store names to check and handle dupes
for i in range(len(loop_sequence)):
name, action_type = get_name_and_type(loop_sequence[i])
action_state = create_action_state(name, action_type)
# "move" states all have the same name and smach requires unique
# state names, so check and add a number to the name if needed
if name in names:
name = name + "2"
while name in names:
name = name[:-1] + str(
int(name[-1]) + 1) # increase num by 1
names.append(name)
if i == len(loop_sequence) - 1:
# tie the last state to the first one, so they just keep looping
StateMachine.add(name,
action_state,
transitions={'succeeded': names[0]})
else:
# add_auto adds this to the state machine and automatically
# ties the next and previous states together
StateMachine.add_auto(name,
action_state,
connector_outcomes=['succeeded'])
# Create the concurrence container for the fully autonomy sequence. This
# runs the state machine for the competition run. It also concurrently runs
# a state that listens to the /click_start_button topic. If this is
# triggered place us into the teleop 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, start_btn_cb))
teleop_concurrence = Concurrence(
outcomes=['enter_autonomy', 'stay', 'done'],
default_outcome='enter_autonomy',
child_termination_cb=teleop_child_term_cb,
outcome_cb=teleop_out_cb)
with teleop_concurrence:
Concurrence.add(
'TOGGLE_LISTEN',
MonitorState('/click_start_button', Empty, start_btn_cb))
Concurrence.add(
'EXIT_LISTEN',
MonitorState('/click_select_button', Empty, select_btn_cb))
# Top level state machine, containing the autonomy and teleop machines.
top_sm = StateMachine(outcomes=['DONE'])
with top_sm:
StateMachine.add('TELEOP_MODE',
teleop_concurrence,
transitions={
'enter_autonomy': 'AUTONOMY_MODE',
'stay': 'TELEOP_MODE',
'done': 'DONE'
})
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 __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):
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 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('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('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 __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()
def __init__(self):
rospy.init_node('Pi_Smach', anonymous=False)
rospy.on_shutdown(self.shutdown)
setup_environment(self)
self.last_nav_state = None
self.recharging = False
nav_states = list()
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,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_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,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0))
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_nav.userdata.X_input = 0
self.sm_nav.userdata.Y_input = 0
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': 'INPUTXY',
'aborted': 'INPUTXY'
})
StateMachine.add('INPUTXY',
InputXY(),
transitions={'succeeded': 'GOCHOSEWAY'},
remapping={
'X_output': 'X_input',
'Y_output': 'Y_input'
})
StateMachine.add('GOCHOSEWAY',
ChoseWay(),
transitions={
'succeeded': 'MOVE_ARM',
'aborted': 'MOVE_ARM'
},
remapping={
'X': 'X_input',
'Y': 'Y_input'
})
StateMachine.add('MOVE_ARM',
Move_Arm(),
transitions={'succeeded': 'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2',
nav_states[2],
transitions={
'succeeded': '',
'aborted': ''
})
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': ''})
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)
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'])
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': ''})
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()
def __init__(self):
# init node
rospy.init_node('pool_patrol', anonymous=False)
# Set the shutdown fuction (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initilalize the mission parameters and variables
setup_task_environment(self)
# A variable to hold the last/current mission goal/state
self.last_mission_state = None
# A flag to indicate whether or not we are recharging
self.recharging = False
# Turn the target locations into SMACH MoveBase and LosPathFollowing action states
nav_terminal_states = {}
nav_transit_states = {}
# DP controller
for target in self.pool_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'odom'
nav_goal.target_pose.pose = self.pool_locations[target]
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_terminal_states[target] = move_base_state
# Path following
for target in self.pool_locations.iterkeys():
nav_goal = LosPathFollowingGoal()
#nav_goal.prev_waypoint = navigation.vehicle_pose.position
nav_goal.next_waypoint = self.pool_locations[target].position
nav_goal.forward_speed.linear.x = self.transit_speed
nav_goal.desired_depth.z = self.search_depth
nav_goal.sphereOfAcceptance = self.search_area_size
los_path_state = SimpleActionState(
'los_path',
LosPathFollowingAction,
goal=nav_goal,
result_cb=self.nav_result_cb,
exec_timeout=self.nav_timeout,
server_wait_timeout=rospy.Duration(10.0))
nav_transit_states[target] = los_path_state
""" Create individual state machines for assigning tasks to each target zone """
# Create a state machine container for the orienting towards the gate subtask(s)
self.sm_gate_tasks = StateMachine(outcomes=[
'found', 'unseen', 'missed', 'passed', 'aborted', 'preempted'
])
# Then add the subtask(s)
with self.sm_gate_tasks:
# if gate is found, pass pixel info onto TrackTarget. If gate is not found, look again
StateMachine.add('SCANNING_OBJECTS',
SearchForTarget('gate'),
transitions={
'found': 'CAMERA_CENTERING',
'unseen': 'BROADEN_SEARCH',
'passed': '',
'missed': ''
},
remapping={
'px_output': 'object_px',
'fx_output': 'object_fx',
'search_output': 'object_search',
'search_confidence_output':
'object_confidence'
})
StateMachine.add('CAMERA_CENTERING',
TrackTarget('gate',
self.pool_locations['gate'].position),
transitions={'succeeded': 'SCANNING_OBJECTS'},
remapping={
'px_input': 'object_px',
'fx_input': 'object_fx',
'search_input': 'object_search',
'search_confidence_input': 'object_confidence'
})
StateMachine.add('BROADEN_SEARCH',
TrackTarget('gate',
self.pool_locations['gate'].position),
transitions={'succeeded': 'SCANNING_OBJECTS'},
remapping={
'px_input': 'object_px',
'fx_input': 'object_fx',
'search_input': 'object_search',
'search_confidence_input': 'object_confidence'
})
# Create a state machine container for returning to dock
self.sm_docking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add states to container
with self.sm_docking:
StateMachine.add('RETURN_TO_DOCK',
nav_transit_states['docking'],
transitions={
'succeeded': 'DOCKING_SECTOR',
'aborted': '',
'preempted': 'RETURN_TO_DOCK'
})
StateMachine.add('DOCKING_SECTOR',
ControlMode(POSE_HEADING_HOLD),
transitions={
'succeeded': 'DOCKING_PROCEEDURE',
'aborted': '',
'preempted': ''
})
StateMachine.add('DOCKING_PROCEEDURE',
nav_terminal_states['docking'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" Assemble a Hierarchical State Machine """
# Initialize the HSM
self.sm_mission = StateMachine(outcomes=[
'succeeded', 'aborted', 'preempted', 'passed', 'missed', 'unseen',
'found'
])
# Build the HSM from nav states and target states
with self.sm_mission:
""" Navigate to GATE in TERMINAL mode """
StateMachine.add('TRANSIT_TO_GATE',
nav_transit_states['gate'],
transitions={
'succeeded': 'GATE_SEARCH',
'aborted': 'DOCKING',
'preempted': 'TRANSIT_TO_GATE'
})
""" When in GATE sector"""
StateMachine.add('GATE_SEARCH',
self.sm_gate_tasks,
transitions={
'passed': 'GATE_PASSED',
'missed': 'TRANSIT_TO_GATE',
'aborted': 'DOCKING'
})
""" Transiting to gate """
StateMachine.add('GATE_PASSED',
ControlMode(OPEN_LOOP),
transitions={
'succeeded': 'TRANSIT_TO_POLE',
'aborted': 'DOCKING',
'preempted': 'TRANSIT_TO_GATE'
})
StateMachine.add('TRANSIT_TO_POLE',
nav_transit_states['pole'],
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
""" When in POLE sector"""
#StateMachine.add('POLE_PASSING_TASK', sm_pole_tasks, transitions={'passed':'POLE_PASSING_TASK','missed':'RETURN_TO_DOCK','aborted':'RETURN_TO_DOCK'})
""" When aborted, return to docking """
StateMachine.add('DOCKING',
self.sm_docking,
transitions={'succeeded': ''})
# Register a callback function to fire on state transitions within the sm_mission state machine
self.sm_mission.register_transition_cb(self.mission_transition_cb,
cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add states to the container
with self.sm_recharge:
""" To be able to dock we have to be in the recharging area """
StateMachine.add('DOCKING',
self.sm_docking,
transitions={'succeeded': 'RECHARGE_BATTERY'})
""" Batteru is recharged by using the set_battery_level service to 100 percent battery level """
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': 'RECHARGE_FINISHED'})
""" when recharge is finished, we have to set the DP controller in open loop so we can use the transit controller """
StateMachine.add('RECHARGE_FINISHED',
ControlMode(OPEN_LOOP),
transitions={'succeeded': ''})
# Create the sm_concurrence state machine using a concurrence container
self.sm_concurrence = 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_mission and a battery MonitorState to the sm_concurrence container
with self.sm_concurrence:
Concurrence.add('SM_MISSION', self.sm_mission)
Concurrence.add(
'MONITOR_BATTERY',
MonitorState("/manta/battery_level", Float32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add sm_concurrence, sm_recharge and a Stop() objective to sm_top
with self.sm_top:
StateMachine.add('PATROL',
self.sm_concurrence,
transitions={
'succeeded': '',
'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(str(rospy.get_name()), self.sm_top,
'/SM_ROOT')
intro_server.start()
# Execute the state machine
hsm_outcome = self.sm_top.execute()
intro_server.stop()
def main():
rospy.init_node('centipede_fsm_node', anonymous=True)
centipede_to_seg_dict = {}
seg_to_centipede_dict = {}
num_segments = None
ros_rate = None
if ((len(sys.argv) < 6)):
print(
"usage: centipede_fsm_node.py centipede_to_segment0 segment0_to_centipede \
centipede_to_segment1 setment1_to_centipede (etc...) num_segments ros_rate"
)
else:
print sys.stderr, sys.argv
num_segments = int(sys.argv[-4])
print("num_segments = " + str(num_segments))
ros_rate = float(sys.argv[-3])
for i in range(num_segments):
centipede_to_seg_dict['seg' + str(i)] = sys.argv[2 * i + 1]
seg_to_centipede_dict['seg' + str(i)] = sys.argv[2 * i + 2]
# Step State instances
# Start
start_state_dict = {}
for i in range(num_segments):
start_state_dict['seg' + str(i)] = Step(
centipede_to_seg_dict['seg' + str(i)], "StartLeft",
seg_to_centipede_dict['seg' + str(i)], "Ready", None, ros_rate,
'seg' + str(i))
# Step A
step_a_state_dict = {}
for i in range(num_segments):
step_a_state_dict['seg' + str(i)] = Step(
centipede_to_seg_dict['seg' + str(i)], "StepLeft",
seg_to_centipede_dict['seg' + str(i)], "Ready", None, ros_rate,
'seg' + str(i))
# Step B
step_b_state_dict = {}
for i in range(num_segments):
step_b_state_dict['seg' + str(i)] = Step(
centipede_to_seg_dict['seg' + str(i)], "StepRight",
seg_to_centipede_dict['seg' + str(i)], "Ready", None, ros_rate,
'seg' + str(i))
# Concurrence instances
# Start
step_start_con_success_dict = {}
for state_name in start_state_dict:
step_start_con_success_dict[state_name.upper()] = 'success'
step_start_con = Concurrence(
outcomes=['success', 'failure'],
default_outcome='failure',
outcome_map={'success': step_start_con_success_dict})
with step_start_con:
for state_name in start_state_dict:
Concurrence.add(state_name.upper(), start_state_dict[state_name])
# Step A
step_a_con_success_dict = {}
for state_name in step_a_state_dict:
step_a_con_success_dict[state_name.upper()] = 'success'
step_a_con = Concurrence(outcomes=['success', 'failure'],
default_outcome='failure',
outcome_map={'success': step_a_con_success_dict})
with step_a_con:
for state_name in step_a_state_dict:
Concurrence.add(state_name.upper(), step_a_state_dict[state_name])
# Step B
step_b_con_success_dict = {}
for state_name in step_b_state_dict:
step_b_con_success_dict[state_name.upper()] = 'success'
step_b_con = Concurrence(outcomes=['success', 'failure'],
default_outcome='failure',
outcome_map={'success': step_b_con_success_dict})
with step_b_con:
for state_name in step_b_state_dict:
Concurrence.add(state_name.upper(), step_b_state_dict[state_name])
# Create a SMACH state machine
centipede_fsm_node = StateMachine(outcomes=['success'])
# Open the SMACH state machine
with centipede_fsm_node:
# Add these concurrent centipede container instances to the top-level StateMachine
StateMachine.add('START',
step_start_con,
transitions={
'failure': 'START',
'success': 'STEP_A'
})
StateMachine.add('STEP_A',
step_a_con,
transitions={
'failure': 'STEP_A',
'success': 'STEP_B'
})
StateMachine.add('STEP_B',
step_b_con,
transitions={
'failure': 'STEP_B',
'success': 'STEP_A'
})
# Create and start the introspection server - for visualization / debugging
sis = smach_ros.IntrospectionServer(
'centipede_left_right_fsm_node' + str(rospy.get_name()),
centipede_fsm_node, '/SM_ROOT') #+ str(rospy.get_name()))
sis.start()
# Give Gazebo some time to start up
user_input = raw_input(
"Please press the 'Return/Enter' key to start executing - type: centipede_left_right_fsm_node.py | node: "
+ str(rospy.get_name()) + "\n")
# Execute SMACH state machine
print(
"Input received. Executing - type: centipede_left_right_fsm_node.py | node: "
+ str(rospy.get_name()) + "...\n")
outcome = centipede_fsm_node.execute()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
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
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()
