def build_raise_lower_boom(): btn_monitors = smach.Concurrence(outcomes=['raise','lower','succeeded'], default_outcome='succeeded', outcome_map={ "raise":{'MOVE_UP_BTN':'invalid','MOVE_DOWN_BTN':'preempted'}, "lower":{'MOVE_UP_BTN':'preempted','MOVE_DOWN_BTN':'invalid'} }, child_termination_cb=force_preempt) with btn_monitors: smach.Concurrence.add("MOVE_UP_BTN", smach_ros.MonitorState("/fmHMI/joy", Joy, up_pressed, -1)) smach.Concurrence.add("MOVE_DOWN_BTN", smach_ros.MonitorState("/fmHMI/joy", Joy, down_pressed, -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"}) smach.StateMachine.add("MOVE_UP", smach_ros.SimpleActionState("/fmImplements/RowClean/move_tool",move_tool_simpleAction,goal=move_tool_simpleGoal(direction=1,timeout=10)), 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=10)), transitions = {"succeeded":"MONITOR_BUTTONS","aborted":"aborted","preempted":"preempted"} ) return sm
def build_sm(): """ Construct the state machine executing the selected behaviours """ row_goal = navigate_in_row_simpleGoal() row_goal.desired_offset_from_row = -0.2 row_goal.distance_scale = -0.8 row_goal.forward_velcoity = 0.5 row_goal.headland_timeout = 20 row_goal.P = 2 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" , behaviours.WaitState(2)) row_navigator = smach.StateMachine(outcomes=["succeeded","aborted","preempted"]) row_navigator.userdata.distance_driven_in_row = 0 with row_navigator: smach.StateMachine.add("FIND_ROW_WITH_TIMEOUT", find_row_timeout_sm, transitions={'succeeded':'NAVIGATE_IN_ROW','aborted':'aborted','preempted':'preempted'} ) smach.StateMachine.add("NAVIGATE_IN_ROW", smach_ros.SimpleActionState("/fmDecisionMakers/navigate_in_row_simple", navigate_in_row_simpleAction,row_goal,result_slots=['distance_traveled']), transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'}, remapping={'distance_traveled':'distance_driven_in_row'} ) #complete_row_turn = build_turn_sm(0.5,0.5) left_turn = build_turn_sm(0.5,0.5,True) right_turn = build_turn_sm(0.5,0.5,False) master = smach.StateMachine(outcomes=['succeeded','aborted','preempted']) master.userdata.last_action = "right" master.userdata.distance_driven_in_row = 0 with master: smach.StateMachine.add("LOWER_IMPLEMENT", smach_ros.SimpleActionState("/fmImplements/RowClean/move_tool",move_tool_simpleAction,goal=move_tool_simpleGoal(direction=0,timeout=7)), transitions = {"succeeded":"DRIVE_IN_ROW","aborted":"aborted","preempted":"preempted"} ) smach.StateMachine.add("DRIVE_IN_ROW", row_navigator, transitions={'succeeded':'RAISE_IMPLEMENT','aborted':'aborted','preempted':'preempted'}, remapping = {'distance_driven_in_row':'distance_driven_in_row'} ) smach.StateMachine.add("RAISE_IMPLEMENT", smach_ros.SimpleActionState("/fmImplements/RowClean/move_tool",move_tool_simpleAction,goal=move_tool_simpleGoal(direction=1,timeout=7)), transitions = {"succeeded":"LEFT_OR_RIGHT","aborted":"aborted","preempted":"preempted"} ) smach.StateMachine.add("LEFT_OR_RIGHT", smach.CBState(turn_left_or_right), transitions={"right":"TURN_OUTROW_RIGHT","left":"TURN_OUTROW_LEFT"}, remapping = {"last_action":"last_action","new_action":"last_action"}) smach.StateMachine.add("TURN_OUTROW_LEFT", left_turn, transitions = {"succeeded":"LOWER_IMPLEMENT","aborted":"aborted","preempted":"preempted"}) smach.StateMachine.add("TURN_OUTROW_RIGHT", right_turn, transitions = {"succeeded":"LOWER_IMPLEMENT","aborted":"aborted","preempted":"preempted"}) m2 = behaviours.wii_states.wii_auto_manuel.create(master, "/fmHMI/joy", 2) m3 = smach.Concurrence(outcomes=['succeeded','aborted','preempted'], default_outcome='aborted', outcome_map={}, child_termination_cb=force_preempt) with m3: smach.Concurrence.add("MASTER",m2) smach.Concurrence.add("MOVE_IMPLEMENT_MANUAL",build_raise_lower_boom()) return m3