from auvsi_robosub import subjugator_states
from uf_smach import common_states, legacy_vision_states, missions
import numpy
import smach
def make_surface(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add(
'SURFACE',
common_states.WaypointState(shared, lambda cur: cur.depth(-.1)))
smach.Sequence.add('SLEEP', common_states.SleepState(5))
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.depth(.3)))
return sm
missions.register_factory('surface', make_surface)
smach.Sequence.add('SLEEP2', common_states.SleepState(3))
sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm_retreat:
smach.Sequence.add('RETREAT',
common_states.VelocityState(shared, numpy.array([-.2, 0, 0])))
smach.Sequence.add('WAIT',
common_states.SleepState(4))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH_SHOOT_1', sm_approach,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_1', sm_retreat,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('SHOOT_1', sm_shoots[0],
transitions={'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_2', sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2', sm_shoots[1],
transitions={'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(shared,
lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
missions.register_factory('shooter', make_shooter)
)
smach.Sequence.add("CENTER_ALL", legacy_vision_states.CenterObjectState(shared, "find2_down_camera"))
smach.Sequence.add(
"ALIGN_ALL", legacy_vision_states.AlignObjectState(shared, "find2_down_camera", body_vec_align=[0, 1, 0])
)
smach.Sequence.add("CENTER_ALL_2", legacy_vision_states.CenterObjectState(shared, "find2_down_camera"))
smach.Sequence.add(
"TURN_DEPTH",
common_states.WaypointState(shared, lambda cur: cur.depth(constants.PIPE_DEPTH).turn_left_deg(90)),
)
smach.Sequence.add("APPROACH", common_states.VelocityState(shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add(
"WAIT_PIPE",
legacy_vision_states.WaitForObjectsState(shared, "find2_down_camera", "pipe"),
transitions={"timeout": "failed"},
)
selector = legacy_vision_states.select_by_body_direction([0, 1 if constants.MODE == "competition" else -1, 0])
smach.Sequence.add("CENTER_PIPE", legacy_vision_states.CenterObjectState(shared, "find2_down_camera", selector))
smach.Sequence.add(
"TURN_HELP_PIPE",
common_states.WaypointState(
shared, lambda cur: cur.turn_left_deg(45) if constants.MODE == "competition" else cur.turn_right_deg(45)
),
)
return sm
missions.register_factory("bins", make_bins)
missions.register_factory("bins_return", make_bins_return)
with sm:
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add('DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add('TRAVEL', sm_travel)
return sm
def make_hydrophones_close(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(1.5)))
smach.Sequence.add('HYDROPHONES_TRAVEL',
hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE),
transitions={'failed': 'HYDROPHONES_TRAVEL'})
smach.Sequence.add('HYDROPHONES_APPROACH',
hydrophone_states.HydrophoneApproachState(shared, FREQ, FREQ_RANGE),
transitions={'failed': 'succeeded'}) # Ensure we drop
smach.Sequence.add('BACKUP',
common_states.WaypointState(shared,
lambda cur: cur.backward(.3)))
return sm
missions.register_factory('hydrophone_grab', make_hydrophones)
missions.register_factory('hydrophone_drop', make_hydrophones_close)
lambda cur: cur.forward(BOARD_DIST-WHEEL_DIST).left(.5)))
smach.Sequence.add('WAIT_WHEEL',
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [wheel], .99))
smach.Sequence.add('WAIT_WHEEL_MORE',
common_states.SleepState(5))
smach.Sequence.add('APPROACH_WHEEL',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera', WHEEL_DIST))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\
.relative([0, .06, .06])))
smach.Sequence.add('TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.down(.2),
lambda cur: cur.right(.2),
lambda cur: cur.up(.2),
lambda cur: cur.left(.2),
lambda cur: cur.down(.2),
lambda cur: cur.right(.2)]))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add('BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
missions.register_factory('manipulation', make_manipulation)
['succeeded', 'failed', 'preempted'],
default_outcome='failed',
outcome_map={'succeeded': {
'WAIT': 'succeeded'
}},
child_termination_cb=lambda so: True)
with search_sm:
smach.Concurrence.add(
'PATTERN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.left(2), lambda cur: cur.forward(2),
lambda cur: cur.right(4), lambda cur: cur.backward(2)
],
speed=.3))
smach.Concurrence.add(
'WAIT',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera',
'pipe'))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('RETURN_WAYPOINT',
common_states.ReturnToWaypointState(
shared, 'last_path'),
transitions={'succeeded': 'SEARCH'})
smach.StateMachine.add('SEARCH', search_sm)
return sm
missions.register_factory('return_path', make_return_path)
from auvsi_robosub import subjugator_states
from uf_smach import common_states, legacy_vision_states, missions
import numpy
import smach
def make_surface(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('SURFACE',
common_states.WaypointState(shared, lambda cur: cur.depth(-.1)))
smach.Sequence.add('SLEEP',
common_states.SleepState(5))
smach.Sequence.add('DOWN',
common_states.WaypointState(shared, lambda cur: cur.depth(.3)))
return sm
missions.register_factory('surface', make_surface)
with sm:
smach.StateMachine.add("APPROACH", sm_approach, transitions={"succeeded": "DESCEND_GRAB"})
smach.StateMachine.add(
"DESCEND_GRAB",
sm_descend_grab,
transitions={"succeeded": "UP", "failed": "APPROACH", "empty": "EXTRA_GRAB"},
)
smach.StateMachine.add(
"EXTRA_GRAB", sm_extra_grab, transitions={"succeeded": "UP", "empty": "RETRY_GRAB_COUNTER"}
)
smach.StateMachine.add("UP", common_states.WaypointState(shared, lambda cur: cur.depth(0.3)))
smach.StateMachine.add(
"RETRY_GRAB_COUNTER",
common_states.CounterState(1),
transitions={"succeeded": "DESCEND_GRAB", "exceeded": "APPROACH"},
)
return sm
def make_grabber_drop(shared):
sm = smach.Sequence(["succeeded", "failed", "preempted"], "succeeded")
with sm:
smach.Sequence.add("DEPTH", common_states.WaypointState(shared, lambda cur: cur.depth(2)))
smach.Sequence.add("OPEN_GRABBER", subjugator_states.OpenGrabberState())
smach.Sequence.add("UP_DEPTH", common_states.WaypointState(shared, lambda cur: cur.depth(0.5)))
return sm
missions.register_factory("grabber", make_grabber)
missions.register_factory("grabber_drop", make_grabber_drop)
speed=constants.PIPE_SPEED),
transitions={'succeeded': 'SEARCH'})
smach.StateMachine.add('SEARCH',
common_states.WaypointSeriesState(shared,
[lambda cur: cur.left(SEARCH_WIDTH),
lambda cur: cur.forward(SEARCH_ADVANCE),
lambda cur: cur.right(SEARCH_WIDTH),
lambda cur: cur.forward(SEARCH_ADVANCE)],
speed=constants.PIPE_SPEED),
transitions={'succeeded': 'SEARCH'})
search_sm = smach.Concurrence(['succeeded', 'failed', 'preempted'],
default_outcome='failed',
outcome_map={'succeeded': {'WAIT': 'succeeded'}},
child_termination_cb=lambda so: True)
with search_sm:
smach.Concurrence.add('PATTERN', search_pattern_sm)
smach.Concurrence.add('WAIT',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera', 'pipe'))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('BUOY', buoy_sm,
transitions={'succeeded': 'SEARCH',
'failed': 'SEARCH'})
smach.StateMachine.add('SEARCH', search_sm)
return sm
missions.register_factory('buoy', make_buoy)
with sm_center:
smach.Sequence.add(
"CENTER_APPROACH_HEDGE",
legacy_vision_states.CenterApproachObjectState(shared, "find2_forward_camera", desired_scale=180),
)
smach.Sequence.add("LEFT", common_states.WaypointState(shared, lambda cur: cur.turn_left_deg(90)))
smach.Sequence.add("GO", common_states.WaypointState(shared, lambda cur: cur.right(4)))
smach.Sequence.add("RIGHT", common_states.WaypointState(shared, lambda cur: cur.turn_right_deg(90)))
sm_pipe = smach.Sequence(["succeeded", "failed", "preempted"], "succeeded")
with sm_pipe:
smach.Sequence.add("DEPTH", common_states.WaypointState(shared, lambda cur: cur.depth(constants.PIPE_DEPTH)))
smach.Sequence.add("APPROACH", common_states.VelocityState(shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add(
"WAIT_PIPE",
legacy_vision_states.WaitForObjectsState(shared, "find2_down_camera", "pipe"),
transitions={"timeout": "failed"},
)
smach.Sequence.add("STOP", common_states.WaypointState(shared, lambda cur: cur))
sm = smach.StateMachine(["succeeded", "failed", "preempted"])
with sm:
smach.StateMachine.add("APPROACH", sm_approach, transitions={"succeeded": "CENTER", "failed": "PIPE"})
smach.StateMachine.add("CENTER", sm_center, transitions={"succeeded": "PIPE", "failed": "PIPE"})
smach.StateMachine.add("PIPE", sm_pipe, transitions={"succeeded": "succeeded"})
return sm
missions.register_factory("hedge", make_hedge)
'empty': 'RETRY_GRAB_COUNTER'
})
smach.StateMachine.add(
'UP', common_states.WaypointState(shared,
lambda cur: cur.depth(.3)))
smach.StateMachine.add('RETRY_GRAB_COUNTER',
common_states.CounterState(1),
transitions={
'succeeded': 'DESCEND_GRAB',
'exceeded': 'APPROACH'
})
return sm
def make_grabber_drop(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(2)))
smach.Sequence.add('OPEN_GRABBER',
subjugator_states.OpenGrabberState())
smach.Sequence.add(
'UP_DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(.5)))
return sm
missions.register_factory('grabber', make_grabber)
missions.register_factory('grabber_drop', make_grabber_drop)
common_states.WaypointState(
shared, lambda cur: cur.heading(math.radians(-179 + (180 if cur.forward_vector[0] > 0 else 0)))
),
)
smach.Sequence.add(
"APPROACH_MANIPULATION_POST_YAW",
legacy_vision_states.CenterApproachObjectState(
shared, "find2_forward_camera", desired_scale=130e3 / 3 ** 2
),
)
smach.Sequence.add("WHEEL", sm_wheel, transitions=dict(failed="BACKUP"))
smach.Sequence.add("BACKUP", common_states.WaypointState(shared, lambda cur: cur.backward(3)))
smach.Sequence.add("REFIND", common_states.VelocityState(shared, numpy.array([0.1, 0, 0])))
smach.Sequence.add(
"WAIT_MANIPULATION2",
legacy_vision_states.WaitForObjectsState(shared, "find2_forward_camera", "grapes/board", timeout=15),
)
smach.Sequence.add(
"APPROACH_MANIPULATION2",
legacy_vision_states.CenterApproachObjectState(
shared, "find2_forward_camera", desired_scale=130e3 / 3 ** 2
),
)
smach.Sequence.add("LEVER", sm_lever)
smach.Sequence.add("BACKUP2", common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
missions.register_factory("manipulation_thresh", make_manipulation)
from auvsi_robosub import subjugator_states, constants
from uf_smach import common_states, legacy_vision_states, missions
import numpy
import smach
def make_starting_gate(shared):
sm = smach.Sequence(["succeeded", "failed", "preempted"], "succeeded")
with sm:
smach.Sequence.add(
"GO",
common_states.WaypointSeriesState(
shared, [lambda cur: cur.depth(constants.PIPE_DEPTH), lambda cur: cur.forward(10)]
),
)
smach.Sequence.add("APPROACH", common_states.VelocityState(shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add(
"WAIT_PIPE",
legacy_vision_states.WaitForObjectsState(shared, "find2_down_camera", "pipe"),
transitions={"timeout": "failed"},
)
return sm
missions.register_factory("starting_gate", make_starting_gate)
transitions={'succeeded': 'SEARCH'})
search_sm = smach.Concurrence(
['succeeded', 'failed', 'preempted'],
default_outcome='failed',
outcome_map={'succeeded': {
'WAIT': 'succeeded'
}},
child_termination_cb=lambda so: True)
with search_sm:
smach.Concurrence.add('PATTERN', search_pattern_sm)
smach.Concurrence.add(
'WAIT',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera',
'pipe'))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('BUOY',
buoy_sm,
transitions={
'succeeded': 'SEARCH',
'failed': 'SEARCH'
})
smach.StateMachine.add('SEARCH', search_sm)
return sm
missions.register_factory('buoy', make_buoy)
'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'
})
smach.StateMachine.add('RETREAT_SHOOT_2',
sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2',
sm_shoots[1],
transitions={
'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'
})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(
shared, lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
def make_shooter_approach(shared):
sm = common_states.WaypointSeriesState(shared, [
lambda cur: cur.turn_left_deg(65)
if constants.MODE == 'competition' else cur.turn_left_deg(30),
lambda cur: cur.right(.5)
])
return sm
missions.register_factory('shooter', make_shooter)
missions.register_factory('shooter_approach', make_shooter_approach)
'find2_down_camera'))
smach.Sequence.add('TURN_DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(constants.PIPE_DEPTH)\
.turn_left_deg(90)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(
shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add('WAIT_PIPE',
legacy_vision_states.WaitForObjectsState(
shared, 'find2_down_camera', 'pipe'),
transitions={'timeout': 'failed'})
selector = legacy_vision_states.select_by_body_direction(
[0, 1 if constants.MODE == 'competition' else -1, 0])
smach.Sequence.add(
'CENTER_PIPE',
legacy_vision_states.CenterObjectState(shared, 'find2_down_camera',
selector))
smach.Sequence.add(
'TURN_HELP_PIPE',
common_states.WaypointState(
shared, lambda cur: cur.turn_left_deg(45) if constants.MODE ==
'competition' else cur.turn_right_deg(45)))
return sm
missions.register_factory('bins', make_bins)
missions.register_factory('bins_return', make_bins_return)
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [wheel], .99))
smach.Sequence.add('WAIT_WHEEL_MORE', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_WHEEL',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
WHEEL_DIST))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\
.relative([0, .06, .06])))
smach.Sequence.add(
'TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.down(.2), lambda cur: cur.right(.2),
lambda cur: cur.up(.2), lambda cur: cur.left(.2),
lambda cur: cur.down(.2), lambda cur: cur.right(.2)
]))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add(
'BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
missions.register_factory('manipulation', make_manipulation)
common_states.SleepState(4))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH_SHOOT_1', sm_approach,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_1', sm_retreat,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('SHOOT_1', sm_shoots[0],
transitions={'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_2', sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2', sm_shoots[1],
transitions={'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(shared,
lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
def make_shooter_approach(shared):
sm = common_states.WaypointSeriesState(shared,
[lambda cur: cur.turn_left_deg(65) if constants.MODE == 'competition' else cur.turn_left_deg(30),
lambda cur: cur.right(.5)])
return sm
missions.register_factory('shooter', make_shooter)
missions.register_factory('shooter_approach', make_shooter_approach)
with sm:
smach.StateMachine.add('APPROACH_SHOOT_1',
sm_approach,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_1',
sm_retreat,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('SHOOT_1',
sm_shoots[0],
transitions={
'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'
})
smach.StateMachine.add('RETREAT_SHOOT_2',
sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2',
sm_shoots[1],
transitions={
'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'
})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(
shared, lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
missions.register_factory('shooter_pf', make_shooter)
from auvsi_robosub import subjugator_states, constants
from uf_smach import common_states, legacy_vision_states, missions
import numpy
import smach
def make_starting_gate(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('GO',
common_states.WaypointSeriesState(shared, [lambda cur: cur.depth(constants.PIPE_DEPTH),
lambda cur: cur.forward(10)]))
smach.Sequence.add('APPROACH',
common_states.VelocityState(shared,
numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add('WAIT_PIPE',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera', 'pipe'),
transitions={'timeout': 'failed'})
return sm
missions.register_factory('starting_gate', make_starting_gate)
common_states.VelocityState(
shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add('WAIT_PIPE',
legacy_vision_states.WaitForObjectsState(
shared, 'find2_down_camera', 'pipe'),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'STOP', common_states.WaypointState(shared, lambda cur: cur))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={
'succeeded': 'CENTER',
'failed': 'PIPE'
})
smach.StateMachine.add('CENTER',
sm_center,
transitions={
'succeeded': 'PIPE',
'failed': 'PIPE'
})
smach.StateMachine.add('PIPE',
sm_pipe,
transitions={'succeeded': 'succeeded'})
return sm
missions.register_factory('hedge', make_hedge)
outcome_map={"succeeded": {"WAIT": "succeeded"}},
child_termination_cb=lambda so: True,
)
with search_sm:
smach.Concurrence.add(
"PATTERN",
common_states.WaypointSeriesState(
shared,
[
lambda cur: cur.left(2),
lambda cur: cur.forward(2),
lambda cur: cur.right(4),
lambda cur: cur.backward(2),
],
speed=0.3,
),
)
smach.Concurrence.add("WAIT", legacy_vision_states.WaitForObjectsState(shared, "find2_down_camera", "pipe"))
sm = smach.StateMachine(["succeeded", "failed", "preempted"])
with sm:
smach.StateMachine.add(
"RETURN_WAYPOINT",
common_states.ReturnToWaypointState(shared, "last_path"),
transitions={"succeeded": "SEARCH"},
)
smach.StateMachine.add("SEARCH", search_sm)
return sm
missions.register_factory("return_path", make_return_path)