class first_center(Task):
def on_first_run(self, results):
self.update_data(results)
path_found = self.path_results.visible > 0
self.centered_checker = ConsistencyCheck(8, 10)
self.center = DownwardTarget(
lambda self=self:
(self.path_results.center_x, self.path_results.center_y),
target=(0, 0),
deadband=(.05, .05),
px=0.5,
py=0.5,
dx=0.02,
dy=0.02,
valid=path_found)
def on_run(self, results):
self.update_data(results)
self.center()
# if not check_seen(results):
# self.finish(success=False)
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
class center(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(18, 20)
self.center = DownwardTarget(lambda self=self:
(self.pipe_results.center_x * 100, self.
pipe_results.center_y * 100),
target=(0, 0),
deadband=(4, 4),
px=0.03,
py=0.03,
dx=0.01,
dy=0.01,
valid=pipe_found)
self.logi("Beginning to center on the pipe")
self.logi("Pipe center" + str(self.pipe_results.center_x) + " " +
str(self.pipe_results.center_y))
def on_run(self):
self.update_data()
self.center()
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
def set_REMOVING(self, xp, yp, hp):
self.state = State.REMOVING
self.remove_start = time.time()
self.old_x = xp
self.old_y = yp
self.old_h = hp
self.removing_initial_hd = normalized_hd()
# handle losing sight of the handle better
# XXX will need to be tweaked, Zander is tweaking vision GUI, grabber, etc
self.remove_positioning_task = Sequential(
OrientTask(lambda: self.removing_initial_hd),
ZeroTask(),
WaitTask(3),
Finite(DownwardTarget(point=self.handlef, min_out=0.1)),
ZeroTask(),
DescendTask(HANDLE_GRAB_DEPTH_PRE),
LogTask("Descending for pre-grab..."),
Concurrent(OrientTask(lambda: self.removing_initial_hd),
Finite(DownwardTarget(point=self.handlef))),
ZeroTask(),
LogTask("Adjusting offset..."),
TimedTask(RelPosTask(REMOVE_SURGE, REMOVE_SWAY), 8),
# note that they are flipped here because the sub is rotated 90 when dropping/removing
ZeroTask(),
#DistSurgeTask(d=REMOVE_SURGE),
#DistSwayTask(d=REMOVE_SWAY),
DescendTask(HANDLE_GRAB_DEPTH),
LogTask("grabbing handle"),
ThunkTask(grab_handle),
WaitTask(2),
DescendTask(DISCARD_DEPTH))
class center(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(8, 10)
self.center = DownwardTarget(
lambda self=self:
(self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(30, 30),
px=0.002,
py=0.002,
dx=0.002,
dy=0.002,
valid=pipe_found)
#self.logi("Beginning to center on the pipe")
def on_run(self):
self.update_data()
self.center()
#self.logi("Results Y: {}".format(str(self.pipe_results.center_y)))
#self.logi("Center: {}".format(str(get_downward_camera_center()[1])))
if not check_seen():
self.finish(success=False)
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(8, 10)
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(30,30), px=0.002, py=0.002, dx=0.002, dy=0.002,
valid=pipe_found)
def on_first_run(self):
self.update_data()
self.align = Heading(
lambda: self.pipe_results.angle + shm.kalman.heading.get(),
deadband=0.5)
self.alignment_checker = ConsistencyCheck(19, 20)
pipe_found = self.pipe_results.heuristic_score > 0
self.center = DownwardTarget(lambda self=self:
(self.pipe_results.center_x * 100, self.
pipe_results.center_y * 100),
target=(0, 0),
deadband=(1, 1),
px=0.03,
py=0.03,
dx=0.01,
dy=0.01,
valid=pipe_found)
self.logi("Beginning to align to the pipe's heading")
c_center = "Camera Center: " + str((.5, .5))
self.logi("Pipe center" + str(self.pipe_results.center_x) + " " +
str(self.pipe_results.center_y))
self.logi(c_center)
def on_first_run(self, results):
self.update_data(results)
path_found = self.path_results.visible > 0
self.centered_checker = ConsistencyCheck(8, 10)
self.center = DownwardTarget(
lambda self=self:
(self.path_results.center_x, self.path_results.center_y),
target=(0, 0),
deadband=(.05, .05),
px=0.5,
py=0.5,
dx=0.02,
dy=0.02,
valid=path_found)
def set_DROPPING(self, target, xp, yp):
self.state = State.DROPPING
self.drop_target = target
self.drop_start = time.time()
target_group = None
if target is Goal.PRIMARY:
target_group = DROP_PRIMARY_GROUP
else:
target_group = DROP_SECONDARY_GROUP
self.drop_target_group = target_group
def targetf():
target = target_group.get()
if target.p >= VISIBLE_P_THRESH:
log("targetf", "(%f, %f)" % (target.x, target.y))
return (target.x, target.y)
else:
log("targetf", "FAILED TO SEE TARGET, providing center")
return (shm.camera.downward_width.get() / 2,
shm.camera.downward_height.get() / 2)
self.drop_initial_heading = shm.kalman.heading.get()
self.drop_positioning_task = Sequential(
LogTask("drop targeting..."),
Finite(DownwardTarget(point=targetf, min_out=0.1)),
LogTask("drop orienting..."),
OrientTask(lambda: (self.drop_initial_heading + 90) % 360),
LogTask("drop waiting..."),
WaitTask(3),
LogTask("drop retargeting..."),
Finite(DownwardTarget(point=targetf, min_out=0.05)),
LogTask("drop adjusting..."),
TimedTask(RelPosTask(DROP_SURGE, DROP_SWAY), 8),
ZeroTask(),
#DistSurgeTask(DROP_SURGE),
#DistSwayTask(DROP_SWAY),
DescendTask(TARGET_DROP_DEPTH),
DropTask(),
WaitTask(3),
TimedTask(RelPosTask(-DROP_SURGE, -DROP_SWAY), 8),
OrientTask(lambda: self.drop_initial_heading),
)
def downward_target_task(pt=(0, 0), deadband=(0.1, 0.1), max_out=0.04):
return DownwardTarget(
point=(lambda: shm_group_getter().center_x.get(), lambda: shm_group_getter().center_y.get()),
target=norm_to_vision_downward(*pt),
deadband=norm_to_vision_downward(-1.0 + deadband[0], -1.0 + deadband[1]),
px=0.0005,
py=0.001,
max_out=max_out,
)
def on_first_run(self, points_func, target=(0, 0), precision=0, *args, **kwargs):
self.task = ConsistentTask(DownwardTarget(
point=lambda: self.centroid(points_func),
target=target,
deadband=self.DEADBANDS[precision],
px=self.PS[precision],
py=self.PS[precision],
dx=self.DS[precision],
dy=self.DS[precision],
max_out=self.MAX_SPEED,
))
def move_to_next_and_drop():
return DelimitBecome(
WithLazyProc(
lambda: foldl(
lambda a, l: a or l.y <= shm.camera.downward_height / 2, False,
[g.get() for g in bin_groups]),
lambda up: become(
Sequential(
DistSurgeTask(1.0),
ZeroAfter(
TimedTask(DownwardTarget(point=most_central_bin_point),
10)), drop_over_most_central())))),
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(18, 20)
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=lambda self=self: get_camera_center(self.pipe_results),
deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001,
valid=pipe_found)
self.logi("Beginning to center on the pipe")
def on_first_run(self):
self.update_data()
self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5)
self.alignment_checker = ConsistencyCheck(49, 50)
pipe_found = self.pipe_results.heuristic_score > 0
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(10,10), px=0.001, py=0.001, dx=0.002, dy=0.002,
valid=pipe_found)
class center(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(18, 20)
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001,
valid=pipe_found)
self.logi("Beginning to center on the pipe")
def on_run(self):
self.update_data()
self.center()
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
def drop_over_most_central():
WithLazyProc(
lambda: (most_central_bin_heading_abs(), shm.kalman.heading.get()),
lambda x: become(
Sequential(
LogTask("drop orienting..."),
OrientTask(lambda:
(x[0] + 90) % 360), LogTask("drop waiting..."),
WaitTask(2), LogTask("drop retargeting..."),
ZeroAfter(
Finite(
DownwardTarget(point=most_central_bin_point,
min_out=0.05))),
LogTask("drop adjusting..."),
ZeroAfter(TimedTask(RelPosTask(DROP_SURGE, DROP_SWAY), 5)),
DescendTask(TARGET_DROP_DEPTH), ThunkTask(drop_dropper),
WaitTask(2),
ZeroAfter(TimedTask(RelPosTask(-DROP_SURGE, -DROP_SWAY), 5)),
OrientTask(lambda: x[1]))))
self._finish()
QuickBins = Catch(
lambda e: logged("Exception, dropping both droppers!", drop_both_droppers),
Timeout(
120,
Sequential(
DescendTask(SEARCH_ALIGN_DEPTH),
Until(lambda: shm.desires.speed.set(0.1),
lambda: contour_count() >= 2 or handle_visible(),
lambda: shm.desires.speed.set(0)),
Sequential(
LazyIf(
handle_visible,
TimedTask(DownwardTarget(point=handle_point), 10),
TimedTask(DownwardTarget(point=most_central_bin_point),
10)),
LazyIf(
handle_visible,
Sequential(
LogTask("going for handle"), ThunkTask(lower_grabber),
ZeroAfter(WithLazy(handle_heading_abs, OrientTask)),
WaitTask(3),
ZeroAfter(
TimedTask(DownwardTarget(point=handle_point), 10)),
DescendTask(HANDLE_GRAB_DEPTH_PRE),
LogTask("Descending for pre-grab..."),
ZeroAfter(
Concurrent(
WithLazy(handle_heading_abs, OrientTask),
# TODO this might not be working
print(angle_1, angle_2, diff)
if self.angle_1_checker.check(
diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish()
if self.angle_2_checker.check(
diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish(success=False)
PipeAlign = lambda heading: Concurrent(
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, -.25),
deadband=(.1, .1),
px=0.5,
py=0.5), Log("Centered on Pipe!"),
FunctionTask(lambda: shm.navigation_desires.heading.set(
-180 / 3.14 * heading.get() + shm.kalman.heading.get())))
FollowPipe = lambda h1, h2: Sequential(
PipeAlign(h1),
Zero(),
Log("Aligned To Pipe!"),
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(0, 0),
deadband=(.1, .1),
px=0.5,
py=0.5),
#!/usr/bin/env python3.4
from mission.framework.combinators import Sequential, Concurrent
from mission.framework.movement import Depth
from mission.framework.targeting import DownwardTarget, HeadingTarget
from shm import shape_handle, shape_lightning, shape_banana, shape_soda, shape_bijection
CenterCover = Sequential(
Depth(1),
Concurrent(
DownwardTarget((shape_handle.x.get, shape_handle.y.get),
target=(200, 200),
deadband=(0, 0))))
CenterLightning = Sequential(
Depth(1),
Concurrent(DownwardTarget((shape_lightning.x.get, shape_lightning.y.get))))
CenterBanana = Sequential(
Depth(1),
Concurrent(DownwardTarget((shape_banana.x.get, shape_banana.y.get))))
CenterSoda = Sequential(
Depth(1), Concurrent(DownwardTarget((shape_soda.x.get, shape_soda.y.get))))
CenterBijection = Sequential(
Depth(1),
Concurrent(DownwardTarget((shape_bijection.x.get, shape_bijection.y.get))))
Log("Picked Up"),
cons(Heading(45)),
Log("Turned to hardcoded heading"),
drop_all,
Log("Dropped"),
)
left = make_bin_chooser(True)
right = make_bin_chooser(False)
center_left = cons (
DownwardTarget(
point=(lambda: left().center_x.get(), lambda: left().center_y.get()),
target=norm_to_vision_downward(0, 0),
deadband=norm_to_vision_downward(-0.9, -0.9),
px=0.0005,
py=0.001,
max_out=0.5,
),
debug=True
)
center_right = cons(
DownwardTarget(
point=(lambda: right().center_x.get(), lambda: right().center_y.get()),
target=norm_to_vision_downward(0, 0),
deadband=norm_to_vision_downward(-0.9, -0.9),
px=0.0005,
py=0.001,
max_out=0.5,
),
def run(self, todo):
log("SearchAlignedTask", "searching in aligned mode...")
if self.continuing:
if time.time() - self.period_start >= 1:
self._finish()
shm.desires.sway_speed.set(0)
self.continuing = False
return
if (Goal.PRIMARY in todo and DROP_PRIMARY_GROUP.p.get() >= VISIBLE_P_THRESH) \
or (Goal.SECONDARY in todo and DROP_SECONDARY_GROUP.p.get() >= VISIBLE_P_THRESH):
log("SearchAlignedTask",
"saw something, continuing for a bit just in case!")
# this is to account for the roll changes caused by sway
self.continuing = True
self.period_start = time.time()
return
shm.desires.depth.set(SEARCH_ALIGN_DEPTH)
if self.turning:
log("SearchAlignedTask", "turning around...")
if time.time() - self.period_start >= 1:
shm.desires.sway_speed.set(0)
shm.desires.speed.set(0)
self.turning = False
self.period_start = time.time()
self.posx = not self.posx
if not self.turning:
if time.time() - self.period_start >= SEARCH_ALIGN_PERIOD_S * (
0.5 if self.first else 1):
self.turning = True
self.period_start = time.time()
self.first = False
return
most = None
mostx = -10 if self.posx else 100000000000
for shape in [
s for s in shape_groups if s.p.get() >= VISIBLE_P_THRESH
]:
if self.posx:
if shape.x.get() > mostx:
most = shape
mostx = shape.x.get()
else:
if shape.x.get() < mostx:
most = shape
mostx = shape.x.get()
if most is None:
log("SearchAlignedTask/diagnostic", "setting fake most!")
most = FakeXY(
shm.camera.downward_width.get() / 2 + 200 *
(1 if self.posx else -1),
shm.camera.downward_height.get() / 2)
else:
log("SearchAlignedTask/diagnostic",
"most: %d, %d" % (most.x.get(), most.y.get()))
if most is not None and most is not self.target_shape:
self.target_shape = most
self.task = DownwardTarget(point=lambda:
(most.x.get(), most.y.get()),
min_out=0.1)
if self.task is None:
if self.posx:
shm.desires.sway_speed.set(SEARCH_ALIGN_SPEED)
else:
shm.desires.sway_speed.set(-SEARCH_ALIGN_SPEED)
else:
self.task()
visiblef,
consistent_frames=(15, 19)
),
Zero(),
Depth(INITIAL_DEPTH, error=0.2))
SearchAnyVampire = lambda: Search(any_visible)
close_to = lambda point1, point2, dbx=20, dby=20: abs(point1[0]-point2[0]) < dbx and abs(point1[1]-point2[1]) < dby
Center = lambda centerf, visiblef, db=15, px=0.001, py=0.001, dx=0.00005, dy=0.00005: Sequential(
Log('Centering'),
MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER, db), count=2.5, total=3.0, invert=False, result=True),
Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False),
While(lambda: DownwardTarget(point=centerf, target=CAM_CENTER, deadband=(0, 0), px=px, py=py), True),
AlwaysLog(lambda: 'center = {}, target = {}'.format(centerf(), CAM_CENTER))))
CenterAny = lambda: Center(center_any, any_visible)
# Descend = lambda depth=DEPTH, db=0.1, size_thresh=SIZE_THRESH: Sequential( # TODO: FIND THE ACTUAL DEPTH1!!
# Log('Descent into Madness'),
# MasterConcurrent( # TODO: TIMEOUT
# Consistent(lambda: abs(shm.kalman.depth.get() - depth) < db or size() > size_thresh, count=2.3, total=3, invert=False, result=True),
# Depth(depth)), # TODO: BigDepth?
# Zero())
close_to = lambda point1, point2, db=20: abs(point1[0]-point2[0]) < db and abs(point1[1]-point2[1]) < db
Align = lambda centerf, anglef, visiblef, closedb=10, aligndb=7: Sequential(
Log('Aligning'),
def size():
return shm.recovery_garlic.size.get()
Search = lambda: Sequential( # TODO: TIMEOUT?
Log('Searching'),
SearchFor(SwaySearch(width=2.5, stride=2),
visible,
consistent_frames=(5, 7)), Zero())
Center = lambda db=40, px=0.0008, py=0.0008: Sequential(
Log('Centering'),
MasterConcurrent(
DownwardTarget(
point=center, target=CAM_CENTER, deadband=(db, db), px=px, py=py),
AlwaysLog(lambda: 'center = {}, target = {}'.format(
center(), CAM_CENTER))))
Descend = lambda depth=DEPTH, db=0.1, size_thresh=SIZE_THRESH: Sequential( # TODO: FIND THE ACTUAL DEPTH1!!
Log('Descent into Madness'),
MasterConcurrent( # TODO: TIMEOUT
Consistent(lambda: abs(shm.kalman.depth.get() - depth) < db or size() >
size_thresh,
count=2.3,
total=3,
invert=False,
result=True), Depth(depth)), # TODO: BigDepth?
Zero())
close_to = lambda point1, point2, db=10: abs(point1[0] - point2[
def set_SURVEYING(self):
self.state = State.SURVEYING
self.survey_task = Concurrent(
DownwardTarget(point=center_approx, min_out=0.1), SurveyTask())
self.survey_start = time.time()
DEPTH_TARGET_ALIGN_BIN = settings.depth_realign
DEPTH_TARGET_DROP = settings.depth_drop
CAM_CENTER = (cameras['downward']['width'] / 2,
cameras['downward']['height'] / 2)
BIN_CENTER = [shm.bins_vision.center_x, shm.bins_vision.center_y]
#GREEN_CENTER = [shm.bins_green0.centroid_x, shm.bins_green0.centroid_y]
GREEN_CENTER = BIN_CENTER
GREEN_ANGLE = shm.bins_green0.angle
HEADING_OFFSET = settings.heading_offset
align_roulette_center = lambda db=20, p=0.0005: DownwardTarget(
(BIN_CENTER[0].get, BIN_CENTER[1].get),
target=CAM_CENTER,
px=p,
py=p,
deadband=(db, db))
align_green_angle = lambda db=10, p=0.8: DownwardAlign(
lambda: GREEN_ANGLE.get() + HEADING_OFFSET, target=0, deadband=db, p=p)
DropBall = lambda: FireBlue()
Search = lambda: SearchFor(
SaneHeadingSearch(),
shm.bins_vision.board_visible.get,
consistent_frames=(1 * 60, 1.5 * 60
) # multiply by 60 to specify in seconds
)
Full = lambda: Retry(
abs(shm.path_results.center_x.get() - v[0]) < deadband and \
abs(shm.path_results.center_y.get() - v[1]) < deadband
PipeAlign = lambda heading, trg, dst, deadband: Sequential(
Log("PipeAlign start"),
MasterConcurrent(
While(
lambda: Sequential(
Log("attempt asdasd"),
Concurrent(
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(lambda: heading_to_vector(heading())
* -dst),
deadband=(deadband, deadband),
px=1,
py=1,
ix=.05,
iy=.05),
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires
.heading.set((shm.kalman.heading.get(
) - .95 * math.degrees(
heading_sub_degrees(
trg, heading(), math.pi * 2)))
% 360)
if shm.path_results.num_lines.get(
) == 2 else None),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'),
def PipeAlign(get_center,
heading_vect,
heading,
get_visible,
trg,
dst,
deadband,
angle_range=math.pi * 2,
abs_offset=(0, 0)):
return Sequential(
Log("PipeAlign start"),
MasterConcurrent(
Consistent(lambda: is_done(heading_vect, heading, get_center, trg,
dst, deadband, abs_offset),
count=.5,
total=.75,
invert=False,
result=True),
While(
lambda: Sequential(
#Log("attempt asdasd"),
Concurrent(
DownwardTarget(
lambda: get_center(),
target=lambda:
(np.float64([heading_vect()]).view(np.complex128) *
-dst).view(np.float64)[0] + abs_offset,
deadband=(deadband, deadband),
px=2,
py=2,
dx=.5,
dy=.5 #, ix=.15, iy=.15
),
While(
lambda: Sequential(
FunctionTask(
lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get(
) - .95 * math.degrees(
heading_sub_degrees(
trg, heading(), angle_range))
) % 360) if get_visible() else None),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get()))),
Timer(.05)),
lambda: abs(
heading_sub_degrees(trg, heading(), math.pi * 2
)) > math.radians(5)
) #, count=6, total=8, invert=False, result=True),
)),
lambda:
True #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08
),
#, count=3, total=4, invert=False, result=True),
#While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True),
#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()))
)