def main():
rospy.init_node('s_m', anonymous=True)
base = smach.Concurrence(outcomes=['looking1', 'looking2', 'looking3', 'looking4',
'sim','base_reset'],
default_outcome='base_reset',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with base:
smach.Concurrence.add('WAITING', Waiting())
smach.Concurrence.add('SENSOR1', smach_ros.MonitorState("chat",
String, monitor_cb1))
smach.Concurrence.add('SENSOR2', smach_ros.MonitorState("chat",
String, monitor_cb2))
smach.Concurrence.add('SENSOR3', smach_ros.MonitorState("chat",
String, monitor_cb3))
smach.Concurrence.add('SENSOR4', smach_ros.MonitorState("chat",
String, monitor_cb4))
smach.Concurrence.add('SENSOR5', smach_ros.MonitorState("chat",
String, monitor_cb5))
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['done'])
# Open the container
with sm:
smach.StateMachine.add('SETUP', Setup(), transitions={'setup_done':'BASE'})
smach.StateMachine.add('BASE', base, transitions={'looking1':'GOAL_1',
'looking2':'GOAL_2',
'looking3':'GOAL_3',
'looking4':'GOAL_4',
'sim':'SIM',
'base_reset':'BASE'})
smach.StateMachine.add('GOAL_1', gaze1(), {'succeeded':'BASE'})
smach.StateMachine.add('GOAL_2', gaze2(), {'succeeded':'BASE'})
smach.StateMachine.add('GOAL_3', gaze3(), {'succeeded':'BASE'})
smach.StateMachine.add('GOAL_4', gaze4(), {'succeeded':'BASE'})
smach.StateMachine.add('SIM', afirmativo(), {'succeeded':'BASE'})
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome6'])
# Open the container
with sm_top:
smach.StateMachine.add('INIT',
smach_ros.MonitorState("/sm_reset", Empty,
monitor_cb),
transitions={
'invalid': 'CON',
'valid': 'INIT',
'preempted': 'INIT'
})
# Create the sub SMACH state machine
sm_con = smach.Concurrence(
outcomes=['outcome4', 'outcome5'],
default_outcome='outcome4',
outcome_map={'outcome5': {
'FOO': 'outcome2',
'BAR': 'outcome1'
}})
# Open the container
with sm_con:
# Add states to the container
smach.Concurrence.add('FOO',
smach_ros.MonitorState(
"/sm_reset2", Empty, monitor_cb),
transitions={
'invalid': 'outcome2',
'valid': 'FOO',
'preempted': 'FOO'
})
smach.Concurrence.add('BAR', Bar())
smach.StateMachine.add('CON',
sm_con,
transitions={
'outcome4': 'CON',
'outcome5': 'outcome6'
})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('my_smach_introspection_server',
sm_top, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
rospy.init_node('st_mach')
rospy.loginfo('Inited node st_mach')
global pub
pub = rospy.Publisher('/drone_state', String, queue_size=10)
rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
rospy.Subscriber('/detected_objects', PoseArray, HumFound)
objects_history = []
global battery
battery = None
# creating the concurrence state machine
flying_concurrence = smach.Concurrence(
outcomes=['patrol_done', 'low_bat', 'hovering'],
default_outcome='patrol_done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with flying_concurrence:
smach.Concurrence.add(
'START_HOVERING',
smach_ros.MonitorState("/sm_hover", Empty, monitor_cb))
smach.Concurrence.add(
'RETURN_TO_BASE',
smach_ros.MonitorState("/sm_charge", Empty, monitor_cb))
smach.Concurrence.add('PATROL_MODE', Flying())
smach.Concurrence.add('BATTERY_MONITOR', Bat_Monitor())
smach.Concurrence.add('OBJECT_FOUND', Camera_Search())
sm = smach.StateMachine(outcomes=['charging'])
with sm:
smach.StateMachine.add('BASE_CHARGING',
Base(),
transitions={'setup_done': 'FLYING'})
smach.StateMachine.add('FLYING',
Flying(),
transitions={
'patrol_done': 'RETURN_TO_BASE',
'low_bat': 'RETURN_TO_BASE',
'hovering': 'HOVER'
})
smach.StateMachine.add('RETURN_TO_BASE',
Landing(),
transitions={'landing_succeded': 'charging'})
smach.StateMachine.add('HOVER',
Hover(),
transitions={'hovering_timeout': 'FLYING'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node("monitor_example")
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
smach.StateMachine.add('PATH_FOLLOWING', smach_ros.MonitorState("/gripper_mode/object", Point, gripper_cb), transitions={'invalid':'READY_TO_GRAB', 'valid':'PATH_FOLLOWING', 'preempted':'PATH_FOLLOWING'})
smach.StateMachine.add('READY_TO_GRAB',smach_ros.MonitorState("/gripper_mode/grab_object", Bool, grab_cb), transitions={'invalid':'GO_TO_STOP_SIGN', 'valid':'READY_TO_GRAB', 'preempted':'READY_TO_GRAB'})
smach.StateMachine.add('GO_TO_STOP_SIGN',smach_ros.MonitorState("/gripper_mode/finished", Bool, grab_cb), transitions={'invalid':'DONE', 'valid':'GO_TO_STOP_SIGN', 'preempted':'GO_TO_STOP_SIGN'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.on_shutdown(onShutdown)
rospy.spin()
sis.stop()
def main():
rospy.init_node("monitor_example")
# Classe de maquina de estado
sm = smach.StateMachine(outcomes=['DONE'])
# Adicao dos estados e as transicoes
with sm:
# Estado do tipo monitor (obrigatoriamente os transitions definidos, serve para acompanhar um topico)
smach.StateMachine.add('ESTEIRA',
smach_ros.MonitorState(
"/rosi/arms_joints_position",
RosiMovementArray, arm_cb),
transitions={
'invalid': 'WAYPOINT_1',
'valid': 'ESTEIRA',
'preempted': 'ESTEIRA'
})
# Estado normal, define os transitions como quiser e quantos quiser
smach.StateMachine.add('BAR',
bar(),
transitions={'bar_succeeded': 'WAYPOINT_1'})
# Estado normal, define os transitions como quiser e quantos quiser
smach.StateMachine.add('WAYPOINT_1',
Waypoint(waypoints[0][1], waypoints[0][2]),
transitions={'success': 'BAR'})
#smach.StateMachine.add('WAYPOINT_2', Waypoint(waypoints[1][1], waypoints[1][2]), transitions={'success':'BAR'})
sis = smach_ros.IntrospectionServer(
'smach_server', sm,
'/SM_ROOT') # era para visualizar os estados, mas nao funcionou
sis.start()
while not rospy.is_shutdown():
sm.execute() # executa a maquina de estados
rospy.spin()
sis.stop()
def main(self):
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
self.sm_top = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
# Open the container
with self.sm_top:
# Add states to the container
smach.StateMachine.add('Mon', smach_ros.MonitorState(
"/sm_reset", String, self.monitor_cb, output_keys=['mon_data'], input_keys=['sm_data']),
transitions={'invalid':'Controller', 'valid':'Controller', 'preempted':'Controller'},
remapping={'mon_data':'sm_data'})
smach.StateMachine.add('Controller', Controller(),
transitions={'outcome1':'Controller',
'outcome2':'Second',
'outcome3':'Mon'},
remapping={'cont_input':'sm_data'})
smach.StateMachine.add('First', First(),
transitions={'outcome2':'outcome4'})
smach.StateMachine.add('Second', Second(),
transitions={'outcome2':'outcome4'})
# Execute SMACH plan
outcome = self.sm_top.execute()
def main():
rospy.init_node("monitor_example")
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
smach.StateMachine.add('FOO',
smach_ros.MonitorState("/sm_reset", Empty,
monitor_cb),
transitions={
'invalid': 'BAR',
'valid': 'BAR2',
'preempted': 'FOO'
})
smach.StateMachine.add('BAR',
bar(),
transitions={'bar_succeeded': 'FOO'})
smach.StateMachine.add('BAR2',
bar(),
transitions={'bar_succeeded': 'FOO'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
# rospy.init_node('competition_smach')
rospy.init_node('competition_smach', log_level=rospy.DEBUG)
# Top level state machine. Seperates the autonomy from the teleop machines.
sm_top = smach.StateMachine(outcomes=['DONE'])
with sm_top:
smach.StateMachine.add('AUTONOMY', smach_ros.MonitorState('/teleop_toggle', Empty, monitor_cb), transitions={'invalid':'TELEOP', 'valid':'AUTONOMY', 'preempted':'AUTONOMY'})
smach.StateMachine.add('TELEOP', smach_ros.MonitorState('/teleop_toggle', Empty, monitor_cb), transitions={'invalid':'AUTONOMY', 'valid':'TELEOP', 'preempted':'TELEOP'})
# sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/COMPETITION_SMACH')
# sis.start()
sm_top.execute()
rospy.spin()
# sis.stop()
sm_top.request_preempt()
def handle_cam_sm():
sm = smach.StateMachine(outcomes=['preempted', 'aborted','failed', 'TBD'])
with sm:
smach.StateMachine.add('RCV_CAM', smach_ros.MonitorState("/networking/cam", Cam, rcv_cam, max_checks=1, output_keys=['msg']), transitions={'invalid':'failed', 'valid':'PROCESS_CAM'})
smach.StateMachine.add('PROCESS_CAM', ProcessCam(), transitions={'done':'RCV_CAM'})
return sm
def main():
rospy.init_node("pepper_imitation_game_node")
top_sm = smach.Concurrence(outcomes = ['game_success', 'game_error', 'game_canceled'],
default_outcome = 'game_error', child_termination_cb = game_termination_cb, outcome_cb = game_outcome_cb)
with top_sm:
main_sm = smach.StateMachine(outcomes = ['main_success', 'main_failed', 'main_preempted'])
with main_sm:
smach.StateMachine.add('SAY_HI', smach.CBState(say_hi), transitions = {'finished' : 'WAIT_USER_INPUT'})
smach.StateMachine.add('WAIT_USER_INPUT', WaitUserInput(), transitions={ 'start':'INIT_GAME', 'stop':'main_success', 'preempted': 'GAME_STOPPED', 'waiting':'WAIT_USER_INPUT'})
smach.StateMachine.add('INIT_GAME', smach.CBState(init_game), transitions = {'finished' : 'GAME_ITERATION'})
smach.StateMachine.add('GAME_ITERATION', GameIteration(), transitions={ 'continue':'SYNC_MUSIC', 'game_over':'END_SESSION', 'preempted': 'GAME_STOPPED'},
remapping={'previous_imitation_succeeded':'game_state_result',
'synchro_time':'game_state_synchro_time',
'next_pose':'game_state_pose'})
smach.StateMachine.add('SYNC_MUSIC', smach_ros.MonitorState("pepper_imitation/audio_player_progress", \
std_msgs.msg.Float32, synchronize_song, input_keys = ['synchro_time']), transitions = {'valid':'SYNC_MUSIC', 'invalid':'SEND_POSE', 'preempted': 'GAME_STOPPED'},
remapping = {'synchro_time':'game_state_synchro_time'})
smach.StateMachine.add('SEND_POSE', smach.CBState(send_pose), transitions = {'finished' : 'CHECK_POSE'}, remapping = {'pose':'game_state_pose'})
smach.StateMachine.add('CHECK_POSE', CheckPoseState(), transitions={'waiting':'CHECK_POSE', 'finished':'GIVE_FEEDBACK', 'no_skeleton':'GET_SKELETON', 'preempted': 'GAME_STOPPED'}, remapping = {'detection_succeeded':'game_state_result'})
smach.StateMachine.add('GIVE_FEEDBACK', smach.CBState(give_feedback), transitions = {'finished' : 'GAME_ITERATION'}, remapping = {'positive_feedback':'game_state_result'})
smach.StateMachine.add('END_SESSION', smach.CBState(end_session), transitions = {'finished' : 'WAIT_USER_INPUT'})
smach.StateMachine.add('GAME_STOPPED', smach.CBState(game_stopped), transitions = {'finished' : 'main_preempted'})
skeleton_sm = smach.StateMachine(outcomes = ['skeleton_found', 'timeout', 'preempted'])
with skeleton_sm:
smach.StateMachine.add('WAIT_SKELETON_INIT', smach.CBState(wait_skeleton_init), transitions = {'finished' : 'WAIT_SKELETON'})
smach.StateMachine.add('WAIT_SKELETON', WaitSkeletonState(), transitions={ 'skeleton_found':'SKELETON_FOUND', 'skeleton_not_found':'SKELETON_NOT_FOUND', 'waiting':'WAIT_SKELETON', 'preempted': 'preempted' })
smach.StateMachine.add('SKELETON_FOUND', smach.CBState(skeleton_found), transitions = {'finished' : 'skeleton_found'})
smach.StateMachine.add('SKELETON_NOT_FOUND', smach.CBState(skeleton_not_found), transitions = {'finished' : 'timeout'})
smach.StateMachine.add("GET_SKELETON", skeleton_sm, transitions = { 'skeleton_found':'INIT_GAME', 'timeout':'main_failed', 'preempted':'GAME_STOPPED' })
smach.Concurrence.add('GAME', main_sm)
smach.Concurrence.add('USER_EXIT_GAME', smach_ros.MonitorState("/pepper_imitation/cmd_user", pepper_imitation.msg.UserCommand, check_user_exit))
introspection_server = smach_ros.IntrospectionServer('pepper_imitation_game_state', top_sm, '/PEPPER_IMITATION_ROOT')
introspection_server.start()
outcome = top_sm.execute();
rospy.spin()
introspection_server.stop();
def main():
rospy.init_node('monitor_example')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome4'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('SU_FH',
Speedup_FH(),
transitions={'outcome1': 'CK_FH'})
smach.StateMachine.add('CK_FH',
smach_ros.MonitorState("/sm_reset", Empty,
monitor_cb),
transitions={
'invalid': 'SU_PF',
'valid': 'SU_FH',
'preempted': 'SU_FH'
})
smach.StateMachine.add('SU_PF',
Speedup_PF(),
transitions={'outcome2': 'CK_PF'})
smach.StateMachine.add('CK_PF',
smach_ros.MonitorState("/sm_reset2", Empty,
monitor_cb),
transitions={
'invalid': 'FINI',
'valid': 'SU_PF',
'preempted': 'SU_PF'
})
smach.StateMachine.add('FINI',
Fini(),
transitions={'outcome3': 'outcome4'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm,
'/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['done', 'exit'])
# Open the container
with sm_top:
goal1 = MoveBaseGoal()
goal1.target_pose.header.frame_id = "dtw_robot1/map"
goal1.target_pose.pose.position.x = 0.5
goal1.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE1',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal1),
transitions={
'succeeded': 'TASK2',
'preempted': 'done',
'aborted': 'done'
})
task2_concurrence = smach.Concurrence(
outcomes=['done', 'exit'],
default_outcome='done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with task2_concurrence:
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = "dtw_robot1/map"
goal2.target_pose.pose.position.x = -0.5
goal2.target_pose.pose.orientation.w = 1.0
smach.Concurrence.add(
'MOVE2',
smach_ros.SimpleActionState('/dtw_robot1/move_base',
MoveBaseAction,
goal=goal2))
smach.Concurrence.add(
'MONITOR2',
smach_ros.MonitorState("/sm_stop", Empty, monitor_cb))
smach.StateMachine.add('TASK2',
task2_concurrence,
transitions={
'done': 'done',
'exit': 'exit'
})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT')
sis.start()
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('starbaby_state_node')
# Init publishers
global cmd_vel_publisher
cmd_vel_publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Create subscribers
rospy.Subscriber("teleop_cmd_vel", Twist, teleop_cmd_vel_callback)
rospy.Subscriber("auto_cmd_vel", Twist, auto_cmd_vel_callback)
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['DONE'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('MANUAL',
smach_ros.MonitorState(
"/starbaby/mode_auto", Bool,
manual_mode_auto_handler),
transitions={
'invalid': 'AUTO',
'valid': 'MANUAL',
'preempted': 'MANUAL'
})
smach.StateMachine.add('AUTO',
smach_ros.MonitorState("/starbaby/mode_auto",
Bool,
auto_mode_auto_handler),
transitions={
'invalid': 'MANUAL',
'valid': 'AUTO',
'preempted': 'AUTO'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/starbaby')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def build_raise_lower_boom():
btn_monitors = smach.Concurrence(outcomes=['raise','lower','raise_auto','lower_auto','succeeded'],
default_outcome='succeeded',
outcome_map={
"raise":{'MOVE_UP_BTN':'invalid','MOVE_DOWN_BTN':'preempted'},
"lower":{'MOVE_UP_BTN':'preempted','MOVE_DOWN_BTN':'invalid'},
"raise_auto":{"MOVE_UP_AUTO":"invalid"},
"lower_auto":{"MOVE_DOWN_AUTO":"invalid"},
},
child_termination_cb=force_preempt)
with btn_monitors:
smach.Concurrence.add("MOVE_UP_BTN", smach_ros.MonitorState("/fmLib/joy", Joy, up_pressed, -1))
smach.Concurrence.add("MOVE_DOWN_BTN", smach_ros.MonitorState("/fmLib/joy", Joy, down_pressed, -1))
smach.Concurrence.add("MOVE_UP_AUTO", smach_ros.MonitorState("/fmData/boom_state",Int8,up_requested, -1))
smach.Concurrence.add("MOVE_DOWN_AUTO", smach_ros.MonitorState("/fmData/boom_state",Int8,down_requested, -1))
sm = smach.StateMachine(outcomes=["succeeded","aborted","preempted"])
with sm:
smach.StateMachine.add("MONITOR_BUTTONS",
btn_monitors,
transitions={"raise":"MOVE_UP",
"lower":"MOVE_DOWN",
"succeeded":"MONITOR_BUTTONS",
"raise_auto":"MOVE_UP",
"lower_auto":"MOVE_DOWN"})
smach.StateMachine.add("MOVE_UP",
smach_ros.SimpleActionState("/fmImplements/RowClean/move_tool",move_tool_simpleAction,goal=move_tool_simpleGoal(direction=1,timeout=12)),
transitions = {"succeeded":"MONITOR_BUTTONS","aborted":"aborted","preempted":"preempted"}
)
smach.StateMachine.add("MOVE_DOWN",
smach_ros.SimpleActionState("/fmImplements/RowClean/move_tool",move_tool_simpleAction,goal=move_tool_simpleGoal(direction=0,timeout=12)),
transitions = {"succeeded":"MONITOR_BUTTONS","aborted":"aborted","preempted":"preempted"}
)
return sm
def build():
behaviour = smach.StateMachine(outcomes=['inspectionDone','preempted','aborted'], input_keys=['next_x', 'next_y'])
# Wrigle goals
wriggle_left_goal = make_turnGoal(amount=1 , vel=0.3, forward_vel=0)
wriggle_right_goal = make_turnGoal(amount=-2, vel=0.3, forward_vel=0)
wriggle_center_goal = make_turnGoal(amount=1, vel=0.3, forward_vel=0)
with behaviour:
smach.StateMachine.add('get_next_search_point', get_step_towards_point.getStepTowardsPoint(),transitions={'succeeded':'wriggle_left'})
smach.StateMachine.add('wriggle_left', smach_ros.SimpleActionState('/fmExecutors/make_turn', make_turnAction, wriggle_left_goal), transitions={'succeeded':'wriggle_right','preempted':'preempted','aborted':'aborted'})
smach.StateMachine.add('wriggle_right', smach_ros.SimpleActionState('/fmExecutors/make_turn', make_turnAction, wriggle_right_goal), transitions={'succeeded':'wriggle_center','preempted':'preempted','aborted':'aborted'})
smach.StateMachine.add('wriggle_center', smach_ros.SimpleActionState('/fmExecutors/make_turn', make_turnAction, wriggle_center_goal), transitions={'succeeded':'go_to_next_search_point','preempted':'preempted','aborted':'aborted'})
smach.StateMachine.add('go_to_next_search_point', smach_ros.SimpleActionState('/fmExecutors/position_planner',positionAction, goal_slots=['x','y']),
transitions={'succeeded':'check_no_mines','preempted':'preempted','aborted':'aborted'},
remapping={'x':'step_next_x','y':'step_next_y'})
smach.StateMachine.add('check_no_mines', smach_ros.MonitorState("/wads", Float64, mine_detect_cb, 1), transitions={'valid':'inspectionDone', 'invalid':'get_next_search_point', 'preempted':'preempted'})
return behaviour
def build_row_nav_sm(row_goal, timeout):
"""
Build the navigate in-row statemachine
This statemachine only navigates the row and returns succeeded when
entering headland
"""
find_row_timeout_sm = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='aborted',
outcome_map={
"aborted": {
'TIMEOUT': 'succeeded',
'FIND_ROW': 'preempted'
},
"succeeded": {
'FIND_ROW': 'invalid'
}
},
child_termination_cb=force_preempt)
with find_row_timeout_sm:
smach.Concurrence.add(
"FIND_ROW",
smach_ros.MonitorState("/fmExtractors/rows", row, on_rows, -1))
smach.Concurrence.add("TIMEOUT", WaitState(timeout))
row_navigator = smach.StateMachine(
outcomes=["succeeded", "aborted", "preempted"])
with row_navigator:
smach.StateMachine.add(
"ROW_NAVIGATOR",
smach_ros.SimpleActionState("/fmExecutors/navigate_in_row_simple",
navigate_rowAction,
goal=row_goal),
transitions={
"succeeded": "succeeded",
"aborted": "aborted",
"preempted": "preempted"
},
)
return row_navigator
def token_sm_member():
# Create top level state machine
sm = TokenPassing()
sm.userdata.flash_pattern = 'flash'
with sm:
smach.StateMachine.add(
'WAIT_FOR_TOKEN',
smach_ros.MonitorState('/networking/token',
Token,
check_token,
max_checks=1,
input_keys=['robot_id']), {
'valid': 'FLASH_LED',
'invalid': 'WAIT_FOR_TOKEN'
})
smach.StateMachine.add('FLASH_LED', FlashLED(), {'done': 'SEND_TOKEN'})
smach.StateMachine.add('SEND_TOKEN', PublishMessage('TOKEN'),
{'done': 'WAIT_FOR_TOKEN'})
# Return final statemachine
return sm
def main():
rospy.init_node("moveit_playback_demonstration")
ROSBAG_PARAM_NAME = "~bagfile"
rospy.set_param(ROSBAG_PARAM_NAME, "~/data/demo_01.bag")
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
# Simple monitor state
smach.StateMachine.add('IDLE', smach_ros.MonitorState("/start_playback", String, transition_cb, output_keys=['bag_file', 'eef_flag','target_topic']), transitions={'invalid': 'PLAYBACK', 'valid':'IDLE', 'preempted':'IDLE'})
# Example below on how to set the bag file with a specific state init
#smach.StateMachine.add('PLAYBACK', MoveitPlaybackBagState(bagfile="/home/ddroids/data/demo_01.bag", eef_flag=False, pose_topic='joint_states'), transitions={'playback_finished':'DONE'})
# Example of passing in a rosparam location for the bagfile name to be
smach.StateMachine.add('PLAYBACK', MoveitPlaybackBagState(bagparam=ROSBAG_PARAM_NAME, eef_flag=False, pose_topic='joint_states'), transitions={'playback_finished':'DONE'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node("preemption_example")
foo_concurrence = smach.Concurrence(outcomes=['foo_done', 'foo_reset'],
default_outcome='foo_done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with foo_concurrence:
smach.Concurrence.add('FOO_CALC', foo())
smach.Concurrence.add(
'FOO_RESET', smach_ros.MonitorState("/sm_reset", Empty,
monitor_cb))
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
smach.StateMachine.add('SETUP',
setup(),
transitions={'setup_done': 'FOO'})
smach.StateMachine.add('FOO',
foo_concurrence,
transitions={
'foo_done': 'BAR',
'foo_reset': 'SETUP'
})
smach.StateMachine.add('BAR',
foo(),
transitions={
'foo_succeeded': 'FOO',
'preempted': 'SETUP'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('smach_example_state_machine')
self.last_bar_state = None
# Create the top level SMACH state machine
self.sm_top = smach.StateMachine(outcomes=['stop'])
# Open the container
with self.sm_top:
smach.StateMachine.add('BAS',
Bas(),
transitions={'succeeded': 'ZOMBIE'})
sm_zombie = smach.StateMachine(outcomes=['power_off', 'resume'])
with sm_zombie:
smach.StateMachine.add('PLATFORM_CHECK',
PlatformCheck(),
transitions={
'platform_check_ok':
'PLATFORM_STATE_LISTENING'
})
smach.StateMachine.add('PLATFORM_STATE_LISTENING',
smach_ros.MonitorState(
"/platform_resume", Empty,
platform_resume_monitor_cb),
transitions={
'invalid': 'power_off',
'valid': 'resume',
'preempted': 'power_off'
})
smach.StateMachine.add('ZOMBIE',
sm_zombie,
transitions={
'power_off': 'END',
'resume': 'BAS'
})
def main():
rospy.init_node('preemption_example')
foo_concurrence = smach.Concurrence(
outcomes=['concurrence_done', 'concurrence_reset'],
default_outcome='concurrence_done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with foo_concurrence:
smach.Concurrence.add('CALC', calc())
smach.Concurrence.add(
'RESET', smach_ros.MonitorState('/sm_reset', Empty, reset_cb))
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
smach.StateMachine.add('SETUP',
setup(),
transitions={'setup_done': 'CONCURRENCE'})
smach.StateMachine.add('CONCURRENCE',
foo_concurrence,
transitions={
'concurrence_done': 'BAR',
'concurrence_reset': 'SETUP'
})
smach.StateMachine.add('BAR',
calc(),
transitions={
'calc_succeeded': 'CONCURRENCE',
'preempted': 'SETUP'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('Stateovi')
# Create a SMACH state machine
sm = smach.StateMachine(['succeeded', 'aborted', 'preempted'])
sm.userdata.time = 3.0
sm.userdata.body_pose = "Stand"
sm.userdata.vocabulary_data = 'yes, please, no, love, kill'
sm.userdata.behavior = 'animations/Stand/Gestures/Stretch_2'
sm.userdata.joint_names = ''
sm.userdata.joint_points = [0]
sm.userdata.word = ""
sm.userdata.word_conf = 0
# Open the container
with sm:
# Wake up robot
smach.StateMachine.add('wake_up',
ServiceState('/wakeup', Empty),
transitions={
'succeeded': 'wait_0',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.userdata.time = 7
smach.StateMachine.add('wait_0',
Wait(),
transitions={
'succeeded': 'vocabulary_0',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
# Fill in the vocabulary
sm.userdata.vocabulary_data = 'happy, angry, fear, sorrow, suprise'
smach.StateMachine.add(
'vocabulary_0',
Fill_Vocabulary(),
transitions={
'succeeded': 'asr_start_1',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'vocabulary_data': 'vocabulary_data'})
# start asr
smach.StateMachine.add('asr_start_1',
ServiceState('/start_asr', SetBool, True),
transitions={
'succeeded': 'tts_happy',
'aborted': 'aborted',
'preempted': 'preempted'
})
# happy 1
smach.StateMachine.add('tts_happy',
ServiceState(
'/tts_speech', SetString,
'Now I will show you my Happiness'),
transitions={
'succeeded': 'run_behavior_1',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.userdata.behavior = 'animations/Stand/Emotions/Positive/Happy_2'
smach.StateMachine.add('run_behavior_1',
Run_Behavior(),
transitions={
'succeeded': 'wait_1',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'behavior': 'behavior'})
sm.userdata.time = 5
smach.StateMachine.add('wait_1',
Wait(),
transitions={
'succeeded': 'tts_angry',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
# angry 2
smach.StateMachine.add('tts_angry',
ServiceState('/tts_speech', SetString,
'Now I will show you my Anger'),
transitions={
'succeeded': 'run_behavior_2',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.userdata.behavior = 'animations/Stand/Emotions/Negative/Angry_2'
smach.StateMachine.add('run_behavior_2',
Run_Behavior(),
transitions={
'succeeded': 'wait_2',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'behavior': 'behavior'})
sm.userdata.time = 5
smach.StateMachine.add('wait_2',
Wait(),
transitions={
'succeeded': 'tts_fear',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
# fear 3
smach.StateMachine.add('tts_fear',
ServiceState('/tts_speech', SetString,
'Now I will show you my fear'),
transitions={
'succeeded': 'run_behavior_3',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.userdata.behavior = 'animations/Stand/Emotions/Negative/Fear_2'
smach.StateMachine.add('run_behavior_3',
Run_Behavior(),
transitions={
'succeeded': 'wait_3',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'behavior': 'behavior'})
sm.userdata.time = 5
smach.StateMachine.add('wait_3',
Wait(),
transitions={
'succeeded': 'tts_sad',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
# sad 4
smach.StateMachine.add('tts_sad',
ServiceState('/tts_speech', SetString,
'Now I will show you my sadness'),
transitions={
'succeeded': 'run_behavior_4',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.userdata.behavior = 'animations/Stand/Emotions/Negative/Sad_2'
smach.StateMachine.add('run_behavior_4',
Run_Behavior(),
transitions={
'succeeded': 'wait_4',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'behavior': 'behavior'})
sm.userdata.time = 5
smach.StateMachine.add('wait_4',
Wait(),
transitions={
'succeeded': 'tts_suprise',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
# suprise 5
smach.StateMachine.add('tts_suprise',
ServiceState('/tts_speech', SetString,
'Now I will show you my suprise'),
transitions={
'succeeded': 'run_behavior_5',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.userdata.behavior = 'animations/Stand/Emotions/Negative/Surprise_2'
smach.StateMachine.add('run_behavior_5',
Run_Behavior(),
transitions={
'succeeded': 'wait_5',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'behavior': 'behavior'})
sm.userdata.time = 5
smach.StateMachine.add('wait_5',
Wait(),
transitions={
'succeeded': 'rest',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
################################ ASR TTS #########################################
# Say word via tts service
smach.StateMachine.add('tts_1',
ServiceState('/tts_speech', SetString, 'Yes'),
transitions={
'succeeded': 'tts_2',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say word via tts service and animate while talking
smach.StateMachine.add('tts_2',
ServiceState(
'/tts_animation', SetString,
'your order for fifteen thousand robots'),
transitions={
'succeeded': 'vocabulary',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Fill in the vocabulary
sm.userdata.vocabulary_data = 'yes, please, no, love, kill'
smach.StateMachine.add(
'vocabulary',
Fill_Vocabulary(),
transitions={
'succeeded': 'asr_start',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'vocabulary_data': 'vocabulary_data'})
# start asr
smach.StateMachine.add('asr_start',
ServiceState('/start_asr', SetBool, True),
transitions={
'succeeded': 'wait_for_word',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Goal callback for state wait_for_word
def word_callback(userdata, msg):
words_str = ""
dummy = msg.data
print dummy
flag = 0
for i in range(0, len(dummy) - 1):
if dummy[i] == ">" and flag == 0:
j = i + 1
flag = 1
while not dummy[j] == "<" or dummy[j] == "'":
print dummy[j]
words_str += str(dummy[j])
j = j + 1
#words_str += dummy[len(dummy) - 2]
print words_str.split(" ")
word = words_str
print word
if word == "yes":
return True
elif word == "no":
return False
#print userdata.word
#print userdata.word_conf
#Got detected word # ovo nece moci preko monitor stat jer tu treba ici topic akcije inace ce uvijek vracati invalid state
smach.StateMachine.add('wait_for_word',
smach_ros.MonitorState('/detected_word',
String,
word_callback,
output_keys=['word']),
transitions={
'invalid': 'wait_for_word',
'valid': 'asr_stop',
'preempted': 'preempted'
})
# Wait for some time
smach.StateMachine.add('wait_asr',
Wait(),
transitions={
'succeeded': 'asr_stop',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'time': 'time'})
# stop asr
smach.StateMachine.add('asr_stop',
ServiceState('/start_asr', SetBool, False),
transitions={
'succeeded': 'run_behavior',
'aborted': 'aborted',
'preempted': 'preempted'
})
######################################### Behaviours ##############################
# Run Behaviour
sm.userdata.behavior = 'animations/Stand/Gestures/Stretch_2'
smach.StateMachine.add('run_behavior',
Run_Behavior(),
transitions={
'succeeded': 'pose_StandZero',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'behavior': 'behavior'})
######################################### Pose ##############################
# Go to pose Stand
sm.userdata.body_pose = "StandZero"
smach.StateMachine.add('pose_StandZero',
GoToPose(),
transitions={
'succeeded': 'joint_trajectory',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'pose': 'body_pose'})
# Joint trajectory
# 'HeadYaw, HeadPitch'
# 'LShoulderPitch, LShoulderRoll, LElbowYaw, LElbowRoll, LWristYaw, LHand'
# 'RShoulderPitch, RShoulderRoll, RElbowYaw, RElbowRoll, RWristYaw, RHand'
# 'HipRoll, HipPitch, KneePitch'
sm.userdata.joint_names = 'LShoulderPitch, LShoulderRoll, LElbowYaw, LElbowRoll, LWristYaw, LHand'
sm.userdata.joint_points = [0.5, 0.5, 0.5, 0.5]
smach.StateMachine.add('joint_trajectory',
JointTrajectory(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={
'joint_names': 'joint_names',
'joint_points': 'joint_points',
'time': 'time'
})
# Go to rest pose
smach.StateMachine.add('rest',
ServiceState('/rest', Empty),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
#########################################################################
# Create and start the introspection server
sis = IntrospectionServer('my_smach_introspection_server', sm, '/Sulla')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
rospy.init_node('state_machine_node')
# Create a TOP level SMACH state machine
top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted'])
# Open the container
with top_sm:
# ===================================== SarBehavior =====================================
# Create a State Machine container
sarbehavior_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'])
# Open the container
with sarbehavior_sm:
# ===================================== IdleThreads =====================================
# Callback for custom outcomes from IdleThreads #
def out_cb(outcome_map):
# To be completed
return 'aborted'
# Create a Concurrence container
idlethreads_concurrence = smach.Concurrence(
outcomes=['missionStart', 'aborted'],
default_outcome='missionStart',
child_termination_cb=lambda so: True,
outcome_cb=out_cb)
# Open the container
with idlethreads_concurrence:
# ADD Idle to IdleThreads #
smach.Concurrence.add('Idle',
# NOT_SUPPORTED
)
# ADD IdleEventMonitoring to IdleThreads #
smach.Concurrence.add(
'IdleEventMonitoring',
smach_ros.MonitorState('bridge/events/mission_start',
SimpleEvent,
cond_cb=lambda ud, msg: False))
# ===================================== IdleThreads END =====================================
# ADD IdleThreads to SarBehavior #
smach.StateMachine.add('IdleThreads',
idlethreads_concurrence,
transitions={'missionStart': 'TakeOff'})
# Response callback
def takeoff_response_cb(userdata, response):
# To be completed
return ''
# ADD TakeOff to SarBehavior #
smach.StateMachine.add('TakeOff',
smach_ros.ServiceState(
'cmd/takeoff',
TakeOff,
request_slots=['req1', 'req2'],
response_cb=takeoff_response_cb,
output_keys=['']),
transitions={'succeeded': 'IdleThreads'})
# ===================================== SarBehavior END =====================================
# ADD SarBehavior to TOP state #
smach.StateMachine.add('SarBehavior', sarbehavior_sm, transitions={})
# Create and start the introspection server (uncomment if needed)
# sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP')
# sis.start()
# Execute SMACH plan
outcome = top_sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
def main():
rospy.init_node('smach_usecase_executive')
sm_root = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('RESET', smach_ros.ServiceState(
'reset', Empty),
transitions={'succeeded': 'SPAWN'})
smach.StateMachine.add('SPAWN', smach_ros.ServiceState(
'spawn', Spawn),
transitions={'succeeded': 'TELEPORT1'})
smach.StateMachine.add('TELEPORT1', smach_ros.ServiceState(
'turtle1/teleport_absolute', TeleportAbsolute, request=TeleportAbsoluteRequest(5.0, 1.0, 0.0)),
transitions={'succeeded': 'DRAW_SHAPES'})
sm_concurrence = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
outcome_map={'succeeded': {'BIG': 'succeeded', 'SMALL': 'succeeded'}})
with sm_concurrence:
shape_goal1 = ShapeActionGoal()
shape_goal1.goal.edges = 11
shape_goal1.goal.radius = 3.0
smach.Concurrence.add('BIG', smach_ros.SimpleActionState(
'turtle_shape1', ShapeAction, goal=shape_goal1.goal))
sm_small = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_small:
smach.StateMachine.add('TELEPORT2', smach_ros.ServiceState(
'turtle2/teleport_absolute', TeleportAbsolute, request=TeleportAbsoluteRequest(9.0, 5.0, 0.0)),
transitions={'succeeded': 'DRAW_WITH_MONITOR'})
sm_con_monitor = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'interrupted'],
default_outcome='succeeded',
child_termination_cb=lambda state_outcomes: True,
outcome_map={'succeeded': {'DRAW': 'succeeded'},
'interrupted': {'MONITOR': 'invalid'},
'preempted': {'MONITOR': 'preempted', 'DRAW': 'preempted'}})
with sm_con_monitor:
shape_goal2 = ShapeActionGoal()
shape_goal2.goal.edges = 6
shape_goal2.goal.radius = 1
smach.Concurrence.add('DRAW', smach_ros.SimpleActionState(
'turtle_shape2', ShapeAction, goal=shape_goal2.goal))
smach.Concurrence.add('MONITOR', smach_ros.MonitorState(
"/turtle2_stop", std_msgs.msg.Empty, monitor_cb))
smach.StateMachine.add('DRAW_WITH_MONITOR', sm_con_monitor, transitions={
'interrupted': 'WAIT_FOR_CLEAR'})
smach.StateMachine.add('WAIT_FOR_CLEAR', smach_ros.MonitorState(
"/turtle2_clear", std_msgs.msg.Header, monitor_clear_cb),
transitions={'invalid': 'TELEPORT2', 'valid': 'WAIT_FOR_CLEAR'})
smach.Concurrence.add('SMALL', sm_small)
smach.StateMachine.add('DRAW_SHAPES', sm_concurrence)
sis = smach_ros.IntrospectionServer('smach_server', sm_root, '/SM_ROOT')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
def main():
""" main execution """
rospy.init_node("find_ground_fire_motion")
sm = smach.StateMachine(outcomes=['DONE', 'FAIL'])
with sm:
nav_control_rate = rospy.get_param('~control_rate', 5.0)
nav_approach_margin = rospy.get_param('~navigation_approach_margin',
[0.1, 0.1, 0.1])
nav_mode = rospy.get_param('~nav_mode', 2)
search_params = {}
search_type = rospy.get_param('~search_type', 'rectangular_grid')
search_params['nav_mode'] = nav_mode
search_params['target_topic_name'] = rospy.get_param(
'~target_topic_name', '/target_object/pos')
search_params['control_rate'] = rospy.get_param('~control_rate', 5.0)
search_params['search_grid_size'] = rospy.get_param(
'~search_grid_size', 1.0)
search_params['reach_margin'] = rospy.get_param('~reach_margin', 0.2)
search_params['search_height'] = rospy.get_param('~search_height', 3.0)
search_params['target_timeout'] = rospy.get_param(
'~target_timeout', 1.0)
search_params['approach_height'] = rospy.get_param(
'~approach_height', 3.0)
search_params['descending_height'] = rospy.get_param(
'~descending_height', 2.0)
search_params['approach_margin'] = rospy.get_param(
'~approach_margin', 0.05)
search_params['height_margin'] = rospy.get_param(
'~height_margin', 0.05)
search_params['covering_pre_height'] = rospy.get_param(
'~covering_pre_height', 1.0)
search_params['covering_post_height'] = rospy.get_param(
'~covering_post_height', 0.2)
search_params['covering_move_dist'] = rospy.get_param(
'~covering_move_dist', 1.0)
search_params['search_area_length'] = rospy.get_param(
'~search_area_length', 2.0)
search_params['trip_number'] = rospy.get_param('~trip_number', 5)
search_params['is_bypass'] = rospy.get_param('~is_bypass', False)
smach.StateMachine.add('INITIAL_STATE',
smach_ros.MonitorState("~task3_start", Empty,
monitor_cb),
transitions={
'invalid': 'TAKEOFF',
'valid': 'INITIAL_STATE',
'preempted': 'INITIAL_STATE'
})
smach.StateMachine.add('TAKEOFF',
TakeoffState(),
transitions={
'success': 'NAVIGATING0',
'fail': 'INITIAL_STATE'
})
search_area_number = rospy.get_param('~search_area_number', 1)
waypoints_list = rospy.get_param('~initial_waypoints', [[[0, 0, 1]]])
area_corners_list = rospy.get_param('~area_corners',
[[[0, 0], [2, 2], 0]])
for i in range(search_area_number):
waypoint_nav_creator = WaypointNavigationStateMachineCreator()
waypoints = waypoints_list[i]
sm_sub_nav = waypoint_nav_creator.create(waypoints,
nav_approach_margin,
nav_control_rate,
nav_mode)
smach.StateMachine.add('NAVIGATING' + str(i),
sm_sub_nav,
transitions={
'success': 'SEARCHING' + str(i),
'failure': 'FAIL'
})
search_params['area_corners'] = area_corners_list[i]
search_sm_creator = PlaneFireFightStateMachineCreator()
sm_sub = search_sm_creator.create(search_type, search_params)
if i == search_area_number - 1:
next_state = 'RESET_LANDING'
else:
next_state = 'NAVIGATING' + str(i + 1)
smach.StateMachine.add('SEARCHING' + str(i),
sm_sub,
transitions={
'success': 'DONE',
'failure': next_state
})
smach.StateMachine.add('RESET_LANDING',
LandState(),
transitions={
'success': 'FAIL',
'fail': 'FAIL'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('state_machine_node')
# Create a TOP level SMACH state machine
top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted'])
# Open the container
with top_sm:
# ===================================== WorkerThreads =====================================
# Callback for custom outcomes from WorkerThreads
def out_cb(outcome_map):
if outcome_map['GoHomeEventMonitoring'] == 'invalid':
rospy.loginfo('Returning goHome Event')
return 'goHome'
return 'aborted'
# Create a Concurrence container
workerthreads_concurrence = smach.Concurrence(
outcomes=['goHome', 'aborted'],
default_outcome='goHome',
child_termination_cb=lambda so: True,
outcome_cb=out_cb)
# Open the container
with workerthreads_concurrence:
# ===================================== WorkerBehavior =====================================
# Create a State Machine container
workerbehavior_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted'])
# Open the container
with workerbehavior_sm:
# ===================================== IdleThreads =====================================
# Callback for custom outcomes from IdleThreads
def out_cb(outcome_map):
if outcome_map['IdleEventMonitoring'] == 'invalid':
rospy.loginfo('Returning boxFound Event')
return 'boxFound'
return 'aborted'
# Create a Concurrence container
idlethreads_concurrence = smach.Concurrence(
outcomes=['boxFound', 'aborted'],
default_outcome='boxFound',
child_termination_cb=lambda so: True,
outcome_cb=out_cb,
output_keys=['box_id', 'scout', 'box_position'])
# Open the container
with idlethreads_concurrence:
# ADD Idle to IdleThreads #
smach.Concurrence.add('Idle',
Idle()
)
def monitor_cb(ud, msg):
rospy.loginfo('Executing monitor_cb')
ud.box_id = msg.id
ud.scout = msg.swarmio.node
ud.box_position = msg.pose
return False
# ADD IdleEventMonitoring to IdleThreads #
smach.Concurrence.add('IdleEventMonitoring',
smach_ros.MonitorState('bridge/events/target_found',
TargetPositionEvent,
cond_cb=monitor_cb,
output_keys=['box_id', 'scout', 'box_position']))
# ===================================== IdleThreads END =====================================
# ADD IdleThreads to WorkerBehavior #
smach.StateMachine.add('IdleThreads',
idlethreads_concurrence,
transitions={'boxFound':'AssignBox'})
# ADD AssignBox to WorkerBehavior #
smach.StateMachine.add('AssignBox',
smach_ros.SimpleActionState('cmd/task_allocation_bid',
TaskAllocationAction,
goal_slots=['task_id', 'task_pose', 'auctioneer'],
result_slots=['task_pose']),
transitions={'succeeded':'MoveToBox', 'aborted':'IdleThreads'},
remapping={'task_id':'box_id', 'task_pose':'box_position', 'auctioneer':'scout'})
# ADD MoveToBox to WorkerBehavior #
smach.StateMachine.add('MoveToBox',
smach_ros.SimpleActionState('cmd/moveto',
MoveBaseAction,
goal_slots=['target_pose']),
transitions={'succeeded':'MoveToDest', 'aborted':'MoveToHome'},
remapping={'target_pose':'box_position'})
# ADD MoveToDest to WorkerBehavior #
def move_dest_cb(ud, goal):
goal = MoveBaseGoal()
goal.target_pose.pose.position.x = 4 # TODO: should be parameter!
goal.target_pose.pose.position.y = 0
goal.target_pose.pose.orientation.w = 1;
rospy.loginfo('Deliver Box to %.2f, %.2f', goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
return goal
smach.StateMachine.add('MoveToDest',
smach_ros.SimpleActionState('cmd/moveto',
MoveBaseAction,
goal_cb=move_dest_cb),
transitions={'succeeded':'PublishState', 'aborted':'MoveToHome'})
# ADD PublishState to WorkerBehavior #
smach.StateMachine.add('PublishState',
smach_ros.SimpleActionState('cmd/target_done',
TargetAction,
goal_slots=['id', 'pose']),
transitions={'succeeded':'MoveToHome', 'aborted':'MoveToHome'},
remapping={'id':'box_id', 'pose':'box_position'})
# ADD MoveToHome to WorkerBehavior #
def move_home_cb(ud, goal):
goal = MoveBaseGoal()
goal.target_pose.pose.position.x = 0 # TODO: should be parameter!
goal.target_pose.pose.position.y = -4
goal.target_pose.pose.orientation.w = 1;
rospy.loginfo('Going home %.2f, %.2f', goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
return goal
smach.StateMachine.add('MoveToHome',
smach_ros.SimpleActionState('cmd/moveto',
MoveBaseAction,
goal_cb=move_home_cb),
transitions={'succeeded':'IdleThreads', 'aborted':'IdleThreads'})
# ===================================== WorkerBehavior END =====================================
# ADD WorkerBehavior to WorkerThreads #
smach.Concurrence.add('WorkerBehavior', workerbehavior_sm)
# ADD GoHomeEventMonitoring to WorkerThreads #
smach.Concurrence.add('GoHomeEventMonitoring',
smach_ros.MonitorState('bridge/events/go_home',
SimpleEvent,
cond_cb=lambda ud, msg: False))
# ===================================== WorkerThreads END =====================================
# ADD WorkerThreads to TOP state #
smach.StateMachine.add('WorkerThreads',
workerthreads_concurrence,
transitions={'goHome':'GoHome'})
def move_home_cb(ud, goal):
goal = MoveBaseGoal()
goal.target_pose.pose.position.x = 0 # TODO: should be parameter!
goal.target_pose.pose.position.y = -4
goal.target_pose.pose.orientation.w = 1;
rospy.loginfo('Going HOME: %.2f, %.2f', goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
return goal
# ADD GoHome to TOP state #
smach.StateMachine.add('GoHome',
smach_ros.SimpleActionState('cmd/moveto',
MoveBaseAction,
goal_cb=move_home_cb),
transitions={})
# Create and start the introspection server (uncomment if needed)
# sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP')
# sis.start()
# Execute SMACH plan
outcome = top_sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
def main():
rospy.init_node('state_machine_node')
# Create a TOP level SMACH state machine
top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted'])
# Open the container
with top_sm:
# ===================================== SarThreads =====================================
# Callback for custom outcomes from SarThreads #
def out_cb(outcome_map):
# To be completed
return 'aborted'
# Create a Concurrence container
sarthreads_concurrence = smach.Concurrence(
outcomes=['missionAbort', 'aborted'],
default_outcome='missionAbort',
child_termination_cb=lambda so: True,
outcome_cb=out_cb
)
# Open the container
with sarthreads_concurrence:
# ===================================== SarBehavior =====================================
# Create a State Machine container
sarbehavior_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted']
)
# Open the container
with sarbehavior_sm:
# ===================================== IdleThreads =====================================
# Callback for custom outcomes from IdleThreads #
def out_cb(outcome_map):
# To be completed
return 'aborted'
# Create a Concurrence container
idlethreads_concurrence = smach.Concurrence(
outcomes=['missionStart', 'aborted'],
default_outcome='missionStart',
child_termination_cb=lambda so: True,
outcome_cb=out_cb
)
# Open the container
with idlethreads_concurrence:
# ADD Idle to IdleThreads #
smach.Concurrence.add('Idle',
# NOT_SUPPORTED
)
# ADD IdleEventMonitoring to IdleThreads #
smach.Concurrence.add('IdleEventMonitoring',
smach_ros.MonitorState('bridge/events/mission_start',
SimpleEvent,
cond_cb=lambda ud, msg: False)
)
# ===================================== IdleThreads END =====================================
# ADD IdleThreads to SarBehavior #
smach.StateMachine.add('IdleThreads',
idlethreads_concurrence,
transitions={'missionStart':'TakeOff'}
)
# ADD TakeOff to SarBehavior #
smach.StateMachine.add('TakeOff',
smach_ros.SimpleActionState('cmd/takeoff',
TakeOffAction,
goal=TakeOffGoal(1.5)),
transitions={'succeeded':'Coverage'}
)
# ADD Coverage to SarBehavior #
smach.StateMachine.add('Coverage',
smach_ros.SimpleActionState('uav_coverage',
CoverageAction),
transitions={'succeeded':'SelectRover'}
)
# ADD SelectRover to SarBehavior #
smach.StateMachine.add('SelectRover',
smach_ros.SimpleActionState('cmd/assign_task',
AssignTaskAction,
goal_slots=['target_id', 'pose']),
transitions={'succeeded':'Tracking', 'aborted':'SelectRover'}
)
# ADD Tracking to SarBehavior #
smach.StateMachine.add('Tracking',
smach_ros.SimpleActionState('uav_tracking',
TrackingAction,
goal_slots=['target', 'cps']),
transitions={'succeeded':'Coverage', 'aborted':'LocalCoverage'}
)
# ADD LocalCoverage to SarBehavior #
smach.StateMachine.add('LocalCoverage',
smach_ros.SimpleActionState('uav_local_coverage',
CoverageAction),
transitions={'aborted':'Coverage', 'succeeded':'Tracking'}
)
# ===================================== SarBehavior END =====================================
# ADD SarBehavior to SarThreads #
smach.Concurrence.add('SarBehavior', sarbehavior_sm)
# ADD AbortEventMonitoring to SarThreads #
smach.Concurrence.add('AbortEventMonitoring',
smach_ros.MonitorState('bridge/events/mission_abort',
SimpleEvent,
cond_cb=lambda ud, msg: False)
)
# ===================================== SarThreads END =====================================
# ADD SarThreads to TOP state #
smach.StateMachine.add('SarThreads',
sarthreads_concurrence,
transitions={'missionAbort':'MissionAbort'}
)
# ===================================== MissionAbort =====================================
# Create a State Machine container
missionabort_sm = smach.StateMachine(
outcomes=['succeeded']
)
# Open the container
with missionabort_sm:
# ADD Land to MissionAbort #
smach.StateMachine.add('Land',
smach_ros.ServiceState('cmd/land',
Empty),
transitions={}
)
# ===================================== MissionAbort END =====================================
# ADD MissionAbort to TOP state #
smach.StateMachine.add('MissionAbort',
missionabort_sm,
transitions={'succeeded':'SarThreads'}
)
# Create and start the introspection server (uncomment if needed)
# sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP')
# sis.start()
# Execute SMACH plan
outcome = top_sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
return 'finished'
if __name__ == "__main__":
# Create a ROS node for this state machine
rospy.init_node("hbp_nrp_backend_sm_exp_control")
sm_one = smach.StateMachine(outcomes=[
'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED',
'ACTION_ERROR'
])
with sm_one:
smach.StateMachine.add(
'CONDITION_30',
smach_ros.MonitorState('/ros_cle_simulation/status', String,
condition_30_cb), {
'valid': 'CONDITION_30',
'invalid': 'ACTION_30',
'preempted': 'CONDITION_PREEMPTED'
})
smach.StateMachine.add('ACTION_30', CBState(action_30_cb),
{'finished': 'FINISHED'})
sm_two = smach.StateMachine(outcomes=[
'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED',
'ACTION_ERROR'
])
with sm_two:
smach.StateMachine.add(
'CONDITION_SPIKE',
def child_term_cb(outcome_map):
if outcome_map['MONITOR_TANGIBLE'] == 'invalid':
return True
else:
return False
sm_monitor = smach.Concurrence(
outcomes=['monitor_continue', 'monitor_done'],
default_outcome='monitor_continue',
input_keys=['tangible_output', 'emotion_output'],
output_keys=['tangible_output', 'emotion_output'],
child_termination_cb=child_term_cb)
with sm_monitor:
smach.Concurrence.add(
'MONITOR_TANGIBLE',
smach_ros.MonitorState("/sm_reset",
String,
monitors.monitor_tangible_cb,
output_keys=['tangible_output_monitor']),
remapping={'tangible_output_monitor': 'tangible_output'})
smach.Concurrence.add(
'MONITOR_EMOTION',
smach_ros.MonitorState("/sm_emotion",
Float32MultiArray,
monitors.monitor_emotion_cb,
output_keys=['emotion_output_monitor']),
remapping={'emotion_output_monitor': 'emotion_output'})
return True
else:
return False
sm_system_robot = smach.Concurrence(outcomes=['system_robot_done','system_robot_continue'],
default_outcome='system_robot_continue',
child_termination_cb = child_term_cb)
sm_system_robot.userdata.robot_command = [0,'blink']
with sm_system_robot:
smach.Concurrence.add('MONITOR_ROBOT',
smach_ros.MonitorState("/robot",
RobotCommand,
monitors.monitor_robot_command,
input_keys=['current_command'],
output_keys=['robot_cmd_output']),
remapping={'robot_cmd_output':'robot_command',
'current_command':'robot_command'})
smach.Concurrence.add('ROBOT_CONTROL',
robot_states.sm_robot_control,
remapping={'robot_cmd_input':'robot_command'})
smach.StateMachine.add('SYSTEM_ROBOT',
sm_system_robot,
transitions={'system_robot_continue':'SYSTEM_ROBOT',
'system_robot_done':'system_robot_shut_down'})