def _OLDbox_heading(self):
dx = self.box_x - self.config['vision/dropper_center_x']
dy = self.config['vision/dropper_center_y'] - self.box_y
if dy == 0:
dy = 0.001 #
deg = math.degrees(math.atan(dx * 1.0 / dy * 1.0))
if abs(deg) < 15:
return HeadingDelta(0)
return HeadingDelta(deg)
def _shape_heading(self):
"""
Assumes that there are multiple shapes in frame, and uses their locations to calculate
the heading of the row of bins
"""
# Get two points
x1 = None
x2 = None
y1 = None
y2 = None
min_prob = self.config['mission/slagathor_interest_threshold']
if shared_vars['net_probability'].get() > min_prob:
if x1 is None:
x1 = shared_vars['net_x'].get()
y1 = shared_vars['net_y'].get()
elif x2 is None:
x2 = shared_vars['net_x'].get()
y2 = shared_vars['net_y'].get()
if shared_vars['shield_probability'].get() > min_prob:
if x1 is None:
x1 = shared_vars['shield_x'].get()
y1 = shared_vars['shield_y'].get()
elif x2 is None:
x2 = shared_vars['shield_x'].get()
y2 = shared_vars['shield_y'].get()
if shared_vars['sword_probability'].get() > min_prob:
if x1 is None:
x1 = shared_vars['sword_x'].get()
y1 = shared_vars['sword_y'].get()
elif x2 is None:
x2 = shared_vars['sword_x'].get()
y2 = shared_vars['sword_y'].get()
if shared_vars['trident_probability'].get() > min_prob:
if x1 is None:
x1 = shared_vars['trident_x'].get()
y1 = shared_vars['trident_y'].get()
elif x2 is None:
x2 = shared_vars['trident_x'].get()
y2 = shared_vars['trident_y'].get()
# Transform (0,0) to be the center of the frame
if x1 is None or x2 is None or y1 is None or y2 is None:
return 0
x1 = x1 - shared_vars['downward_width'].get() / 2.0
y1 = -1 * y1 + shared_vars['downward_height'].get() / 2.0
x2 = x2 - shared_vars['downward_width'].get() / 2.0
y2 = -1 * y2 + shared_vars['downward_height'].get() / 2.0
# Calculate the angle between the line and the +x-axis
angle = math.degrees(math.atan2((y2 - y1), (x2 - x1)))
#Minimize the angle
if angle > 90:
angle -= 180
elif angle < -90:
angle += 180
return HeadingDelta(-1 * angle) #Flip sign due to coord frame
def _pipe_heading(self):
pipe_heading = shared_vars['pipe_heading'].get()
return HeadingDelta(
pipe_heading
) # this this is based off a slope, it should never return > 180
def _x_heading(self):
# difference in percent of the field of view
px = (self.x_x - shared_vars['forward_width'].get() / 2) / (
shared_vars['forward_width'].get() / 2)
return HeadingDelta(px * self.config['vision/forward_hor_fov'])
def _box_heading_delta(self):
return HeadingDelta(shared_vars['box_heading_delta'].get())