def build_main_sm():
"""
Construct the state machine executing the selected behaviours and actions
"""
#
# Create the inrow behaviour
#
row_goal = navigate_in_row_simpleGoal()
row_goal.desired_offset_from_row = 0
row_goal.distance_scale = -0.35
row_goal.forward_velcoity = 0.8
row_goal.headland_timeout = 5
row_goal.P = 0.3
row_nav = build_row_nav_sm(row_goal, 2)
# length_in, length_out, width, turn_radius , direction_l,vel_fw,vel_turn, fix_offset):
uturn_right = build_u_turn_sm(7, 3, 9.5, 3, False, 0.4, 0.4, -0.035)
uturn_left = build_u_turn_sm(7, 3, 8.1, 3, True, 0.4, 0.4, -0.035)
main_sm = smach.StateMachine(["succeeded", "aborted", "preempted"])
main_sm.userdata.next_turn = "left"
with main_sm:
smach.StateMachine.add("NAVIGATE_IN_ROW", row_nav, transitions={"succeeded": "TURN_SELECTOR"})
smach.StateMachine.add(
"TURN_SELECTOR",
smach.CBState(
on_turn_selection,
outcomes=["left", "right"],
input_keys=["next_turn_in"],
output_keys=["next_turn_out"],
),
transitions={"left": "MAKE_TURN_LEFT", "right": "MAKE_TURN_RIGHT"},
remapping={"next_turn_in": "next_turn", "next_turn_out": "next_turn"},
)
smach.StateMachine.add("MAKE_TURN_RIGHT", uturn_right, transitions={"succeeded": "NAVIGATE_IN_ROW"})
smach.StateMachine.add("MAKE_TURN_LEFT", uturn_left, transitions={"succeeded": "NAVIGATE_IN_ROW"})
return main_sm
def build_sm():
"""
Construct the state machine executing the selected behaviours
"""
row_navigator = smach.StateMachine(outcomes=["succeeded","aborted","preempted"])
row_goal = navigate_in_row_simpleGoal()
row_goal.desired_offset_from_row = 1.1
row_goal.distance_scale = -0.3
row_goal.forward_velcoity = 0.8
row_goal.headland_timeout = 0
row_goal.P = 1.2
turn_goal = make_turnGoal()
turn_goal.amount = math.pi
turn_goal.vel = 0.6
wiggle_goal = make_turnGoal()
wiggle_goal.amount = 0.1
wiggle_goal.vel = 0.5
fw_goal = drive_forwardGoal()
fw_goal.amount = 2
fw_goal.vel = 0.6
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))
with row_navigator:
smach.StateMachine.add("FIND_ROW_WITH_TIMEOUT",
find_row_timeout_sm,
transitions={'succeeded':'NAVIGATE_IN_ROW','aborted':'NAVIGATE_IN_ROW'}
)
smach.StateMachine.add("NAVIGATE_IN_ROW",
smach_ros.SimpleActionState("/navigate_in_row_simple", navigate_in_row_simpleAction,row_goal),
transitions={'succeeded':'aborted'}
)
laser_state = smach.Concurrence(outcomes=['yellow_active','red_active'],
outcome_map={'yellow_active':{'WAIT_FOR_YELLOW':'invalid','WAIT_FOR_RED':'preempted'},
'red_active' :{'WAIT_FOR_RED':'invalid'}}
,default_outcome='red_active',child_termination_cb=force_preempt)
red_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_red, max_checks=-1)
red_not_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_red_neg, max_checks=-1)
yellow_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_yellow, max_checks=-1)
yellow_not_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_yellow_neg, max_checks=-1)
with laser_state:
smach.Concurrence.add("WAIT_FOR_YELLOW",
yellow_active
)
smach.Concurrence.add("WAIT_FOR_RED",
red_active
)
control_state = smach.Concurrence(outcomes=['stop','retract','aborted'],
outcome_map={'stop':{'MONITOR_LASER':'yellow_active'},
'retract':{'MONITOR_LASER':'red_active'},
'aborted':{'NAVIGATE_IN_ROW':'aborted'}},
default_outcome='stop',
child_termination_cb=force_preempt)
with control_state:
smach.Concurrence.add("MONITOR_LASER", laser_state)
smach.Concurrence.add("NAVIGATE_IN_ROW", row_navigator)
stop_state = smach.Concurrence(outcomes=['continue','retreat'],
outcome_map={'continue':{'MONITOR_YELLOW':'invalid','MONITOR_RED':'preempted'},
'retreat':{'MONITOR_RED':'invalid'}
},
default_outcome='continue',
child_termination_cb=force_preempt)
with stop_state:
smach.Concurrence.add("MONITOR_RED", red_active)
smach.Concurrence.add("MONITOR_YELLOW",yellow_not_active)
retreat_state = smach.StateMachine(outcomes=['clear','aborted','preempted'])
with retreat_state:
smach.StateMachine.add("BACK_UP",
smach_ros.SimpleActionState("/drive_forward",drive_forwardAction,drive_forwardGoal(amount=-0.3,vel=0.3)),
transitions ={'succeeded':'MONITOR_RED','aborted':'aborted','preempted':'preempted'})
smach.StateMachine.add("MONITOR_RED",
red_not_active,
transitions={'valid':'BACK_UP','invalid':'clear'})
master = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
with master:
smach.StateMachine.add("DRIVE_IN_ROW",
control_state,
transitions={'stop':'STOP_STATE','retract':'RETREAT_STATE','aborted':'TURN_180'})
smach.StateMachine.add("STOP_STATE",
stop_state,
transitions={'continue':'DRIVE_IN_ROW','retreat':'RETREAT_STATE'})
smach.StateMachine.add("RETREAT_STATE",
retreat_state,
transitions={'clear':'TURN_180','aborted':'aborted'})
smach.StateMachine.add("TURN_180",
smach_ros.SimpleActionState("/make_turn", make_turnAction, goal=make_turnGoal(amount=math.pi-0.01,vel=0.6)),
transitions={'succeeded':'DRIVE_IN_ROW'})
return master
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
