def on_first_run(self, *args, **kwargs):
self.use_task(
NavigationSpeed(
Sequential(
Depth(recovery_constants.tower_depth, error=0.08),
ConsistentTask(Heading(60 * TOWARDS_BOWL_SIGN)),
MoveX(30, deadband=0.08),
), 0.4))
def on_first_run(self, vision, tower=True, *args, **kwargs):
obj_func = (lambda: vision.tubes) if tower else (
lambda: vision.ellipses)
self.use_task(
ConsistentTask(
IdentifyObjects(obj_func, 3),
success=20,
total=30,
))
def on_first_run(self, vision, bin_func, *args, **kwargs):
self.use_task(ConsistentTask(PIDLoop(
input_value=lambda: bin_func().obs.length,
output_function=RelativeToCurrentDepth(),
target=1.2,
deadband=0.02,
p=0.7,
d=0.3,
)))
def AvoidWall():
if not shm.recovery_world_table.know_pos.get():
return Sequential(
Log('Moving away from wall'),
ConsistentTask(Heading(WALL_TOWER_HEADING)),
MoveXRough(2),
)
else:
return NoOp()
def on_first_run(self, vision, *args, **kwargs):
def AvoidWall():
if not shm.recovery_world_table.know_pos.get():
return Sequential(
Log('Moving away from wall'),
ConsistentTask(Heading(WALL_TOWER_HEADING)),
MoveXRough(2),
)
else:
return NoOp()
def Iteration(first_run=False):
return Sequential(
# Sequential(
# Log('Classifying tubes'),
# Retry(lambda: MoveAboveTower(vision), 3),
# FunctionTask(vision.classify_tubes),
# ) if first_run else NoOp(),
GetTube(vision),
Succeed(GetTube(
vision)), # It's OK if we don't grab the second tube
Surface(),
AvoidWall(),
# Initially classify ellipses as their size / color may change
# as tubes are added
Sequential(
Log('Classifying ellipses'),
Retry(lambda: MoveAboveTable(vision), 3),
FunctionTask(vision.classify_ellipses),
) if first_run else NoOp(),
PlaceTubes(vision),
)
self.use_task(
Except(
Sequential(
Retry(lambda: MoveAboveTable(vision), 1000),
Log('Moving towards wall'),
ConsistentTask(Heading((WALL_TOWER_HEADING + 180) % 360)),
MoveXRough(2),
Iteration(first_run=True),
While(
Iteration,
lambda: count_tubes(shm.recovery_world_tower) > 0,
),
),
Fail(Log('Global timeout, done trying recovery T_T')),
GlobalTimeoutError,
))
def cons(task, total=1 * 60, success=1 * 60 * 0.85, debug=False):
return ConsistentTask(task, total=total, success=success, debug=debug)