def build_turn_sm(forward_vel,turn_vel,turn_left=True):
turn_sm = smach.Sequence(outcomes=["succeeded","aborted","preempted"],connector_outcome='succeeded')
with turn_sm:
smach.Sequence.add(
"DRIVE_OUT_OF_ROW",
smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
drive_forwardAction,
goal=drive_forwardGoal(amount=2.5,vel=forward_vel))
)
if turn_left:
smach.Sequence.add(
"TURN_LEFT",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=math.pi/2,vel=turn_vel))
)
else:
smach.Sequence.add(
"TURN_RIGHT",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=-math.pi/2,vel=turn_vel))
)
smach.Sequence.add(
"DRIVE_TO_NEXT_ROW",
smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
drive_forwardAction,
goal=drive_forwardGoal(amount=0.5,vel=forward_vel))
)
if turn_left:
smach.Sequence.add(
"TURN_LEFT_INTO_ROW",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=math.pi/2,vel=turn_vel))
)
else:
smach.Sequence.add(
"TURN_RIGHT_INTO_ROW",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=-math.pi/2,vel=turn_vel))
)
smach.Sequence.add(
"DRIVE_INTO_ROW",
smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
drive_forwardAction,
goal=drive_forwardGoal(amount=1.5,vel=forward_vel))
)
return turn_sm
def build_u_turn_sm(length_in, length_out, width, turn_radius , direction_l,vel_fw,vel_turn,fix_offset):
"""
"""
# vel is in m/s turn radius is in meter
if not direction_l:
turn = vel_turn/turn_radius + 0.08
else:
turn = vel_turn/turn_radius
if direction_l:
lr_amount = 1.42
else:
lr_amount = -1.42
uturn_sm = smach.StateMachine(outcomes=["succeeded","aborted","preempted"])
with uturn_sm:
smach.StateMachine.add("DRIVE_FW_IN",
smach_ros.SimpleActionState("/fmExecutors/drive_forward",
drive_forwardAction,
goal = drive_forwardGoal(amount=(length_in-turn_radius), vel=vel_fw, fix_offset=fix_offset ) ),
transitions={"succeeded":"TURN_1"},
)
smach.StateMachine.add("TURN_1",
smach_ros.SimpleActionState("/fmExecutors/make_turn",
make_turnAction,
goal= make_turnGoal(amount = lr_amount, vel=turn, forward_vel=vel_turn )),
transitions={"succeeded":"DRIVE_FW_CROSS"}
)
smach.StateMachine.add("DRIVE_FW_CROSS",
smach_ros.SimpleActionState("/fmExecutors/drive_forward",
drive_forwardAction,
goal = drive_forwardGoal(amount=(width-turn_radius*2), vel=vel_fw, fix_offset=fix_offset ) ),
transitions={"succeeded":"TURN_2"},
)
smach.StateMachine.add("TURN_2",
smach_ros.SimpleActionState("/fmExecutors/make_turn",
make_turnAction,
goal= make_turnGoal(amount = lr_amount, vel=turn, forward_vel=vel_turn)),
transitions={"succeeded":"DRIVE_FW_OUT"}
)
smach.StateMachine.add("DRIVE_FW_OUT",
smach_ros.SimpleActionState("/fmExecutors/drive_forward",
drive_forwardAction,
goal = drive_forwardGoal(amount=(length_out-turn_radius), vel=vel_fw, fix_offset=fix_offset ) ),
transitions={"succeeded":"succeeded"},
)
return uturn_sm
def build_fish_tail(turn_vel,drive_vel):
fish_tail = smach.Sequence(outcomes=['succeeded','aborted','preempted'], connector_outcome='succeeded')
with fish_tail:
smach.Sequence.add("TURN1",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=0.5025,vel=turn_vel))
)
smach.Sequence.add("DRIVE1",
smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
drive_forwardAction,
goal=drive_forwardGoal(amount=1,vel=drive_vel))
)
smach.Sequence.add("TURN2",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=1.05,vel=turn_vel))
)
smach.Sequence.add("DRIVE_REV1",
smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
drive_forwardAction,
goal=drive_forwardGoal(amount=-1,vel=drive_vel))
)
smach.Sequence.add("TURN_REV1",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=1.05,vel=turn_vel))
)
smach.Sequence.add("DRIVE_REV2",
smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
drive_forwardAction,
goal=drive_forwardGoal(amount=1,vel=drive_vel))
)
smach.Sequence.add("TURN_REV2",
smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
make_turnAction,
goal=make_turnGoal(amount=0.5025,vel=turn_vel))
)
return fish_tail
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
