def on_first_run(self, desire, *args, **kwargs):
subtasks = []
increment = 20
deadband = 25
current = shm.kalman.heading.get()
while abs(heading_sub_degrees(current, desire)) % 360 > deadband:
current -= math.copysign(increment, heading_sub_degrees(current, desire))
subtasks.append(Heading(current))
self.task = Sequential(subtasks=subtasks)
def on_run(self, target, *args, **kwargs):
current = shm.kalman.heading.get()
diff = heading_sub_degrees(target, current)
inc = [-1, 1][diff > 0] * 12
RelativeToCurrentHeading(inc)()
if abs(diff) < 5:
self.finish()
def on_run(self, n):
new_roll = shm.kalman.roll.get()
self.cumul += heading_sub_degrees(new_roll, self.roll)
self.roll = new_roll
if abs(self.cumul) > 360 * n:
self.finish()
def on_run(self, desire, *args, **kwargs):
current = shm.kalman.heading.get()
diff = heading_sub_degrees(desire, current)
relative = math.copysign(self.relative_desire, diff)
RelativeToCurrentHeading(relative)()
if abs(diff) < self.deadband:
self.finish()
def on_run(self, n):
new_heading = shm.kalman.heading.get()
self.cumul += heading_sub_degrees(new_heading, self.heading)
self.heading = new_heading
if abs(self.cumul) > 360 * n:
self.finish()
def on_run(self, target, *args, **kwargs):
current = shm.kalman.heading.get()
diff = heading_sub_degrees(target, current)
inc = [-1, 1][diff > 0] * 12
RelativeToCurrentHeading(inc)()
if abs(diff) < 5:
self.finish()
def on_run(self, n):
new_heading = shm.kalman.heading.get()
self.cumul += heading_sub_degrees(new_heading, self.heading)
self.heading = new_heading
if abs(self.cumul) > 360 * n:
self.finish()
def find_closest_angle(self, target, *angles, post=False):
if len(angles) == 0: return
# print(angles)
closest = min(angles, key=lambda x: abs_heading_sub_degrees(target, x))
print(closest)
if post:
shm.bins_garlic.angle_offset.set(heading_sub_degrees(target, closest))
return closest
def on_run(self, desire, relative_desire, relative_deadband, *args, **kwargs):
current = shm.kalman.heading.get()
diff = heading_sub_degrees(call_if_function(desire), current)
if abs(diff) < relative_deadband:
self.finish()
return
relative = math.copysign(relative_desire, diff)
self.rel_curr_heading(relative)
def is_done(heading_vect, heading, pos, trg, dst, deadband, abs_offset):
v = (np.float64([heading_vect()]).view(np.complex128) * -dst).view(
np.float64)[0] + abs_offset
pos_var = pos()
#print(shm.bins_status.cover_x.get(), shm.bins_status.cover_y.get(), v)
#print(pos_var)
print(pos_var - v, trg, heading())
vv = abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(5) and \
abs(pos_var[0] - v[0]) < deadband and \
abs(pos_var[1] - v[1]) < deadband
#print(vv)
return vv
def find_crucifix_angles(self, garlic_mask):
lines = find_lines(garlic_mask, 2, pi / 180,
self.options['garlic_line_threshold'])[0]
# print(lines)
center = garlic_mask.shape[0] // 2, garlic_mask.shape[1] // 2
def distance_from_center(line):
num = abs((line[3] - line[1]) * center[1] -
(line[2] - line[0]) * center[0] + line[2] * line[1] -
line[3] * line[0])
denom = sqrt((line[3] - line[1])**2 + (line[2] - line[0])**2)
return num / denom
lines = list(filter(lambda x: distance_from_center(x) < 20, lines))
angles = np.array([lines_to_angles(l) * 180 / pi for l in lines],
dtype=np.float32)
average = average_headings_degrees(angles) + 90
# print(average)
shm.recovery_crucifix.angle_offset.set(
heading_sub_degrees(self.options['manipulator_angle'], average))
return lines, average
def find_vampire(self, mat, split, color, distance):
# mask = thresh_color_distance(split, color, distance)
mask, _ = thresh_color_distance(split, color, distance, weights=[0.5, 2, 2])
self.post('purple', mask)
rects = self.intersect_rectangles(self.rectangles, mask, self.options['intersection_size_min'])
if self.rectangles:
empty = max([r['rectangle'] for r in self.rectangles], key=lambda r: r[1][0] * r[1][1])
align_angle = empty[2] + 90 if empty[1][1] > empty[1][0] else empty[2]
align_angle = 360 + align_angle if align_angle < 0 else align_angle
shm.recovery_vampire.empty_visible.set(True)
shm.recovery_vampire.empty_x.set(int(empty[0][0]))
shm.recovery_vampire.empty_y.set(int(empty[0][1]))
shm.recovery_vampire.empty_offset_x.set(int(empty[0][0] + min(empty[1]) * self.options['open_offset']))
shm.recovery_vampire.empty_offset_y.set(int(empty[0][1]))
shm.recovery_vampire.empty_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], align_angle))
shm.recovery_vampire.empty_size.set(empty[1][0] * empty[1][1])
cv2.circle(mat, (int(empty[0][0]), int(empty[0][1])), 5, color=(255, 255, 255), thickness=-1)
opened = []
closed = []
for j in range(len(rects)):
i, mask_r = rects[j]
self.post('rectangle_%d' % j, mask_r)
purple = cv2.bitwise_and(mask, mask_r)
purple_contours = outer_contours(purple)
purple_center = contour_centroid(max(purple_contours, key=contour_area))
# print(purple_center)
align_angle = self.rectangles[i]['rectangle'][2] if self.rectangles[i]['rectangle'][1][1] > self.rectangles[i]['rectangle'][1][0] else self.rectangles[i]['rectangle'][2] + 90
align_angle = 360 + align_angle if align_angle < 0 else align_angle
def point_in_rectangle(point, rect):
contour = np.float32([cv2.boxPoints(rect)]).reshape(-1, 1, 2)
return cv2.pointPolygonTest(contour, point, measureDist=False)
if point_in_rectangle(purple_center, self.rectangles[i]['rectangle_org']) > 0:
color = (0, 0, 255)
opened.append({'center': purple_center, 'align': align_angle, 'size': self.rectangles[i]['rectangle'][1][0] * self.rectangles[i]['rectangle'][1][1], 'offset': (int(min(self.rectangles[i]['rectangle'][1]) * self.options['open_offset']), int(max(self.rectangles[i]['rectangle'][1]) * self.options['vert_offset']))})
else:
color = (255, 0, 0)
direction = 1 if self.rectangles[i]['rectangle'][0][0] > purple_center[0] else -1
closed.append({'center': purple_center, 'align': ((align_angle + 180) % 360) if direction == 1 else align_angle, 'size': self.rectangles[i]['rectangle'][1][0] * self.rectangles[i]['rectangle'][1][1], 'offset': (int(min(self.rectangles[i]['rectangle'][1]) * self.options['closed_offset']), int(max(self.rectangles[i]['rectangle'][1]) * self.options['vert_offset'])), 'direction': direction})
# cv2.circle(mat, purple_center, 20, color=color, thickness=-1)
# draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=purple_center), thickness=5)
# draw_line(mat, *angle_to_line(align_angle, origin=purple_center), color=color, thickness=5)
opened = max(opened, key=lambda x: x['size']) if opened else None
closed = max(closed, key=lambda x: x['size']) if closed else None
if opened:
cv2.circle(mat, opened['center'], 5, color=(0, 0, 255), thickness=-1)
draw_line(mat, *angle_to_line(opened['align'], origin=opened['center']), color=(0, 0, 255), thickness=5)
draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=opened['center']), thickness=5)
# print('nani %d' % opened['align'])
shm.recovery_vampire.open_visible.set(True)
shm.recovery_vampire.open_handle_x.set(opened['center'][0])
shm.recovery_vampire.open_handle_y.set(opened['center'][1])
shm.recovery_vampire.open_offset_x.set(opened['center'][0] + opened['offset'][0])
shm.recovery_vampire.open_offset_y.set(opened['center'][1] + opened['offset'][1])
shm.recovery_vampire.open_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], opened['align']))
shm.recovery_vampire.open_size.set(opened['size'])
else:
shm.recovery_vampire.open_visible.set(False)
if closed:
draw_line(mat, *angle_to_line(closed['align'], origin=closed['center']), color=(0, 255, 0), thickness=5)
draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=closed['center']), thickness=5)
# print('what %d' % closed['align'])
cv2.circle(mat, closed['center'], 5, color=(0, 255, 0), thickness=-1)
shm.recovery_vampire.closed_visible.set(True)
shm.recovery_vampire.closed_handle_x.set(closed['center'][0])
shm.recovery_vampire.closed_handle_y.set(closed['center'][1])
shm.recovery_vampire.closed_handle_direction.set(closed['direction'])
shm.recovery_vampire.closed_offset_x.set(closed['center'][0] + closed['offset'][0])
shm.recovery_vampire.closed_offset_y.set(closed['center'][1])
shm.recovery_vampire.closed_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], closed['align']))
shm.recovery_vampire.closed_size.set(closed['size'])
else:
shm.recovery_vampire.closed_visible.set(False)
self.post('hmm', mat)
def desired_heading():
return shm.kalman.heading.get() - (heading_sub_degrees(
call_if_function(desire),
call_if_function(current),
mod=mod
))
v = heading_to_vector(heading()) * -dst
print(shm.path_results.center_x.get(), shm.path_results.center_y.get(), v)
return abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(3) and \
abs(shm.path_results.center_x.get() - v[0]) < deadband and \
abs(shm.path_results.center_y.get() - v[1]) < deadband
# def on_run(self, error, p=0.0005, i=0, d=0.0, db=0.01875, max_out=0.5, negate=False, *args, **kwargs):
FollowLine = Concurrent(
PIDSway(shm.path_results.center_x.get, negate=True, db=0, p=1.2, i=.05),
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get() - .95 * math.degrees(
heading_sub_degrees(
math.pi / 2,
shm.path_results.angle_1.get() % math.pi, math.pi * 2))
) % 360)),
#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'),
Timer(.05)),
lambda: True))
aaa = Sequential(Log("Started mission"), Depth(1.5), Log("At depth"),
FakeMoveX(2, .65), Zero(), Log("Starting tracking"), Timer(1),
Log("following"),
Timed(Concurrent(VelocityX(.5), FollowLine), 24),
Log("following done"), Timed(VelocityX(-.5), .1), Zero(),
Timer(1), FakeMoveY(-.6, .65), Timed(VelocityY(.5), .05),
Zero(), Timer(1), FakeMoveX(2.8, .65),
Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"),
RelativeToInitialHeading(90), Log("turning 2"), Timer(1),
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()))
)
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'),
Timer(.05)),
lambda: abs(
heading_sub_degrees(trg, heading(), math.pi * 2)
) > math.radians(5)
) #, count=6, total=8, invert=False, result=True),
)),
lambda: not is_done(
heading, trg, dst, deadband
) #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
),
global space_time
global space_heading
global heading_var
sys.stdout.write("Press <ENTER> when sub is at %.3f heading\n" % heading)
event_function()
space_time = time.time()
space_heading = heading_var.get()
thread.start_new_thread(input_thread, (
raw_input,
target_heading,
))
time.sleep(0.001)
initial_diff = heading_sub_degrees(target_heading, heading_var.get())
while 1:
diff = heading_sub_degrees(target_heading, heading_var.get())
if initial_diff * diff < 0:
sensor_time = time.time()
break
if space_time is None:
sys.stdout.write(
"The sub reached the target heading before you pressed enter!\n")
while space_time is None:
pass
sys.stdout.write("Lag: %f\n" % (sensor_time - space_time))
sys.stdout.write("The sub was at %.3f degrees heading at enter\n" %
def is_done(heading, trg, dst, deadband):
v = heading_to_vector(heading()) * -dst
print(shm.path_results.center_x.get(), shm.path_results.center_y.get(), v)
return abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(5) and \
abs(shm.path_results.center_x.get() - v[0]) < deadband and \
abs(shm.path_results.center_y.get() - v[1]) < deadband
space_heading = None
def input_thread(event_function, heading):
global space_time
global space_heading
global heading_var
sys.stdout.write("Press <ENTER> when sub is at %.3f heading\n" % heading)
event_function()
space_time = time.time()
space_heading = heading_var.get()
thread.start_new_thread(input_thread, (raw_input, target_heading,))
time.sleep(0.001)
initial_diff = heading_sub_degrees(target_heading, heading_var.get())
while 1:
diff = heading_sub_degrees(target_heading, heading_var.get())
if initial_diff * diff < 0:
sensor_time = time.time()
break
if space_time is None:
sys.stdout.write("The sub reached the target heading before you pressed enter!\n")
while space_time is None:
pass
sys.stdout.write("Lag: %f\n" % (sensor_time - space_time))
sys.stdout.write("The sub was at %.3f degrees heading at enter\n" % space_heading)
