def create(auto_state,joy_topic,btn_index): """ Creates a master state by wrapping the auto state with a manuel mode, where the mode can be changed by pressing the selected wii btn. If the auto state succeeds the state returns to manuel mode. """ manuel_req = switch_muxRequest(); manuel_req.mode = switch_muxRequest.MANUEL auto_req = switch_muxRequest(); auto_req.mode = switch_muxRequest.AUTO auto_wrapper = smach.Concurrence(outcomes=['succeeded','aborted','preempted'], default_outcome='aborted', outcome_map={"preempted":{'MONITOR_BTN':'invalid'}, "succeeded":{'AUTO_STATE':'succeeded'}}, child_termination_cb=should_preempt) auto_wrapper.userdata.btn_id = btn_index; with auto_wrapper: smach.Concurrence.add("MONITOR_BTN",smach_ros.MonitorState(joy_topic, Joy, btn_pressed, max_checks=-1),remapping={"btn_id":"btn_id"}) smach.Concurrence.add("AUTO_STATE",auto_state) master = smach.StateMachine(outcomes=['succeeded','aborted','preempted']) master.userdata.btn_id = btn_index; with master: smach.StateMachine.add("SET_MANUEL_MODE", smach_ros.ServiceState("/fmTools/cmd_vel_mux/cmd_vel_mux", switch_mux, request=manuel_req), transitions={'succeeded':'WAIT'} ) smach.StateMachine.add("WAIT", behaviours.wait_state.WaitState(rospy.Duration(0.5)), transitions={"succeeded":"MONITOR_BTN"}) smach.StateMachine.add("MONITOR_BTN", smach_ros.MonitorState(joy_topic, Joy, btn_pressed, max_checks=-1), transitions={'invalid':'SET_AUTO_MODE','valid':'aborted'}, remapping={"btn_id":"btn_id"} ) smach.StateMachine.add("SET_AUTO_MODE", smach_ros.ServiceState("/fmTools/cmd_vel_mux/cmd_vel_mux", switch_mux, request=auto_req), transitions={'succeeded':'WAIT2','aborted':'SET_MANUEL_MODE'} ) smach.StateMachine.add("WAIT2", behaviours.wait_state.WaitState(rospy.Duration(0.5)), transitions={"succeeded":"AUTO_MODE"}) smach.StateMachine.add("AUTO_MODE", auto_wrapper, transitions={'succeeded':'SET_MANUEL_MODE','preempted':'SET_MANUEL_MODE'}, remapping={"btn_id":"btn_id"} ) return master
if __name__ == "__main__": # # rospy.init_node("MissionMaster") infile = rospy.get_param("~field_path_filename", "path.txt") p_home = load_path(infile) # # Manuel statemachine # manuel_mode = smach.StateMachine(outcomes=['succeeded','preempted','aborted']) manuel_req = switch_muxRequest(); manuel_req.mode = switch_muxRequest.MANUEL auto_req = switch_muxRequest(); auto_req.mode = switch_muxRequest.AUTO with manuel_mode: smach.StateMachine.add("SET_MANUEL_MODE", smach_ros.ServiceState("/fmTools/cmd_vel_mux/cmd_vel_mux", switch_mux, request=manuel_req), transitions={'succeeded':'WAIT'} ) smach.StateMachine.add("WAIT", behaviours.wait_state.WaitState(rospy.Duration(0.5)), transitions={"succeeded":"MONITOR_BTN_A"}) smach.StateMachine.add("MONITOR_BTN_A", smach_ros.MonitorState("/fmHMI/joy", Joy, btn_3_pressed, max_checks=-1),
def main(): rospy.init_node("MissionMaster") # # Manuel statemachine # manuel_mode = smach.StateMachine(outcomes=['succeeded','preempted','aborted']) manuel_req = switch_muxRequest(); manuel_req.mode = switch_muxRequest.MANUEL auto_req = switch_muxRequest(); auto_req.mode = switch_muxRequest.AUTO with manuel_mode: smach.StateMachine.add("SET_MANUEL_MODE", smach_ros.ServiceState("/fmTools/cmd_vel_mux/cmd_vel_mux", switch_mux, request=manuel_req), transitions={'succeeded':'MONITOR_BTN_A'} ) smach.StateMachine.add("MONITOR_BTN_A", smach_ros.MonitorState("/fmHMI/joy", Joy, btn_3_pressed, max_checks=-1), transitions={'invalid':'SET_AUTO_MODE','valid':'aborted'} ) smach.StateMachine.add("SET_AUTO_MODE", smach_ros.ServiceState("/fmTools/cmd_vel_mux/cmd_vel_mux", switch_mux, request=auto_req), transitions={'succeeded':'WAIT'} ) smach.StateMachine.add("WAIT", behaviours.WaitState(0.5),transitions={'succeeded':'preempted'}) # # Drive slow mode # speed_selector = smach.StateMachine(outcomes=['succeeded','aborted','preempted']) speed_toggle = SpeedToggle(0.5,1.0) with speed_selector: smach.StateMachine.add("CHANGE_REQUESTED", smach_ros.MonitorState("/fmHMI/joy", Joy, btn_5_pressed, max_checks=-1), transitions={'invalid':'TOGGLE_SPEED','valid':'aborted'} ) smach.StateMachine.add("TOGGLE_SPEED",smach.CBState(set_speed, cb_args=[speed_toggle], outcomes=['done']), transitions={'done':'WAIT'}) smach.StateMachine.add("WAIT",behaviours.wait_state.WaitState(rospy.Duration(0.5)), transitions={'succeeded':'CHANGE_REQUESTED'}) # # # local_path = smach.StateMachine(outcomes=['succeeded','aborted','preempted']) local_path_goal = generate_local_path() with local_path: smach.StateMachine.add("FOLLOW_PATH_LOCAL", smach_ros.SimpleActionState("/fmExecutors/follow_path",follow_pathAction,local_path_goal)) auto_mode_2 = smach.Concurrence(outcomes=['succeeded','aborted','preempted'], default_outcome='aborted', outcome_map={"preempted":{'MONITOR_BTN_A':'invalid'}, "succeeded":{'AUTO_MODE':'succeeded'}}, child_termination_cb=should_preempt) with auto_mode_2: smach.Concurrence.add("MONITOR_BTN_A",smach_ros.MonitorState("/fmHMI/joy", Joy, btn_3_pressed, max_checks=-1)) smach.Concurrence.add("AUTO_MODE",local_path) smach.Concurrence.add("SPEED_SELECTOR",speed_selector) master = smach.StateMachine(outcomes=['succeeded','aborted','preempted']) with master: smach.StateMachine.add("MANUEL_MODE", manuel_mode, transitions={'preempted':'AUTO_MODE'}) smach.StateMachine.add("AUTO_MODE", auto_mode_2, transitions={'succeeded':'MANUEL_MODE','preempted':'MANUEL_MODE'}) intro_server = smach_ros.IntrospectionServer('field_mission',master,'/FIELDMISSION') intro_server.start() smach_thread = threading.Thread(target = master.execute) smach_thread.start() rospy.spin(); master.request_preempt() intro_server.stop() print "Really ending"