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 __init__(self):
self.find_people_ = MonitorState('follow',
xm_FollowPerson,
self.people_cb,
max_checks =5,
output_keys=['pos_xm'])
self.tf_listener = tf.TransformListener()
def test_userdata(self):
"""Test serial manipulation of userdata."""
pinger_thread = threading.Thread(target=pinger)
pinger_thread.start()
init_ud = UserData()
init_ud.b = 'A'
sm = StateMachine(['valid', 'invalid', 'preempted'])
sm.set_initial_state(['MON'], userdata=init_ud)
assert 'b' in sm.userdata
assert sm.userdata.b == 'A'
with sm:
StateMachine.add(
'MON',
MonitorState('/ping',
Empty,
cond_cb,
input_keys=['b'],
output_keys=['a']))
outcome = sm.execute()
assert outcome == 'invalid'
assert 'b' in sm.userdata
assert sm.userdata.b == 'A'
assert 'a' in sm.userdata
assert sm.userdata.a == 'A'
def __init__(self):
self.tracker = MonitorState('/people_tracked',
PersonArray,
self.people_cb,
max_checks=5,
input_keys=['people_id'],
output_keys=['pos_xm', 'people_id'])
self.tf_listener = tf.TransformListener()
def __init__(self):
rospy.sleep(5.0)
self.people_direct = MonitorState('/hark_source',
HarkSource,
self.hark_cb,
max_checks=30,
output_keys=['turn_degree'])
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map={'key_points': {
'MONITOR_AR': 'invalid'
}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add(
'MONITOR_AR',
MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'key_points': 'PATROL_KEYPOINTS',
'stop': 'STOP'
})
StateMachine.add('PATROL_KEYPOINTS',
PatrolThroughKeyPoints(
self.keyPointManager).getSM(),
transitions={'succeeded': 'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('xm_arm_ar_tags')
rospy.on_shutdown(self.shutdown)
# design the main part of the test snippet
#
# self.arm_concurrence = StateMachine(outcomes =['succeeded','aborted','preempted'])
#
# TODO:add the speech-recognize function with the pocketsphinx package
# this will download from the source in the kinetic
# http://blog.csdn.net/x_r_su/article/details/53022746?locationNum=1&fps=1
# with self.arm_concurrence:
self.sm_ArTags = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_ArTags:
#TODO:here we need to make the topic only return when the ar_tags is detected
# and we should do that:the libfreenectgrabber to publish the PointClouds data real-time
# but make the Octomap only generate for a certain time
self.sm_ArTags.userdata.object_position = PointStamped()
StateMachine.add('GETPOISITION',
MonitorState('ar_pose_marker',
AlvarMarkers,
self.position_cb,
max_checks=1,
output_keys=['obj_pos']),
transitions={
'valid': 'PICK',
'invalid': 'aborted',
'preempted': 'aborted'
},
remapping={'obj_pos': 'object_position'})
StateMachine.add('PICK',
ArmGo(),
transitions={
'succeeded': 'WAIT',
'aborted': 'GETPOISITION',
'preempted': 'aborted'
},
remapping={'obj_pos': 'object_position'})
self.sm_ArTags.userdata.rec = 2.0
StateMachine.add('WAIT',
Wait(),
remapping={'rec': 'rec'},
transitions={'succeeded': 'GETPOISITION'})
outcome = self.sm_ArTags.execute()
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold',
50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add(
'MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={
'invalid': 'RECHARGE_BATTERY',
'valid': 'MONITOR_BATTERY',
'preempted': 'MONITOR_BATTERY'
},
)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
request=100),
transitions={
'succeeded': 'MONITOR_BATTERY',
'aborted': 'MONITOR_BATTERY',
'preempted': 'MONITOR_BATTERY'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery',
sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
def __init__(self):
Concurrence.__init__(
self,
outcomes=['exit', 'restart', 'aborted', 'preempted'],
default_outcome='restart',
output_keys=['control_infos'],
child_termination_cb=self.child_termination_cb,
outcome_cb=self.outcome_cb)
self.userdata.control_infos = {}
with self:
Concurrence.add(
'CHANGE_CONTROL_MONITOR',
MonitorState("/change_control",
String,
self.change_control_cb,
input_keys=['control_infos'],
output_keys=['control_infos']))
Concurrence.add('TIMER', Timer())
Concurrence.add('CONTROL', Control())
def __init__(self):
rospy.init_node('xm_arm_ar')
rospy.logwarn('Come on! Let us go now!')
self.arm_stack_service = rospy.Service('arm_stack',xm_PickOrPlace,self.callback)
self.sm_ArTags =StateMachine(outcomes =['succeeded','aborted','error'])
with self.sm_ArTags:
self.sm_ArTags.userdata.arm_ps = PointStamped()
StateMachine.add('GETPOISITION',
MonitorState('ar_pose_marker',AlvarMarkers,self.position_cb,max_checks=1,output_keys =['obj_pos']),
transitions={'valid':'PICK','invalid':'aborted','preempted':'aborted'},
remapping={'obj_pos':'arm_ps'})
self.sm_ArTags.userdata.arm_mode_1 =1
StateMachine.add('PICK',
ArmGo(),
transitions={'succeeded':'PLACE','aborted':'GETPOISITION','error':'error'},
remapping={'arm_ps':'arm_ps','arm_mode':'arm_mode_1'})
self.sm_ArTags.userdata.arm_mode_2 = 2
StateMachine.add('PLACE',ArmGo(),
transitions={"succeeded":'succeeded','aborted':'aborted','error':'error'},
remapping={'arm_ps':'arm_ps','arm_mode':'arm_mode_2'})
outcome = self.sm_ArTags.execute()
def __init__(self):
rospy.init_node('smach_home_status', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
#####################################
# JO IS AWAKE
#####################################
# State machine for Jo-awake-go-sleep
self.sm_jo_awake_sleep = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_awake_sleep:
StateMachine.add('LOOK_WAKE',
MonitorState("/JO/sleep", Empty, self.empty_cb),
transitions={
'valid': 'GOING_SLEEP',
'preempted': 'preempted',
'invalid': 'GOING_SLEEP'
})
StateMachine.add('GOING_SLEEP',
JoGoingSleep(),
transitions={'succeeded': 'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED',
MonitorState("/myo_disconnected", Empty,
self.empty_cb),
transitions={
'valid': 'IN_BED',
'preempted': 'preempted',
'invalid': 'IN_BED'
})
StateMachine.add('IN_BED',
JoInBed(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake-bothgo-sleep
self.sm_jo_awake_bothsleep = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_awake_bothsleep:
StateMachine.add('LOOK_WAKE',
MonitorState("/BOTH/sleep", Empty, self.empty_cb),
transitions={
'valid': 'GOING_SLEEP',
'preempted': 'preempted',
'invalid': 'GOING_SLEEP'
})
StateMachine.add('GOING_SLEEP',
BothGoingSleep(),
transitions={'succeeded': 'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED',
MonitorState("/myo_disconnected", Empty,
self.empty_cb),
transitions={
'valid': 'IN_BED',
'preempted': 'preempted',
'invalid': 'IN_BED'
})
StateMachine.add('IN_BED',
BothInBed(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake-go-out
self.sm_jo_awake_out = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_awake_out:
StateMachine.add('LOOK_OUT',
MonitorState("/JO/go_out", Empty, self.empty_cb),
transitions={
'valid': 'PAUSE',
'preempted': 'preempted',
'invalid': 'PAUSE'
})
StateMachine.add('PAUSE',
Pause(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake
self.sm_jo_awake = Concurrence(
outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'],
default_outcome='succeeded',
child_termination_cb=self.jo_awake_child_termination_cb,
outcome_cb=self.jo_awake_outcome_cb)
with self.sm_jo_awake:
Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep)
Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep)
Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out)
#####################################
# JO IS SLEEPING
#####################################
# State machine for Jo-sleep-waking
self.sm_jo_sleep_waking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_sleep_waking:
StateMachine.add('WAIT_WAKE_UP',
MonitorState("/JO/wake_up", Empty, self.empty_cb),
transitions={
'valid': 'WAKING_UP',
'preempted': 'preempted',
'invalid': 'WAKING_UP'
})
StateMachine.add('WAKING_UP',
JoWakingUp(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-sleep-bothwaking
self.sm_jo_sleep_bothwaking = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_sleep_bothwaking:
StateMachine.add('WAIT_WAKE_UP',
MonitorState("/BOTH/wake_up", Empty,
self.empty_cb),
transitions={
'valid': 'WAKING_UP',
'preempted': 'preempted',
'invalid': 'WAKING_UP'
})
StateMachine.add('WAKING_UP',
BothWakingUp(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-awake
self.sm_jo_sleep = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'],
default_outcome='succeeded',
child_termination_cb=self.jo_sleep_child_termination_cb,
outcome_cb=self.jo_sleep_outcome_cb)
with self.sm_jo_sleep:
Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking)
Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking)
#####################################
# JO IS OUT TODO
#####################################
# State machine for Jo-out-back
self.sm_jo_out_back = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_out_back:
StateMachine.add('WAIT_BACK_HOME',
MonitorState("/JO/back_home", Empty,
self.empty_cb),
transitions={
'valid': 'WAIT_MYO',
'preempted': 'preempted',
'invalid': 'WAIT_MYO'
})
StateMachine.add('WAIT_MYO',
MonitorState("/myo_connected", Empty,
self.empty_cb),
transitions={
'valid': 'COMING_BACK',
'preempted': 'preempted',
'invalid': 'COMING_BACK'
})
StateMachine.add('COMING_BACK',
JoBackHome(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-out-bothback
self.sm_jo_out_bothback = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo_out_bothback:
StateMachine.add('WAIT_BACK_HOME',
MonitorState("/BOTH/back_home", Empty,
self.empty_cb),
transitions={
'valid': 'WAIT_MYO',
'preempted': 'preempted',
'invalid': 'WAIT_MYO'
})
StateMachine.add('WAIT_MYO',
MonitorState("/myo_connected", Empty,
self.empty_cb),
transitions={
'valid': 'COMING_BACK',
'preempted': 'preempted',
'invalid': 'COMING_BACK'
})
StateMachine.add('COMING_BACK',
BothBackHome(),
transitions={'succeeded': 'succeeded'})
# State machine for Jo-out
self.sm_jo_out = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'back_home'],
default_outcome='succeeded',
child_termination_cb=self.jo_out_child_termination_cb,
outcome_cb=self.jo_out_outcome_cb)
with self.sm_jo_out:
Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back)
Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback)
#####################################
# TOP LVL JO SM
#####################################
# State machine for JO
self.sm_jo = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_jo:
StateMachine.add('AWAKE',
self.sm_jo_awake,
transitions={
'succeeded': 'succeeded',
'stop': 'aborted',
'go_sleep': 'SLEEP',
'go_out': 'OUT'
})
StateMachine.add('SLEEP',
self.sm_jo_sleep,
transitions={
'succeeded': 'succeeded',
'wake_up': 'AWAKE'
})
StateMachine.add('OUT',
self.sm_jo_out,
transitions={
'succeeded': 'succeeded',
'back_home': 'AWAKE'
})
#####################################
# TOP LVL CAROLE SM TODO
#####################################
# State machine for CAROLE
self.sm_carole = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_carole:
StateMachine.add('WAIT3',
MonitorState("/TEST/wait3", Empty, self.empty_cb),
transitions={
'valid': 'PAUSE',
'preempted': 'preempted',
'invalid': 'PAUSE'
})
StateMachine.add('PAUSE',
Pause(),
transitions={
'succeeded': 'WAIT3',
'aborted': 'aborted'
})
#####################################
# TOP LVL EAT SM TODO
#####################################
# State machine for EAT
self.sm_eat = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_eat:
StateMachine.add('WAIT2',
MonitorState("/TEST/wait2", Empty, self.empty_cb),
transitions={
'valid': 'PAUSE',
'preempted': 'preempted',
'invalid': 'PAUSE'
})
StateMachine.add('PAUSE',
Pause(),
transitions={
'succeeded': 'WAIT2',
'aborted': 'aborted'
})
#####################################
# TOP LVL SHOWER SM
#####################################
# State machine for SHOWER
self.sm_shower = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_shower:
StateMachine.add('WAIT_SHOWER',
MonitorState("/HOME/go_shower", Empty,
self.empty_cb),
transitions={
'valid': 'PREPARING_SHOWER',
'preempted': 'preempted',
'invalid': 'PREPARING_SHOWER'
})
StateMachine.add('PREPARING_SHOWER',
PreparingShower(),
transitions={
'succeeded': 'GO_SHOWER',
'aborted': 'WAIT1'
})
StateMachine.add('GO_SHOWER',
GoShower(),
transitions={
'succeeded': 'STOP_SHOWER',
'aborted': 'aborted'
})
StateMachine.add('STOP_SHOWER',
StopShower(),
transitions={
'succeeded': 'WAIT1',
'aborted': 'aborted'
})
#####################################
# TOP LVL SM
#####################################
# Create the top level state machine
self.sm_top = Concurrence(
outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
Concurrence.add('SM_JO', self.sm_jo)
Concurrence.add('SM_CAROLE', self.sm_carole)
Concurrence.add('SM_EAT', self.sm_eat)
Concurrence.add('SM_SHOWER', self.sm_shower)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('patrol_task_concurrence', anonymous=False)
# 设置关闭机器人函数(stop the robot)
rospy.on_shutdown(self.shutdown)
# 初始化voice_id
self.voice_id_data = -1
# 初始化一些参数和变量
setup_task_environment(self)
# 跟踪到达目标位置的成功率
self.n_succeeded, self.n_aborted, self.n_preempted = 0, 0, 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(180.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(180.0),
server_wait_timeout=rospy.Duration(10.0))
# 为语音指令子任务创建一个状态机
sm_voice_command = StateMachine(outcomes=[
'livingroom_id', 'bedroom_id', 'kitchen_id', 'balcony_id',
'None_id'
])
# 然后添加子任务
with sm_voice_command:
StateMachine.add('RECOGNIZE_VOICE_COMMAND',
Voice_Command(self.voice_id_data, 5),
transitions={
'livingroom_id': '',
'bedroom_id': '',
'kitchen_id': '',
'balcony_id': '',
'None_id': ''
})
# 为等待任务创建一个状态机
sm_waite_command = StateMachine(outcomes=['waite', 'preempted'])
# 然后添加子任务
with sm_waite_command:
StateMachine.add('WAITE_VOICE_COMMAND',
None_Command('waite_voice_command', 5),
transitions={
'waite': '',
'preempted': ''
})
# 为living_room子任务创建一个状态机
sm_living_room = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_living_room:
StateMachine.add('CLEAN_LIVING_ROOM',
LivingRoom('living_room', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为bedroom子任务创建一个状态机
sm_bedroom = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_bedroom:
StateMachine.add('CLEAN_BEDROOM',
Bedroom('bedroom', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为kitchen子任务创建一个状态机
sm_kitchen = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_kitchen:
StateMachine.add('CLEAN_KITCHEN',
Kitchen('kitchen', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 为balcony子任务创建一个状态机
sm_balcony = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 然后添加子任务
with sm_balcony:
StateMachine.add('CLEAN_BALCONY',
Balcony('balcony', 5),
transitions={
'succeeded': '',
'aborted': '',
'preempted': ''
})
# 初始化导航的状态机
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 使用transitions将导航的状态添加到状态机
with self.sm_nav:
StateMachine.add('VOICE_COMMAND',
sm_voice_command,
transitions={
'livingroom_id': 'CLEAN_LIVING_ROOM',
'bedroom_id': 'CLEAN_BEDROOM',
'kitchen_id': 'CLEAN_KITCHEN',
'balcony_id': 'CLEAN_BALCONY',
'None_id': 'WAITE_VOICE_COMMAND'
})
''' Add the living room subtask(s) '''
StateMachine.add('CLEAN_LIVING_ROOM',
nav_states['living_room'],
transitions={
'succeeded': 'CLEAN_LIVING_ROOM_TASKS',
'aborted': '',
'preempted': ''
})
# 当任务完成时, 继续进行kitchen任务
StateMachine.add('CLEAN_LIVING_ROOM_TASKS',
sm_living_room,
transitions={
'succeeded': 'VOICE_COMMAND',
'aborted': '',
'preempted': ''
})
''' Add the kitchen subtask(s) '''
StateMachine.add('CLEAN_BEDROOM',
nav_states['bedroom'],
transitions={
'succeeded': 'CLEAN_BEDROOM_TASKS',
'aborted': '',
'preempted': ''
})
# 当任务完成时, 继续进行bathroom任务
StateMachine.add('CLEAN_BEDROOM_TASKS',
sm_bedroom,
transitions={
'succeeded': 'VOICE_COMMAND',
'aborted': '',
'preempted': ''
})
''' Add the bathroom subtask(s) '''
StateMachine.add('CLEAN_KITCHEN',
nav_states['kitchen'],
transitions={
'succeeded': 'CLEAN_KITCHEN_TASKS',
'aborted': '',
'preempted': ''
})
# 当任务完成时, 继续进行hallway任务
StateMachine.add('CLEAN_KITCHEN_TASKS',
sm_kitchen,
transitions={
'succeeded': 'VOICE_COMMAND',
'aborted': '',
'preempted': ''
})
''' Add the balcony subtask(s) '''
StateMachine.add('CLEAN_BALCONY',
nav_states['balcony'],
transitions={
'succeeded': 'CLEAN_BALCONY_TASKS',
'aborted': '',
'preempted': ''
})
# 当任务完成时, stop
StateMachine.add('CLEAN_BALCONY_TASKS',
sm_balcony,
transitions={
'succeeded': 'VOICE_COMMAND',
'aborted': '',
'preempted': ''
})
''' Add the balcony subtask(s) '''
# 当没有识别到语音指令时，等待
StateMachine.add('WAITE_VOICE_COMMAND',
sm_waite_command,
transitions={
'waite': 'VOICE_COMMAND',
'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', 'waite'])
# 将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()
def __init__(self):
self.door_detect_ = MonitorState('DoorState',
Bool,
self.door_cb,
max_checks=1)
def __init__(self):
rospy.on_shutdown(self.ShutdownCallback)
# Subscribe to message to cancel missions
self.__cancel_sub = rospy.Subscriber('/missions/mission_cancel', Empty,
self.CancelCallback)
# Create an instance of the missions helper class
self.__missions_helper = MissionsHelper()
# ------------------------- Top level state machine -------------------------
# Create top level state machine
self.__sm = StateMachine(outcomes=['preempted','aborted'],
output_keys=['mission_data'])
with self.__sm:
# Add a state which monitors for a mission to run
StateMachine.add('WAITING',
MonitorState('/missions/mission_request',
String,
self.MissionRequestCB,
output_keys = ['mission']),
transitions={'valid':'WAITING', 'invalid':'PREPARE',
'preempted':'preempted'})
# Add state to prepare the mission
StateMachine.add('PREPARE',
Prepare(self.__missions_helper),
transitions={'mission1':'MISSION1','mission2':'MISSION2','mission4':'MISSION4',
'done_task':'WAITING','head_default':'DEFAULT_HEAD_POSITION',
'move_head':'MOVE_HEAD'})
# Add the reporting state
StateMachine.add('REPORT',
Report(),
transitions={'success':'DEFAULT_HEAD_POSITION'})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt = self.__missions_helper.CameraDefaultPos()
head_goal = point_headGoal()
head_goal.absolute = True
head_goal.pan = default_position_pan
head_goal.tilt = default_position_tilt
# Add the default camera position state. Which moves the head to the default position
StateMachine.add('DEFAULT_HEAD_POSITION',
SimpleActionState('head_control_node',
point_headAction,
result_cb = self.__missions_helper.CameraAtDefaultPos,
goal = head_goal),
transitions={'succeeded':'WAITING','preempted':'WAITING','aborted':'aborted'})
# Add the move head state
StateMachine.add('MOVE_HEAD',
SimpleActionState('head_control_node',
point_headAction,
goal_slots=['absolute', 'pan', 'tilt']),
transitions={'succeeded':'WAITING', 'preempted':'WAITING', 'aborted':'aborted'},
remapping={'absolute':'user_data_absolute', 'pan':'user_data_pan', 'tilt':'user_data_tilt'})
# ------------------------- Sub State machine for mission 1 ---------------------
# Create a sub state machine for mission 1 - take a message to
self.__sm_mission1 = missions_lib.Mission1StateMachine(self.__missions_helper)
# Now add the sub state machine for mission 1 to the top level one
StateMachine.add('MISSION1',
self.__sm_mission1,
transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'})
# ------------------------- Sub State machine for mission 2 ---------------------
# Create a sub state machine for mission 2 - face detection and greeting
self.__sm_mission2 = missions_lib.Mission2StateMachine(self.__missions_helper)
# Now add the sub state machine for mission 2 to the top level one
StateMachine.add('MISSION2',
self.__sm_mission2,
transitions={'complete':'REPORT','preempted':'REPORT','aborted':'aborted'})
# ------------------------- Sub State machine for mission 4 ---------------------
# Create a sub state machine for mission 4 - Go Home
self.__sm_mission4 = missions_lib.Mission4StateMachine(self.__missions_helper)
# Now add the sub state machine for mission 4 to the top level one
StateMachine.add('MISSION4',
self.__sm_mission4,
transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'})
# -------------------------------------------------------------------------------
# Create and start the introspective server so that we can use smach_viewer
sis = IntrospectionServer('server_name', self.__sm, '/SM_ROOT')
sis.start()
self.__sm.execute()
# Wait for ctrl-c to stop application
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
# 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(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(10.0))
# Initialize the navigation state machine
self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':'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))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'})
StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# 设置关闭机器人函数(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 = list()
# 把waypoints变成状态机的状态
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(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(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))
# 初始化导航的状态机
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# 使用transitions将导航的状态添加到状态机
with self.sm_nav:
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
nav_states[1],
transitions={
'succeeded': 'NAV_STATE_2',
'aborted': 'NAV_STATE_2'
})
StateMachine.add('NAV_STATE_2',
nav_states[2],
transitions={
'succeeded': 'NAV_STATE_3',
'aborted': 'NAV_STATE_3'
})
StateMachine.add('NAV_STATE_3',
nav_states[3],
transitions={
'succeeded': 'NAV_STATE_4',
'aborted': 'NAV_STATE_4'
})
StateMachine.add('NAV_STATE_4',
nav_states[0],
transitions={
'succeeded': '',
'aborted': ''
})
# 在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('operating_mode', anonymous=False)
global mb
mb = MongoBridge()
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create the top level SMACH state machine
self.sm_top = StateMachine(outcomes=['stop'])
# Open the container
with self.sm_top:
# ===== SETUP State =====
StateMachine.add('SETUP',
Setup(),
transitions={
'succeeded': 'TOP_CONTROL',
'preempted': 'stop',
'aborted': 'stop'
})
StateMachine.add('RECHARGE',
Recharge(),
transitions={
'succeeded': 'SETUP',
'preempted': 'stop',
'aborted': 'stop'
})
# ===== TOP_CONTROL State =====
self.sm_battery_concurrence = Concurrence(
outcomes=[
'restart', 'recharge', 'preempted', 'aborted', 'stop'
],
default_outcome='recharge',
child_termination_cb=self.child_termination_cb,
outcome_cb=self.outcome_cb)
self.sm_battery_concurrence.userdata.mode_change_infos = {
'mode': None,
'change_mode': False
}
# Open the container
with self.sm_battery_concurrence:
# Add states to the container
Concurrence.add(
'BATTERY_MONITOR',
MonitorState("/battery_level", Float32,
self.battery_monitor_cb))
Concurrence.add(
'CHANGE_MODE_MONITOR',
MonitorState("/change_mode",
String,
self.change_mode_monitor_cb,
input_keys=['mode_change_infos'],
output_keys=['mode_change_infos']))
self.operating_mode = StateMachine(
outcomes=['aborted', 'preempted', 'standby'],
input_keys=['mode_change_infos'])
with self.operating_mode:
StateMachine.add(
'MODE_MANAGEMENT',
OperatingMode(),
transitions={'succeeded': 'MODE_MANAGEMENT'})
Concurrence.add('OPERATING_MODE', self.operating_mode)
StateMachine.add('TOP_CONTROL',
self.sm_battery_concurrence,
transitions={
'restart': 'TOP_CONTROL',
'recharge': 'RECHARGE',
'aborted': 'stop',
'preempted': 'stop'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('mode', 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():
# 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 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_ros.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('HOME_automation_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi / 2, pi, 3 * pi / 2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
self.waypoints.append(
Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(
Pose(Point(self.square_size, self.square_size, 0.0),
quaternions[1]))
self.waypoints.append(
Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# State machine for light entry
self.sm_light_entry = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_light_entry.userdata.day_mode = 1
with self.sm_light_entry:
StateMachine.add('LOOK_ENTRY',
MonitorState("/HOME/entry_move", Empty,
self.empty_cb),
transitions={
'valid': 'LOOK_ENTRY',
'invalid': 'LIGHT_UP'
})
StateMachine.add('LIGHT_UP',
LightEntry(),
transitions={'succeeded': 'succeeded'})
# State machine for dark entry
self.sm_dark_entry = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_dark_entry.userdata.day_mode = 1
with self.sm_dark_entry:
StateMachine.add('LOOK_ENTRY_OFF',
MonitorState("/HOME/entry_noOne", Empty,
self.empty_cb),
transitions={
'valid': 'LOOK_ENTRY_OFF',
'invalid': 'LIGHT_DOWN'
})
StateMachine.add('LIGHT_DOWN',
DarkEntry(),
transitions={'succeeded': 'succeeded'})
# State machine for day mode
self.sm_day_mode = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_shower', 'go_sleep',
'go_eat', 'go_out'
],
default_outcome='succeeded',
child_termination_cb=self.daymode_child_termination_cb,
outcome_cb=self.daymode_outcome_cb)
self.sm_day_mode.userdata.day_mode = 1
with self.sm_day_mode:
Concurrence.add(
'LOOK_SHOWER',
MonitorState("/HOME/go_shower", Empty, self.empty_cb))
Concurrence.add(
'LOOK_LEAVING',
MonitorState("/HOME/leaving_home", Empty, self.empty_cb))
Concurrence.add(
'LOOK_SLEEP',
MonitorState("/HOME/go_sleep", Empty, self.empty_cb))
Concurrence.add('LOOK_EAT',
MonitorState("/HOME/go_eat", Empty, self.empty_cb))
Concurrence.add('LOOK_ENTRY', self.sm_light_entry)
Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry)
# State machine for leaving home
self.sm_leaving_home = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_leaving_home:
StateMachine.add('LEAV',
Pause(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# State machine for going to sleep
self.sm_going_sleep = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_going_sleep:
StateMachine.add('GOING_SLEEP',
GoingSleep(),
transitions={'succeeded': 'succeeded'})
# State machine for day mode
self.sm_wait_bed = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=self.useless_child_termination_cb,
outcome_cb=self.useless_outcome_cb)
with self.sm_wait_bed:
Concurrence.add(
'LOOK_INBED',
MonitorState("/METAWATCH/button2", Empty, self.empty_cb))
Concurrence.add('TIMEOUT', TimeoutToBed())
# State machine for in bed
self.sm_in_bed = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_in_bed:
StateMachine.add('IN_BED',
InBed(),
transitions={'succeeded': 'succeeded'})
# State machine for light entry
self.sm_lightn_entry = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_lightn_entry.userdata.day_mode = 0
with self.sm_lightn_entry:
StateMachine.add('LOOK_ENTRY',
MonitorState("/HOME/entry_move", Empty,
self.empty_cb),
transitions={
'valid': 'LOOK_ENTRY',
'invalid': 'LIGHT_UP'
})
StateMachine.add('LIGHT_UP',
LightEntry(),
transitions={'succeeded': 'succeeded'})
# State machine for night mode
self.sm_night_mode = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'],
default_outcome='succeeded',
child_termination_cb=self.nightmode_child_termination_cb,
outcome_cb=self.nightmode_outcome_cb)
self.sm_night_mode.userdata.day_mode = 0
with self.sm_night_mode:
Concurrence.add(
'LOOK_WAKE', MonitorState("/HOME/wake_up", Empty,
self.empty_cb))
Concurrence.add('LOOK_ENTRY', self.sm_lightn_entry)
Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry)
# State machine for night mode
#self.sm_night_mode = StateMachine(outcomes=['succeeded','aborted','preempted'])
#self.sm_night_mode.userdata.day_mode = 0;
#with self.sm_night_mode:
# StateMachine.add('NIGHT_MOD', Pause(),
# transitions={'succeeded':'succeeded',
# 'aborted':'aborted'})
# State machine for waking up
self.sm_waking_up = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_waking_up.userdata.day_mode = 0
with self.sm_waking_up:
StateMachine.add('WAKING_UP',
WakingUp(),
transitions={'succeeded': 'succeeded'})
# State machine for waking up
self.sm_going_eat = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_going_eat.userdata.day_mode = 1
with self.sm_going_eat:
StateMachine.add('EATTTTTTTT',
Pause(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# State machine for home
self.sm_home = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted', 'going_out'])
with self.sm_home:
StateMachine.add('DAY_MODE',
self.sm_day_mode,
transitions={
'succeeded': 'DAY_MODE',
'go_shower': 'PREPARING_SHOWER',
'go_sleep': 'GOING_SLEEP',
'go_eat': 'GOING_EAT',
'go_out': 'LEAVING_HOME',
'aborted': 'aborted'
})
StateMachine.add('PREPARING_SHOWER',
PreparingShower(),
transitions={
'succeeded': 'GO_SHOWER',
'aborted': 'aborted'
})
StateMachine.add('GO_SHOWER',
GoShower(),
transitions={
'succeeded': 'STOP_SHOWER',
'aborted': 'aborted'
})
StateMachine.add('STOP_SHOWER',
StopShower(),
transitions={
'succeeded': 'DAY_MODE',
'aborted': 'aborted'
})
StateMachine.add('LEAVING_HOME',
self.sm_leaving_home,
transitions={
'succeeded': 'going_out',
'aborted': 'aborted'
})
StateMachine.add('GOING_SLEEP',
self.sm_going_sleep,
transitions={
'succeeded': 'WAIT_TO_BED',
'aborted': 'aborted'
})
StateMachine.add('WAIT_TO_BED',
self.sm_wait_bed,
transitions={
'succeeded': 'IN_BED',
'preempted': 'IN_BED',
'aborted': 'aborted'
})
StateMachine.add('IN_BED',
self.sm_in_bed,
transitions={
'succeeded': 'NIGHT_MODE',
'aborted': 'aborted'
})
StateMachine.add('NIGHT_MODE',
self.sm_night_mode,
transitions={
'succeeded': 'NIGHT_MODE',
'wake_up': 'WAKING_UP',
'aborted': 'aborted'
})
StateMachine.add('WAKING_UP',
self.sm_waking_up,
transitions={
'succeeded': 'DAY_MODE',
'aborted': 'aborted'
})
StateMachine.add('GOING_EAT',
self.sm_going_eat,
transitions={
'succeeded': 'DAY_MODE',
'aborted': 'aborted'
})
# State machine for waking up
self.sm_guarding = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_guarding:
StateMachine.add('GUARD',
Pause(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# State machine with concurrence
self.sm_incoming_home = Concurrence(
outcomes=['succeeded', 'aborted'],
default_outcome='succeeded',
child_termination_cb=self.incoming_child_termination_cb,
outcome_cb=self.incoming_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_incoming_home:
Concurrence.add(
'LOOK_CONNECTION',
MonitorState("/METAWATCH/connected", Empty, self.empty_cb))
Concurrence.add(
'LOOK_ENTERING',
MonitorState("/HOME/entry_door_open", Empty, self.empty_cb))
# State machine for away
self.sm_away = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted', 'entering_home'])
with self.sm_away:
StateMachine.add('GUARDING_MODE',
self.sm_guarding,
transitions={
'succeeded': 'INCOMING_HOME',
'aborted': 'aborted'
})
StateMachine.add('INCOMING_HOME',
self.sm_incoming_home,
transitions={
'succeeded': 'entering_home',
'aborted': 'aborted'
})
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('AT_HOME',
self.sm_home,
transitions={
'succeeded': 'AT_HOME',
'going_out': 'AWAY'
})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('AWAY',
self.sm_away,
transitions={
'succeeded': 'AWAY',
'entering_home': 'AT_HOME'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('tinker_mission_manipulation')
trans = tf.TransformListener()
rospy.loginfo("Waiting for tf ...")
rospy.sleep(3)
assert (len(trans.getFrameStrings()) > 0)
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
def kinect_callback(userdata, result):
userdata.objects = []
objects = []
sum_x = 0
for obj in result.objects.objects:
position = obj.pose.pose.pose.position
if position.y > 0.5 or position.y < -0.5:
continue
_levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80]
for l in _levels:
if fabs(l - position.z) < 0.05:
position.z = l + 0.05
if position.z > 1.1:
position.z += 0.05
obj.header.stamp = rospy.Time(0)
kinect_point = PointStamped(header=obj.header, point=position)
odom_point = trans.transformPoint('odom', kinect_point)
sum_x += odom_point.point.x
rospy.loginfo(
colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'),
odom_point.point.x, odom_point.point.y, odom_point.point.z)
objects.append(odom_point)
avg_x = sum_x / len(objects)
for from_point in objects:
to_point = copy.deepcopy(from_point)
to_point.point.x = avg_x
to_point.point.z = find_div(from_point.point.z)
userdata.objects.append({'from': from_point, 'to': to_point})
rospy.loginfo(
colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'),
to_point.point.x, to_point.point.y, to_point.point.z)
rospy.loginfo(colored('Total Object: %d', 'green'), len(objects))
return 'succeeded'
StateMachine.add('Arm_Mode_Kinect',
ArmModeState(ArmModeState.Arm_Mode_Kinect),
transitions={'succeeded': 'Start_Button'})
StateMachine.add('Start_Button',
MonitorStartButtonState(),
transitions={
'valid': 'Start_Button',
'invalid': 'S_Kinect_Recognition'
})
StateMachine.add('S_Kinect_Recognition',
ServiceState(service_name='/kinect_find_objects',
service_spec=FindObjects,
input_keys=['objects'],
output_keys=['objects'],
response_cb=kinect_callback),
transitions={'succeeded': 'Generate_Report'})
StateMachine.add('Generate_Report',
GenerateReportState(image='result.png',
text='object_names.txt'),
transitions={'succeeded': 'IT_Objects_Iterator'})
objects_iterator = Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['objects'],
output_keys=[],
it=lambda: state.userdata.objects,
it_label='target',
exhausted_outcome='succeeded')
with objects_iterator:
fetch_object_sequence = Sequence(
outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
input_keys=['target'],
connector_outcome='succeeded')
with fetch_object_sequence:
Sequence.add('Speak', SpeakState('New Object recognized'))
Sequence.add('Gripper_Photo',
GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_For_Photo',
MoveArmState(Point(-0.7, 0, 0),
target_key='from'),
transitions={'aborted': 'continue'})
concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=['target'])
with concurrence:
Concurrence.add(
'Move_Fetch',
MoveArmState(Point(0.06, 0, 0), target_key='from'))
Concurrence.add(
'Gripper_Laser_sensor',
MonitorState('/gripper_laser_sensor',
Bool,
cond_cb=lambda x, y: False))
Sequence.add('Move_Fetch_Concurrence', concurrence)
Sequence.add('Gripper_Fetch',
GripperState(GripperState.GRIPPER_CLOSE))
Sequence.add(
'Move_Fetch_Back',
MoveArmState(Point(-0.7, 0, 0), target_key='from'))
Sequence.add('Move_Down',
MoveArmState(Point(-0.6, 0, 0), target_key='to'))
Sequence.add('Move_Put',
MoveArmState(Point(0, 0, 0), target_key='to'))
Sequence.add('Gripper_Put',
GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_Put_Back',
MoveArmState(Point(-0.6, 0, 0), target_key='to'),
transitions={'succeeded': 'continue'})
Iterator.set_contained_state('Seq_Fetch_Object',
fetch_object_sequence,
loop_outcomes=['continue'])
# end of objects_iterator
StateMachine.add('IT_Objects_Iterator',
objects_iterator,
transitions={
'succeeded': 'A_Move_Reset',
'aborted': 'A_Move_Reset'
})
StateMachine.add('A_Move_Reset',
ArmModeState(ArmModeState.Arm_Mode_Init),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_manipulation', state,
'/tinker_mission_manipulation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
rospy.init_node('carry_smach_move', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (0.0, 3 * pi / 2, pi / 2, 3 * pi / 2, pi / 2,
3 * pi / 2, 3 * pi / 2, 0.0, pi, 0.0, pi, 3 * pi / 2)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, -3.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(2.9, 1.0, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(3.0, 1.2, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[5]))
self.waypoints.append(Pose(Point(4.4, -0.6, 0.0), quaternions[6]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[7]))
self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[8]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[9]))
self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[10]))
self.waypoints.append(Pose(Point(-1.6, 0.1, 0.0), quaternions[11]))
# State machine
#self.sm_out_main = StateMachine(outcomes=['succeeded','aborted','preempted'])
#self.sm_out_main.userdata.goal = self.waypoints[2];
#with self.sm_out_main:
# StateMachine.add('GO_OUT_MAIN_ROOM', Nav2Waypoint(),
# transitions={'succeeded':'succeeded',
# 'aborted':'aborted'})
# Concurrent State machine
self.sm_in_main_room = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_kitchen',
'go_bedroom', 'go_sleep'
],
default_outcome='succeeded',
child_termination_cb=self.in_main_room_child_termination_cb,
outcome_cb=self.in_main_room_outcome_cb)
self.sm_in_main_room.userdata.goal = self.waypoints[1]
with self.sm_in_main_room:
Concurrence.add(
'GO_TO_KITCHEN',
MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_BEDROOM',
MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_SLEEP',
MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[1]))
# Concurrent State machine
self.sm_sleep = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_kitchen',
'go_bedroom', 'go_main_room'
],
default_outcome='succeeded',
child_termination_cb=self.in_sleep_child_termination_cb,
outcome_cb=self.in_sleep_outcome_cb)
self.sm_sleep.userdata.goal = self.waypoints[0]
with self.sm_sleep:
Concurrence.add(
'GO_TO_KITCHEN',
MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_BEDROOM',
MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_MAIN_ROOM',
MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[0]))
# Concurrent State machine
self.sm_in_kitchen = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_main_room',
'go_bedroom', 'go_sleep'
],
default_outcome='succeeded',
child_termination_cb=self.in_kitchen_child_termination_cb,
outcome_cb=self.in_kitchen_outcome_cb)
self.sm_in_kitchen.userdata.goal = self.waypoints[6]
with self.sm_in_kitchen:
Concurrence.add(
'GO_TO_MAIN_ROOM',
MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_BEDROOM',
MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_SLEEP',
MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[6]))
# Concurrent State machine
self.sm_in_bedroom = Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'go_main_room',
'go_kitchen', 'go_sleep'
],
default_outcome='succeeded',
child_termination_cb=self.in_bedroom_child_termination_cb,
outcome_cb=self.in_bedroom_outcome_cb)
self.sm_in_bedroom.userdata.goal = self.waypoints[11]
with self.sm_in_bedroom:
Concurrence.add(
'GO_TO_MAIN_ROOM',
MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_KITCHEN',
MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
Concurrence.add(
'GO_TO_SLEEP',
MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
#Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[11]))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('IN_MAIN_ROOM',
self.sm_in_main_room,
transitions={
'succeeded': 'IN_MAIN_ROOM',
'go_kitchen': 'NAV_M2K_M',
'go_sleep': 'IN_SLEEP',
'go_bedroom': 'NAV_M2B_M'
})
StateMachine.add('IN_KITCHEN',
self.sm_in_kitchen,
transitions={
'succeeded': 'succeeded',
'go_main_room': 'NAV_K2M_K',
'go_sleep': 'NAV_K2S_K',
'go_bedroom': 'NAV_K2B_K'
})
StateMachine.add('IN_BEDROOM',
self.sm_in_bedroom,
transitions={
'succeeded': 'succeeded',
'go_kitchen': 'NAV_B2K_B',
'go_sleep': 'NAV_B2S_B',
'go_main_room': 'NAV_B2M_B'
})
StateMachine.add('IN_SLEEP',
self.sm_sleep,
transitions={
'succeeded': 'succeeded',
'go_kitchen': 'NAV_S2K_M',
'go_main_room': 'IN_MAIN_ROOM',
'go_bedroom': 'NAV_S2B_M'
})
StateMachine.add('NAV_M2K_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_M2K_K',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2K_K',
Nav2Waypoint(self.waypoints[5]),
transitions={
'succeeded': 'NAV_M2K_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2K_END',
Nav2Waypoint(self.waypoints[6]),
transitions={
'succeeded': 'IN_KITCHEN',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2M_K',
Nav2Waypoint(self.waypoints[4]),
transitions={
'succeeded': 'NAV_K2M_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2M_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_K2M_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2M_END',
Nav2Waypoint(self.waypoints[1]),
transitions={
'succeeded': 'IN_MAIN_ROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_M2B_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_C',
Nav2Waypoint(self.waypoints[8]),
transitions={
'succeeded': 'NAV_M2B_B',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_B',
Nav2Waypoint(self.waypoints[10]),
transitions={
'succeeded': 'NAV_M2B_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_M2B_END',
Nav2Waypoint(self.waypoints[11]),
transitions={
'succeeded': 'IN_BEDROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2S_K',
Nav2Waypoint(self.waypoints[4]),
transitions={
'succeeded': 'NAV_K2S_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2S_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_K2S_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2S_END',
Nav2Waypoint(self.waypoints[0]),
transitions={
'succeeded': 'IN_SLEEP',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_K',
Nav2Waypoint(self.waypoints[4]),
transitions={
'succeeded': 'NAV_K2B_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_C',
Nav2Waypoint(self.waypoints[8]),
transitions={
'succeeded': 'NAV_K2B_B',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_B',
Nav2Waypoint(self.waypoints[10]),
transitions={
'succeeded': 'NAV_K2B_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_K2B_END',
Nav2Waypoint(self.waypoints[11]),
transitions={
'succeeded': 'IN_BEDROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_B',
Nav2Waypoint(self.waypoints[9]),
transitions={
'succeeded': 'NAV_B2K_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_C',
Nav2Waypoint(self.waypoints[7]),
transitions={
'succeeded': 'NAV_B2K_K',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_K',
Nav2Waypoint(self.waypoints[5]),
transitions={
'succeeded': 'NAV_B2K_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2K_END',
Nav2Waypoint(self.waypoints[6]),
transitions={
'succeeded': 'IN_KITCHEN',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_B',
Nav2Waypoint(self.waypoints[9]),
transitions={
'succeeded': 'NAV_B2S_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_C',
Nav2Waypoint(self.waypoints[7]),
transitions={
'succeeded': 'NAV_B2S_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_B2S_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2S_END',
Nav2Waypoint(self.waypoints[0]),
transitions={
'succeeded': 'IN_SLEEP',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_B',
Nav2Waypoint(self.waypoints[9]),
transitions={
'succeeded': 'NAV_B2M_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_C',
Nav2Waypoint(self.waypoints[7]),
transitions={
'succeeded': 'NAV_B2M_M',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_M',
Nav2Waypoint(self.waypoints[3]),
transitions={
'succeeded': 'NAV_B2M_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_B2M_END',
Nav2Waypoint(self.waypoints[1]),
transitions={
'succeeded': 'IN_MAIN_ROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_S2B_C',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_C',
Nav2Waypoint(self.waypoints[8]),
transitions={
'succeeded': 'NAV_S2B_B',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_B',
Nav2Waypoint(self.waypoints[10]),
transitions={
'succeeded': 'NAV_S2B_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2B_END',
Nav2Waypoint(self.waypoints[11]),
transitions={
'succeeded': 'IN_BEDROOM',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2K_M',
Nav2Waypoint(self.waypoints[2]),
transitions={
'succeeded': 'NAV_S2K_K',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2K_K',
Nav2Waypoint(self.waypoints[5]),
transitions={
'succeeded': 'NAV_S2K_END',
'aborted': 'aborted'
})
StateMachine.add('NAV_S2K_END',
Nav2Waypoint(self.waypoints[6]),
transitions={
'succeeded': 'IN_KITCHEN',
'aborted': 'aborted'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('carry_sm', self.sm_top,
'/SM_CARRY_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('petit_smach_ai', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Action1
self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action1.userdata.mandibles_sleep = 0.1
with self.sm_action1:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPCUBE_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(),
# transitions={'succeeded':'CLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action2
self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action2.userdata.speed = -0.1;
self.sm_action2.userdata.distance = 0.3;
self.sm_action2.userdata.mandibles_sleep = 0.1
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'OPEN_MANDIBLES',
'aborted':'aborted'})
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'MOVE_BACKWARD',
'aborted':'aborted'})
StateMachine.add('MOVE_BACKWARD', MoveForward(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action3
self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action3.userdata.mandibles_sleep = 0.1
self.sm_action3.userdata.speed = -0.1;
self.sm_action3.userdata.distance = 0.2;
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'MOVE_PUSH',
'aborted':'aborted'})
StateMachine.add('MOVE_PUSH', MovePush(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action4
self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action4.userdata.mandibles_sleep = 0.1
self.sm_action4.userdata.speed_x = 0.1
self.sm_action4.userdata.speed_y = 0.1
self.sm_action4.userdata.distance = 0.5;
with self.sm_action4:
StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'SLIDE',
'aborted':'aborted'})
StateMachine.add('SLIDE', Slide(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action5
self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action5.userdata.mandibles_sleep = 0.1
with self.sm_action5:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
'aborted':'ALMOSTCLOSE_MANDIBLES'})
StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action6
self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action6.userdata.mandibles_sleep = 0.1
self.sm_action6.userdata.speed = 0.1;
self.sm_action6.userdata.distance = 0.2;
with self.sm_action6:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'FORWARD',
'aborted':'aborted'})
StateMachine.add('FORWARD', MoveForward(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'HCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action7
self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action7.userdata.mandibles_sleep = 0.1
with self.sm_action7:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
self.sm_actions.userdata.waypoints = self.waypoints
with self.sm_actions:
StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb,
output_keys=['waypoint_out'],
outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']),
transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'},
remapping={'waypoint_out':'patrol_waypoint'})
#StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
# remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'},
remapping={'waypoint_in':'remove_waypoint'})
# State machine with concurrence
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_ACTIONS', self.sm_actions)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
# @smach.cb_interface()
# def requestPrioCube_cb(userdata, request):
# update_request = UpdatePriority().Request
# update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
# update_request.goal.pose.position.y = 0.800
# update_request.prio = 100
# return update_request
StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'})
StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'})
StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
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 __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
location_goal = MoveBaseGoal()
result_goal = MoveBaseGoal()
#recognize_goal = VisionGoal()
#flag = 1
class wait(State):
def __init__(self):
State.__init__(self, outcomes=['succeeded', 'aborted'])
def execute(self, userdata):
if flag == 0:
rospy.loginfo('waitting')
return 'aborted'
else:
return 'succeeded'
class MoveItDemo(State):
# spin() simply keeps python from exiting until this node is stopped
def __init__(self):
# Initialize the move_group API
State.__init__(self, outcomes=['succeeded', 'aborted'])
def execute(self, userdata):
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_ik')
#rospy.Subscriber("chatter", Pose, callback)
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.11)
right_arm.set_goal_orientation_tolerance(0.15)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_init')
right_arm.go()
rospy.sleep(2)
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
#global a,b,c,d,e,f,g
target_pose.pose.position.x = 0.3
target_pose.pose.position.y = -0.45
target_pose.pose.position.z = 1.35
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
#Set the start state to the current state
right_arm.set_start_state_to_current_state()
#print a
# Set the goal pose of the end effector to the stored pose
right_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a second
rospy.sleep(5)
#right_arm.set_named_target('right_arm_init')
right_arm.go()
return 'succeeded'
class MoveItDemo1(State):
# spin() simply keeps python from exiting until this node is stopped
def __init__(self):
# Initialize the move_group API
State.__init__(self, outcomes=['succeeded', 'aborted'])
def execute(self, userdata):
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_ik')
#rospy.Subscriber("chatter", Pose, callback)
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
right_arm.set_named_target('right_arm_init')
right_arm.go()
return 'succeeded'
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(20.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(20.0),
server_wait_timeout=rospy.Duration(10.0))
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 0.1
pose_target.position.x = 0.7
pose_target.position.y = -0.0
pose_target.position.z = 1.1
result_goal.target_pose.header.frame_id = 'map'
result_goal.target_pose.pose = (Pose(Point(0.5, 1.5, 0.0),
Quaternion(0.0, 0.0, 0.0, 1.0)))
# Initialize the navigation state machine
self.sm_nav = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('WAITTING',
wait(),
transitions={
'succeeded': 'NAV_STATE_0',
'aborted': 'WAITTING'
})
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
SimpleActionState('move_base',
MoveBaseAction,
goal=location_goal),
transitions={
'succeeded': 'ARM',
'aborted': 'ARM'
})
#StateMachine.add('VISION', SimpleActionState('drv_action', VisionAction, goal=recognize_goal),
#transitions={'succeeded': 'ARM', 'aborted': 'ARM'})
StateMachine.add('ARM',
MoveItDemo(),
transitions={
'succeeded': 'NAV_STATE_2',
'aborted': 'NAV_STATE_2'
})
StateMachine.add('NAV_STATE_2',
nav_states[0],
transitions={
'succeeded': 'ARM1',
'aborted': 'ARM1'
})
StateMachine.add('ARM1',
MoveItDemo1(),
transitions={
'succeeded': '',
'aborted': ''
})
# StateMachine.add('NAV_STATE_2', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']),
# transitions={'succeeded': 'NAV_STATE_0'}, remapping={'target_pose': 'user_data'})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION',
nav_docking_station,
transitions={'succeeded': 'RECHARGE_BATTERY'})
StateMachine.add('RECHARGE_BATTERY',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel,
100,
response_cb=self.recharge_cb),
transitions={'succeeded': ''})
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
# Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb))
Concurrence.add(
'LOCATION_GOAL',
MonitorState("nav_location_goal", Pose,
self.nav_location_goal_cb))
Concurrence.add(
'RECOGNIZE_GOAL',
MonitorState("/comm/msg/control/recognize_goal", String,
self.recognize_goal_cb))
#Concurrence.add('RESULT', MonitorState("/drv_action/result", VisionActionResult, self.result_goal_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
rospy.loginfo('=============ce shi=============')
time.sleep(5)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
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('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.docking_station_pose = (Pose(Point(-0.5, 0.0, 0.0),
Quaternion(0.0, 0.0, 0.0, 1.0)))
self.waypoints = list()
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose.position.x = 0.7
nav_goal.target_pose.pose.position.y = 0.0
nav_goal.target_pose.pose.position.z = 0.0
nav_goal.target_pose.pose.orientation.w = 1.0
self.waypoints.append(nav_goal)
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose.position.x = 2.51611566544
nav_goal.target_pose.pose.position.y = 0.100562250018
nav_goal.target_pose.pose.position.z = 0.0
nav_goal.target_pose.pose.orientation.w = 1.0
self.waypoints.append(nav_goal)
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose.position.x = 2.94330668449
nav_goal.target_pose.pose.position.y = -0.77200148106
nav_goal.target_pose.pose.position.z = 0.0
nav_goal.target_pose.pose.orientation.z = -0.585479230542
nav_goal.target_pose.pose.orientation.w = 0.81068740622
self.waypoints.append(nav_goal)
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose.position.x = 2.94330668449
nav_goal.target_pose.pose.position.y = -1.67200148106
nav_goal.target_pose.pose.position.z = 0.0
nav_goal.target_pose.pose.orientation.z = -0.585479230542
nav_goal.target_pose.pose.orientation.w = 0.81068740622
self.waypoints.append(nav_goal)
# 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()
# 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))
# 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
self.sm_nav.userdata.waypoints = self.waypoints
with self.sm_nav:
StateMachine.add('PICK_WAYPOINT',
PickWaypoint(),
transitions={'succeeded': 'NAV_WAYPOINT'},
remapping={'waypoint_out': 'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': 'PICK_WAYPOINT',
'aborted': 'PICK_WAYPOINT',
},
remapping={'waypoint_in': 'patrol_waypoint'})
# 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/set_battery',
SetBattery,
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/battery_level", Float32,
self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'recharge': 'RECHARGE',
'stop': 'STOP'
})
StateMachine.add('RECHARGE',
self.sm_recharge,
transitions={'succeeded': 'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('smach_usecase_executive')
rospy.Subscriber("turtle2/pose", turtlesim.msg.Pose, turtle2pose_cb)
sm_root = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'])
#input_keys = ['my_awesome_goal'],
#output_keys= ['some_awesome_result'])
with sm_root:
#add state to call ROS service by turtlesim to reset sim
smach.StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
transitions={'succeeded': 'SPAWN'})
#add state to spawn new turtle at 0,0 called turtle2
smach.StateMachine.add('SPAWN',
ServiceState('spawn',
turtlesim.srv.Spawn,
request=turtlesim.srv.SpawnRequest(
0, 0, 0, 'turtle2')),
transitions={'succeeded': 'TELEPORT1'})
smach.StateMachine.add(
'TELEPORT1',
ServiceState('turtle1/teleport_absolute',
turtlesim.srv.TeleportAbsolute,
request=turtlesim.srv.TeleportAbsoluteRequest(
5.0, 1.0, 0)),
transitions={'succeeded': 'DRAW_SHAPES'})
# Create DRAW SHAPES container
sm_draw_shapes = smach.Concurrence(
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'BIG': 'succeeded',
'SMALL': 'succeeded'
}
})
smach.StateMachine.add('DRAW_SHAPES',
sm_draw_shapes,
transitions={'succeeded': 'succeeded'})
with sm_draw_shapes:
smach.Concurrence.add(
'BIG',
SimpleActionState('turtle_shape1',
turtle_actionlib.msg.ShapeAction,
goal=turtle_actionlib.msg.ShapeGoal(
edges=11, radius=4.0)))
#Create SMALL container
sm_small = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'])
smach.Concurrence.add('SMALL', sm_small)
with sm_small:
smach.StateMachine.add(
'TELEPORT2',
ServiceState('turtle2/teleport_absolute',
turtlesim.srv.TeleportAbsolute,
request=turtlesim.srv.TeleportAbsoluteRequest(
9.0, 5.0, 0)),
transitions={'succeeded': 'DRAW_WITH_MONITOR'})
smach.StateMachine.add('WAIT_FOR_CLEAR',
MonitorState("turtle1/pose",
turtlesim.msg.Pose,
wait_for_clear_callback),
transitions={
'invalid': 'TELEPORT2',
'valid': 'WAIT_FOR_CLEAR'
})
#Create DRAW WITH MONITOR
sm_draw_with_monitor = smach.Concurrence(
outcomes=[
'succeeded', 'preempted', 'aborted', 'interrupted'
],
default_outcome='aborted',
child_termination_cb=child_term_cb,
outcome_map={
'succeeded': {
'DRAW': 'succeeded',
'MONITOR': 'valid'
},
'preempted': {
'DRAW': 'preempted',
'MONITOR': 'preempted'
},
'interrupted': {
'MONITOR': 'invalid'
}
})
smach.StateMachine.add(
'DRAW_WITH_MONITOR',
sm_draw_with_monitor,
transitions={'interrupted': 'WAIT_FOR_CLEAR'})
with sm_draw_with_monitor:
smach.Concurrence.add(
'DRAW',
SimpleActionState('turtle_shape2',
turtle_actionlib.msg.ShapeAction,
goal=turtle_actionlib.msg.ShapeGoal(
edges=6, radius=0.5)))
smach.Concurrence.add(
'MONITOR',
MonitorState("turtle1/pose", turtlesim.msg.Pose,
monitor_callback))
# Commented out to run as actionlib server
#start introspection server to use smach_viewer.py
sis = smach_ros.IntrospectionServer('server_name', sm_root, '/SM_ROOT')
sis.start()
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm_root.execute)
smach_thread.start()
#outcome = sm_root.execute()
rospy.spin()
sis.stop()
sm_root.request_preempt()
def __init__(self):
rospy.init_node('random_patrol', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
#setup_task_environment(self)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi / 2, pi, 3 * pi / 2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
self.waypoints.append(
Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(
Pose(Point(self.square_size, self.square_size, 0.0),
quaternions[1]))
self.waypoints.append(
Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
# Initialize the patrol state machine
self.sm_patrol = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Set the userdata.waypoints variable to the pre-defined waypoints
self.sm_patrol.userdata.waypoints = self.waypoints
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('PICK_WAYPOINT',
PickWaypoint(),
transitions={
'succeeded': 'NAV_WAYPOINT',
'aborted': 'NAV_WAYPOINT'
},
remapping={'waypoint_out': 'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT',
Nav2Waypoint(),
transitions={
'succeeded': '',
'aborted': ''
},
remapping={'waypoint_in': 'patrol_waypoint'})
# Create the nav_patrol state machine using a Concurrence container
self.sm_concurrent = Concurrence(
outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_NAV', self.sm_patrol)
Concurrence.add(
'MONITOR_TIME',
MonitorState("/GENERAL/remain", Int32, self.time_cb))
#Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('CONCURRENT',
self.sm_concurrent,
transitions={
'succeeded': 'CONCURRENT',
'stop': 'STOP'
})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
