def on_first_run(self, vision, amlan, blind=False):
initial_tube = amlan.shm_tube.get()
def apply_drop():
# Add tube to table
table_tubes = get_shm_array(shm.recovery_world_table, 'has_tube')
table_tubes[initial_tube] = True
set_shm_array(shm.recovery_world_table, 'has_tube', table_tubes)
self.use_task(
Sequential(
AlignAmlan(
vision,
lambda: vision.obj_with_id(vision.ellipses,
amlan.shm_tube.get()),
amlan,
FastDepth(constants.ellipse_depth),
blind=blind,
align_during_depth=True,
),
Sequential(
Log('Moving to tube drop depth'),
GradualDepth(constants.drop_depth),
) if not blind else NoOp(),
Timer(1),
amlan.Retract(),
FunctionTask(apply_drop),
Sequential(
Log('Moving up away from table slowly'),
GradualDepth(constants.drop_depth + self.AFTER_DROP_DELTA),
) if not blind else NoOp(),
))
def on_first_run(self, vision, *args, **kwargs):
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(),
)
self.use_task(
Sequential(
place_tube(AMLANS[0])
if AMLANS[0].shm_tube.get() != -1 else NoOp(),
place_tube(AMLANS[1])
if AMLANS[1].shm_tube.get() != -1 else NoOp(),
))
def on_first_run(self,
main_task,
on_success=None,
on_fail=None,
*args,
**kwargs):
self.main_task = main_task
self.on_success = on_success if on_success is not None else NoOp()
self.on_fail = on_fail if on_fail is not None else NoOp()
def on_first_run(self,
vision,
obj_func,
amlan,
align_depth_task,
blind=False,
align_during_depth=False,
*args,
**kwargs):
def tube_angle_heading():
return shm.kalman.heading.get() + (obj_func().obs.angle % 180 - 90)
self.must_see_obj = not blind
def unsee():
self.must_see_obj = False
self.task = Sequential(
Sequential(
Log('Centering object'),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
Zero(),
Sequential(
Log('Aligning to object'),
Concurrent(
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
GradualHeading(tube_angle_heading),
finite=False,
),
) if not align_during_depth else NoOp(),
Log('Going down and precisely aligning to object'),
ConsistentTask(
Concurrent(
GradualHeading(tube_angle_heading),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=2),
align_depth_task,
finite=False,
)),
FunctionTask(unsee),
) if not blind else NoOp(),
PositionalControl(),
Zero(),
Log('Applying Amlan offset'),
amlan.CenterFromDownCam(),
)
def on_first_run(self, vision, *args, **kwargs):
table = shm.recovery_world_table.get()
def record_position():
table.know_pos = True
table.north = shm.kalman.north.get()
table.east = shm.kalman.east.get()
shm.recovery_world_table.set(table)
self.use_task(
Sequential(
# Move to the table depth first so we don't blow things off
Log('Moving to table depth'),
FastDepth(constants.table_depth),
Sequential(
Log('Returning to table position ({}, {})'.format(
table.north, table.east)),
GoToPositionRough(table.north, table.east, optimize=False),
) if table.know_pos else NoOp(),
Log('Searching for table'),
MasterConcurrent(
IdentifyObjects(lambda: vision.ellipses, 2),
SearchWithGlobalTimeout(timeout=self.TIMEOUT)),
Log('Centering table'),
CenterCentroid(lambda: vision.coords(vision.ellipses)),
Zero(),
Log('Recording table position'),
FunctionTask(record_position),
))
def __init__(self,
target_validator,
collision_validator,
locator_task,
concurrent_task=NoOp(),
ram_speed=None,
*args,
**kwargs):
"""
target_validator - a function that returns True when a target is
visible and False otherwise.
collision_validator - a function that returns True when a collision is
made and False otherwise.
concurrent_task - an optional argument for a task to run while moving
forward to ram the target. It may be used to continually align with
the target while ramming it.
ram_speed - a function that returns a speed at which to ram the target
"""
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.target_validator = target_validator
self.collision_validator = collision_validator
self.ram_speed = ram_speed
self.ram_task = VelocityX()
self.locator_task = locator_task
self.concurrent_task = concurrent_task
self.commit_task = Sequential(VelocityX(1), Timer(1), VelocityX(0))
self.ram_commit_phase = False
self.TIMEOUT = 25
def approach_right_passageway_task():
align_task = align_right_width()
return FinishIf(
condition=lambda: gate_elems() == 0 or
(align_task.finished and align_task.success),
task=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 1),
on_success=Concurrent(
focus_elem(lambda: shm.gate.leftmost_x, offset=right_offset),
VelocityX(0.2)),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() >= 2),
on_success=Concurrent(
# Align distance in between the right and middle poles
align_task,
# Align to middle of right and middle poles
PIDLoop(input_value=lambda: (shm.gate.rightmost_x.get() +
shm.gate.middle_x.get()) / 2
if shm.gate.rightmost_visible.get() else
(shm.gate.leftmost_x.get() + shm.gate.middle_x.get(
)) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.3,
deadband=lambda: shm.gate.img_width.get() * 0.05,
output_function=RelativeToCurrentHeading(),
negate=True),
finite=False,
),
on_fail=NoOp(),
finite=False),
finite=False))
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):
tower_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube')
search_tubes = 2 if sum(tower_tubes) == len(constants.colors) else 1
tower = shm.recovery_world_tower.get()
go_to_tower = NoOp()
if tower.know_pos:
go_to_tower = Sequential(
Log('Returning to tower position ({}, {})'.format(
tower.north, tower.east)),
GoToPositionRough(tower.north, tower.east, optimize=False),
)
if sum(tower_tubes) > 0:
search_tower = Sequential(
Log('Searching for tower'),
FastDepth(constants.tower_depth),
MasterConcurrent(
IdentifyObjects(
lambda: vision.tubes,
search_tubes,
),
SearchWithGlobalTimeout(),
),
)
else:
search_tower = Log('Not searching for tower, no tubes on tower')
if sum(tower_tubes) > 0:
center_tower = Sequential(
Log('Centering tower'),
CenterCentroid(lambda: vision.coords(vision.tubes)),
Zero(),
)
else:
center_tower = Log('No tubes on tower, not centering')
self.use_task(
Sequential(
# Go to depth early so we don't hit the tower
Log('Going to default tower depth of {}'.format(
constants.tower_depth)),
FastDepth(constants.tower_depth),
go_to_tower,
search_tower,
center_tower,
))
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),
)
def on_first_run(self, vision, angle, double_align=False, *args, **kwargs):
def CenterTargetBin(precision):
return CenterBins(vision, lambda bin: bin.id == vision.TARGET_BIN, precision=precision)
def AlignTargetBin():
return AlignHeadingToAngle(lambda: vision.target_bin().obs.angle, angle, mod=180)
def DepthAlign(depth):
return Concurrent(
AlignTargetBin(),
CenterTargetBin(1),
Depth(depth),
finite=False,
)
self.task = Zeroed(Timeout(Sequential(
Log('Centering over target bin'),
CenterTargetBin(0),
Log('Aligning target bin'),
Concurrent(
AlignTargetBin(),
CenterTargetBin(0),
finite=False,
),
Log('Going half down to precisely align with target bin'),
DepthAlign((constants.see_both_depth + constants.above_bin_depth) / 2),
Sequential(
Log('Going down to fully align to bin'),
DepthAlign(constants.above_bin_depth),
) if double_align else NoOp(),
Zero(),
PositionalControl(),
), self.TIMEOUT))
def __init__(self,
location_validator,
target_coordinates,
vision_group,
collision_validator,
ram_concurrent_task=NoOp(),
first_buoy=False,
right=True,
yellow=False,
*args,
**kwargs):
super().__init__(*args, **kwargs)
self.logv("Starting {} task".format(self.__class__.__name__))
self.location_validator = location_validator
self.target_coordinates = target_coordinates
self.collision_validator = collision_validator
self.ram_concurrent_task = ram_concurrent_task
self.heading_task = HeadingRestore()
if first_buoy:
self.locator_task_a = LocateAlignedBuoy(self.location_validator,
forward=True)
else:
self.locator_task_a = LocateAdjacentBuoy(self.location_validator,
right=right)
self.locator_task_b = LocateAlignedBuoy(self.location_validator,
forward=False)
self.align_task = AlignTarget(self.location_validator,
self.locator_task_a,
self.target_coordinates, vision_group,
self.heading_task)
self.ram_task = RamTarget(
self.location_validator,
self.collision_validator,
# LocateBuoy(self.location_validator,checkBehind=True),
self.locator_task_b,
self.ram_concurrent_task)
if yellow:
self.forward_task = DirectionalSurge(6, .5)
self.retreat_task = DirectionalSurge(4, -.5)
else:
self.forward_task = DirectionalSurge(constants.FORWARD_TIME, .2)
self.retreat_task = DirectionalSurge(constants.BACKUP_TIME, -1)
self.depth_task = DepthRestore()
self.tasks = Sequential(
Zero(),
SeqLog("Restoring Depth"),
self.depth_task,
SeqLog("Locating Buoy"),
self.locator_task_a,
SeqLog("Aligning"),
self.align_task,
SeqLog("Approaching"),
self.ram_task,
Zero(),
Timer(.5),
)
# self.tasks = Sequential(Zero(), self.depth_task, self.locator_task, self.align_task,
# self.ram_task, self.forward_task, self.retreat_task,
# self.heading_task
# )
self.TIMEOUT = 90
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True),
#While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True),
),
Log("Centered on Pipe in PipeAlign!"),
#FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get()))
)
def t180(a):
return (a + math.pi) % (2 * math.pi)
find_bins = lambda: Sequential(
Depth(.1),
FunctionTask(VelocityX(.5)),
While(lambda: NoOp(), lambda: not shm.bins_status.cover_visible.get()),
VelocityX(0),
)
def get_cover_vect():
return np.float32(
[shm.bins_status.cover_maj_x.get(),
shm.bins_status.cover_maj_y.get()])
def get_cover_center():
return shm.bins_status.cover_x.get(), shm.bins_status.cover_y.get()
def get_wolf_center():
main_task=FunctionTask(lambda: gate_elems() == 1),
on_success=Concurrent(
focus_elem(lambda: shm.gate.leftmost_x, offset=offset),
PIDLoop(input_value=lambda: shm.gate.leftmost_len.get() /
(1e-30 + shm.gate.img_height.get()),
target=simple_approach_target_percent_of_screen,
output_function=VelocityX(),
p=3,
deadband=0.03),
finite=False),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 0),
on_success=Sequential(Log('we see no elems, failed'),
Timed(VelocityX(-0.2), 2),
finite=False),
on_fail=NoOp(),
finite=False),
finite=False),
Log('Approaching until we are aligned with two elements'),
Timeout(approach_side_task(), 60), Zero(), Log('Pre Spin Charging...'),
FunctionTask(save_heading), (Depth(SPIN_DEPTH_TARGET, error=0.15)
if spin else NoOp()),
Succeed(Timeout(Heading(lambda: saved_heading, error=5), 5)),
Timed(
FinishIf(
condition=lambda: gate_elems() == 0,
task=Concurrent(
VelocityX(pre_spin_charge_vel),
Conditional(main_task=FunctionTask(lambda: gate_elems() ==
2),
on_success=PIDLoop(
surfaces=False,
timeout=timeouts['highway'],
)
wait_for_track = MissionTask(
name='StuckInTraffic',
cls=WaitForTime,
modules=None,
surfaces=False,
)
track = MissionTask(
name='Track',
cls=TrackerGetter(
# We can't actually find roulette because the vision module is disabled
found_roulette=NoOp(),
found_cash_in=NoOp(),
enable_roulette=False,
),
modules=[shm.vision_modules.CashInDownward],
surfaces=False,
on_exit=TrackerCleanup(),
timeout=timeouts['track'],
)
# get a little closer to cash-in before we start tracking
interlude = lambda dist, speed: lambda: MissionTask(
name='Interlude',
cls=Sequential(FakeMoveX(dist=dist, speed=speed), ),
modules=None,
surfaces=False,
t1 = PipeAlign(a1, math.pi / 2, .2, DEADBAND * 1.5)
t2 = PipeAlign(a1, math.pi / 2, 0, DEADBAND * 1.5)
t3 = PipeAlign(a2, -math.pi / 2, 0, DEADBAND)
t4 = PipeAlign(a2, -math.pi / 2, .3, DEADBAND)
return Sequential(Log(s), Log("t1"), t1, Timer(.5), Log("t2"), t2,
Timer(.15), Log("t3"), t3, Timer(.5), Log("t4"), t4,
Timer(.5), Log("done"))
path2 = Sequential(
Log("Searching for path..."),
While(
lambda: Conditional(
FunctionTask(lambda: shm.path_results.num_lines.get() in (0, 1)),
on_success=MasterConcurrent(
While(lambda: NoOp(), lambda: shm.path_results.num_lines.get()
< 2),
Log("Centering on blob..."),
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, 0),
deadband=(.1, .1),
px=.5,
py=.5,
ix=.05,
iy=.05),
),
on_fail=Sequential(Log("Blind search..."), SearchTask())),
lambda: shm.path_results.num_lines.get() < 2), Zero(),
Log("Found Pipe!"),
Conditional(
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),
),
# If this is the second buoy, then don't search again goddamit
on_fail=NoOp(),
))
# This is the radius of the dots on the die
MIN_DOT_RADIUS = settings.min_dot_radius
BackUp = lambda: Sequential(
Log('Backing up...'),
def __init__(self,
location_validator,
target_coordinates,
vision_group,
collision_validator,
ram_concurrent_task=NoOp(),
first_buoy=False,
right=True,
yellow=False,
*args,
**kwargs):
"""
location_validator - a function that returns True when the target has
been found and False otherwise
target_coordinates - a tuple representing the coordinates of the target
in the xz-plane
vision_group - the shm group for the buoy
collision_validator - a function that returns True when there has been
a collision with the target and False otherwise.
ram_concurrent_task - an optional task to run concurrently when ramming
the target
"""
super().__init__(*args, **kwargs)
self.logv("Starting {} task".format(self.__class__.__name__))
self.location_validator = location_validator
self.target_coordinates = target_coordinates
self.collision_validator = collision_validator
self.ram_concurrent_task = ram_concurrent_task
self.heading_task = HeadingRestore()
if first_buoy:
self.locator_task_a = LocateAlignedBuoy(self.location_validator,
forward=True)
else:
self.locator_task_a = LocateAdjacentBuoy(self.location_validator,
right=right)
self.locator_task_b = LocateAlignedBuoy(self.location_validator,
forward=False)
self.align_task = AlignTarget(self.location_validator,
self.locator_task_a,
self.target_coordinates, vision_group,
self.heading_task)
self.ram_task = RamTarget(
self.location_validator,
self.collision_validator,
# LocateBuoy(self.location_validator,checkBehind=True),
self.locator_task_b,
self.ram_concurrent_task)
if yellow:
self.forward_task = DirectionalSurge(6, .5)
self.retreat_task = DirectionalSurge(3, -.5)
else:
self.forward_task = DirectionalSurge(constants.FORWARD_TIME, .2)
self.retreat_task = DirectionalSurge(constants.BACKUP_TIME, -1)
self.depth_task = DepthRestore()
self.tasks = Sequential(Zero(),
SeqLog("Restoring Depth"), self.depth_task,
SeqLog("Locating Buoy"), self.locator_task_a,
SeqLog("Aligning"), self.align_task,
SeqLog("Approaching"), self.ram_task, Zero(),
Timer(.5),
SeqLog("Ramming"), self.forward_task,
SeqLog("Backing up"), self.retreat_task,
SeqLog("Restoring heading"), self.heading_task)
# self.tasks = Sequential(Zero(), self.depth_task, self.locator_task, self.align_task,
# self.ram_task, self.forward_task, self.retreat_task,
# self.heading_task)
self.TIMEOUT = 90
wtf = Sequential(Concurrent(PauseTwice(),
Sequential(PauseTwice(), PauseTwice()),
Concurrent(PauseTwice(), PauseTwice())),
PauseTwice())
while not wtf.finished:
wtf()
# Note: A lambda was used to create new instances of the task. Without the lambda, all the tasks would be running the
# same pause_twice task (and finish after only 2 seconds!). With the lambda wtf takes teh expected 12 seconds.
# Other combinators and primitive tasks.
# Conditional runs one task if an argument is true and another if it isn't.
# (NoOp is a task from primitive that simply does nothing)
probably = Conditional(lambda: True, Pause(), NoOp())
## State
# Now, let's use some state.
# SHM should be used as it is normally
class DisplayBuoyInfo(Task):
def run(self):
# Reading from Shared Memory
self.log('Buoy information: X: {0}, Y: {1}, Area: {2}, Probability: {3}'
.format(red_buoy_results.center_x.get(),
red_buoy_results.center_y.get(),
red_buoy_results.area.get(),
red_buoy_results.probability.get()))
def on_run(self):
self.use_task(Fail(NoOp()))