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'})