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"
