def NavigationSpeed(task, speed):
speed_var = shm.navigation_settings.max_speed
init_speed = speed_var.get()
return Defer(
Sequential(
FunctionTask(lambda: speed_var.set(speed)),
task,
),
FunctionTask(lambda: speed_var.set(init_speed)),
)
def on_first_run(self, vision, tube, amlan, *args, **kwargs):
def apply_grab():
# Add tube to Amlan in shm
amlan.shm_tube.set(tube)
# Remove tube from tower in shm
tower_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube')
tower_tubes[tube] = False
set_shm_array(shm.recovery_world_tower, 'has_tube', tower_tubes)
# Remove tube from vision pattern
vision.remove_tube(tube)
north = shm.kalman.north.get()
east = shm.kalman.east.get()
depth = shm.desires.depth.get()
initial_tubes = sum(t.obs is not None for t in vision.tubes)
self.use_task(
Conditional(
Sequential(
amlan.FastRetract(),
Defer(
Conditional(
GrabTube(vision, tube, amlan),
on_fail=Fail(
Sequential(
Log('Failed to grab, prematurely retracting grabber'
),
amlan.FastRetract(),
)),
),
Sequential(
Zero(),
Log('Restoring position from before grab'),
FastDepth(depth),
GoToPositionRough(north, east, optimize=False),
),
),
# TODO continue always assuming good grab?
# VerifyGrab(vision, initial_tubes),
FunctionTask(apply_grab),
),
Log('Grabbed {} tube with {} Amlan!'.format(
constants.colors[tube].name,
amlan.name,
)),
Fail(amlan.FastRetract()),
))
def on_first_run(self, vision, reset_state=True, *args, **kwargs):
# Limit navigation speed to prevent merge cutting out computer
nav_speed = shm.navigation_settings.max_speed
orig_speed = nav_speed.get()
nav_speed.set(0.5)
if reset_state:
for loc in ['table', 'tower']:
g_name = getattr(shm, 'recovery_world_{}'.format(loc))
g = g_name.get()
g.know_pos = False
for i in range(self.MAX_COLORS):
setattr(g, 'has_tube{}'.format(i), loc == 'tower'
and i < len(constants.colors))
g_name.set(g)
for a in AMLANS:
a.shm_tube.set(-1)
self.use_task(
Conditional(
Defer(
Sequential(
Log('Starting recovery!'),
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
TryRecovery(vision),
),
Sequential(
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
Log('Returning to pre-recovery depth'),
FastDepth(shm.kalman.depth.get()),
Log('Resetting navigation speed'),
FunctionTask(lambda: nav_speed.set(orig_speed)),
Log('Moving away from table'),
Heading(WALL_TOWER_HEADING),
MoveX(5, deadband=0.08),
),
),
Log('Recovery success!'),
Log('Recovery failure!'),
))
def on_first_run(self, task, enable=True, optimize=False):
enable_var = shm.navigation_settings.position_controls
optimize_var = shm.navigation_settings.optimize
init_enable, init_optimize = enable_var.get(), optimize_var.get()
def set_shm(enable, optimize):
enable_var.set(enable)
optimize_var.set(optimize)
self.use_task(
Defer(
Sequential(
FunctionTask(lambda: set_shm(enable, optimize)),
task,
),
FunctionTask(lambda: set_shm(init_enable, init_optimize)),
))
def place_tube(amlan):
return Defer(
Conditional(
Retry(
lambda: Sequential(
Log('Moving above table'),
MoveAboveTable(vision),
Log('Placing {} tube'.format(constants.colors[
amlan.shm_tube.get()].name)),
DropTube(vision, amlan),
), 3),
on_fail=Succeed(
Sequential(
Log('Failed to drop tube normally, performing blind drop'
),
DropTube(vision, amlan, blind=True),
)),
),
amlan.FastRetract(),
)
def Zeroed(task):
""" Guarantee that a task is zeroed on completion """
return Defer(task, Zero())
invert=False),
),
),
)
BackUpUntilVisible = lambda num, speed, timeout: Conditional(
Defer(
main_task=Either(
Sequential(
# Get at least a little bit away first
FakeMoveX(dist=-0.3, speed=0.2),
MasterConcurrent(
Consistent(lambda: shm_vars[num].visible.get(),
count=1,
total=1.5,
result=True,
invert=False),
VelocityX(-speed),
),
),
Fail(
Timer(timeout), # don't back up too far
),
),
deferred=Sequential(VelocityX(0), ),
),
on_success=Succeed(Sequential(NoOp(), ), ),
on_fail=Conditional(
FunctionTask(lambda: num == 0),
on_success=Sequential(
Log('Timed out, searching for buoy again'),
SearchBuoy(num=num, count=4, total=5),