Merge "Move log reader and writer over to split timestamp channels"
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 14c2afc..2c94169 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -1,4 +1,5 @@
from gi.repository import Gtk
+from collections import namedtuple
window = Gtk.Window()
screen = window.get_screen()
@@ -17,23 +18,103 @@
ROBOT_SIDE_TO_HATCH_PANEL = 0.1
HATCH_PANEL_WIDTH = 0.4826
-FIELD = 2020
+FieldType = namedtuple(
+ 'Field', ['name', 'tags', 'year', 'width', 'length', 'json_name'])
-if FIELD == 2019:
- # Half Field
- WIDTH_OF_FIELD_IN_METERS = 8.258302
-elif FIELD == 2020:
- # Full Field
- WIDTH_OF_FIELD_IN_METERS = 15.98295
- LENGTH_OF_FIELD_IN_METERS = 8.21055
+GALACTIC_SEARCH = "Galactic Search"
+ARED = "A Red"
+BRED = "B Red"
+ABLUE = "A Blue"
+BBLUE = "B Blue"
+AUTONAV = "AutoNav"
+BOUNCE = "Bounce"
+SLALOM = "Slalom"
+BARREL = "Barrel"
+
+FIELDS = {
+ "2019 Field":
+ FieldType(
+ "2019 Field",
+ tags=[],
+ year=2019,
+ width=8.258302,
+ length=8.258302,
+ json_name="spline_2019.json"),
+ "2020 Field":
+ FieldType(
+ "2020 Field",
+ tags=[],
+ year=2020,
+ width=15.98295,
+ length=8.21055,
+ json_name="spline_2020.json"),
+ "2021 Galactic Search BRed":
+ FieldType(
+ "2021 Galactic Search BRed",
+ tags=[GALACTIC_SEARCH, BRED],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="spline_red_a.json"),
+ "2021 Galactic Search ARed":
+ FieldType(
+ "2021 Galactic Search ARed",
+ tags=[GALACTIC_SEARCH, ARED],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="spline_red_b.json"),
+ "2021 Galactic Search BBlue":
+ FieldType(
+ "2021 Galactic Search BBlue",
+ tags=[GALACTIC_SEARCH, BBLUE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="spline_blue_b.json"),
+ "2021 Galactic Search ABlue":
+ FieldType(
+ "2021 Galactic Search ABlue",
+ tags=[GALACTIC_SEARCH, ABLUE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="spline_blue_a.json"),
+ "2021 AutoNav Barrel":
+ FieldType(
+ "2021 AutoNav Barrel",
+ tags=[AUTONAV, BARREL],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="autonav_barrel.json"),
+ "2021 AutoNav Slalom":
+ FieldType(
+ "2021 AutoNav Slalom",
+ tags=[AUTONAV, SLALOM],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="autonav_slalom.json"),
+ "2021 AutoNav Bounce":
+ FieldType(
+ "2021 AutoNav Bounce",
+ tags=[AUTONAV, BOUNCE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ json_name="autonav_bounce.json"),
+}
+
+FIELD = FIELDS["2021 Galactic Search BRed"]
def pxToM(p):
- return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
+ return p * FIELD.width / SCREEN_SIZE
def mToPx(m):
- return (m * SCREEN_SIZE / WIDTH_OF_FIELD_IN_METERS)
+ return (m * SCREEN_SIZE / FIELD.width)
def inToM(i):
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index a364e40..8548b54 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -60,6 +60,91 @@
cr.scale(widthb, -heightb)
+def draw_at_home_grid(cr):
+ field = np.zeros(shape=(5, 11), dtype=bool)
+ # field[row from bottom][column from left]
+
+ if GALACTIC_SEARCH in FIELD.tags:
+ # Galactic search start zone
+ field[1][0] = True
+ field[3][0] = True
+
+ # Galactic search end zone
+ field[1][10] = True
+ field[3][10] = True
+
+ if ARED in FIELD.tags:
+ field[4][5] = True
+ field[2][2] = True
+ field[1][4] = True
+ elif ABLUE in FIELD.tags:
+ field[0][5] = True
+ field[3][6] = True
+ field[2][8] = True
+ elif BRED in FIELD.tags:
+ field[3][2] = True
+ field[1][4] = True
+ field[3][6] = True
+ elif BBLUE in FIELD.tags:
+ field[1][5] = True
+ field[3][7] = True
+ field[1][9] = True
+ elif AUTONAV in FIELD.tags:
+ # start/end zone
+ field[1][0] = True
+ field[1][1] = True
+ field[3][0] = True
+ field[3][1] = True
+
+ if BAREL in FIELD.tags:
+ # barrels
+ field[1][4] = True
+ field[3][8] = True
+ field[1][10] = True
+ if BARREL in FIELD.tags:
+ field[1][3:8] = True # 3 to 7 inclusive
+ field[1][9] = True
+ if BOUNCE in FIELD.tags:
+ # turn on two rows
+ field[1][:11] = True
+ field[3][:11] = True
+
+ # turn off parts of rows
+ field[3][2] = False
+ field[3][5] = False
+ field[3][8] = False
+
+ field[1][3] = False
+ field[1][5] = False
+ field[1][8] = False
+
+ # markers to hit
+ field[4][2] = True
+ field[4][5] = True
+ field[4][8] = True
+
+ # Move origin to bottom left
+ xorigin = -mToPx(FIELD.width) / 2.0
+ yorigin = -mToPx(FIELD.length) / 2.0
+
+ color = palette["BLACK"]
+ # markers are at least 6.35 x 6.35 cm
+ marker_length = mToPx(0.0635)
+
+ for row, row_array in enumerate(field):
+ for column, has_marker in enumerate(row_array):
+ one_indexed_row = row + 1
+ one_indexed_column = column + 1
+
+ # 76.2 cm increments
+ pos_y = one_indexed_row * mToPx(0.762)
+ pos_x = one_indexed_column * mToPx(0.762)
+
+ if has_marker:
+ draw_px_x(cr, xorigin + pos_x, yorigin + pos_y, marker_length,
+ color)
+
+
def markers(cr):
SHOW_MARKERS = False
if SHOW_MARKERS:
@@ -111,9 +196,9 @@
def draw_init_lines(cr):
set_color(cr, palette["RED"])
- init_line_x = WIDTH_OF_FIELD_IN_METERS / 2.0 - inToM(120)
- init_start_y = -LENGTH_OF_FIELD_IN_METERS / 2.0
- init_end_y = LENGTH_OF_FIELD_IN_METERS / 2.0
+ init_line_x = FIELD.width / 2.0 - inToM(120)
+ init_start_y = -FIELD.length / 2.0
+ init_end_y = FIELD.length / 2.0
cr.move_to(mToPx(init_line_x), mToPx(init_start_y))
cr.line_to(mToPx(init_line_x), mToPx(init_end_y))
@@ -124,7 +209,7 @@
def draw_trench_run(cr):
- edge_of_field_y = LENGTH_OF_FIELD_IN_METERS / 2.0
+ edge_of_field_y = FIELD.length / 2.0
edge_of_trench_y = edge_of_field_y - inToM(55.5)
trench_start_x = inToM(-108.0)
trench_length_x = inToM(216.0)
@@ -133,7 +218,7 @@
ball_two_x = -inToM(36)
ball_three_x = 0.0
# The fourth/fifth balls are referenced off of the init line...
- ball_fourfive_x = WIDTH_OF_FIELD_IN_METERS / 2.0 - inToM(120.0 + 130.36)
+ ball_fourfive_x = FIELD.width / 2.0 - inToM(120.0 + 130.36)
for sign in [1.0, -1.0]:
set_color(cr, palette["GREEN"])
@@ -167,9 +252,9 @@
def draw_control_panel(cr): # Base plates are not included
set_color(cr, palette["LIGHT_GREY"])
- edge_of_field_y = LENGTH_OF_FIELD_IN_METERS / 2.0
+ edge_of_field_y = FIELD.length / 2.0
edge_of_trench_y = edge_of_field_y - inToM(55.5)
- high_x = inToM(374.59) - WIDTH_OF_FIELD_IN_METERS / 2.0
+ high_x = inToM(374.59) - FIELD.width / 2.0
low_x = high_x - inToM(30)
for sign in [1.0, -1.0]:
# Bottom Control Panel
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 9ba87ab..5f68444 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -36,6 +36,7 @@
end = -2.0 * AXIS_MARGIN_SPACE
height = 0.5 * (SCREEN_SIZE) - AXIS_MARGIN_SPACE
zero = AXIS_MARGIN_SPACE - SCREEN_SIZE / 2.0
+ legend_entries = {}
if mypoints.getLibsplines():
distanceSpline = DistanceSpline(mypoints.getLibsplines())
traj = Trajectory(distanceSpline)
@@ -44,14 +45,18 @@
XVA = traj.GetPlanXVA(dT)
if len(XVA[0]) > 0:
self.draw_x_axis(cr, start, height, zero, XVA, end)
- self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end,
+ legend_entries)
self.drawAcceleration(cr, XVA, start, height, skip, zero,
- AXIS_MARGIN_SPACE, end)
- self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ AXIS_MARGIN_SPACE, end, legend_entries)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end,
+ legend_entries)
cr.set_source_rgb(0, 0, 0)
cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE,
zero + height / 2.0)
+ self.drawLegend(cr, XVA, start, height, skip, zero, end,
+ legend_entries)
cr.stroke()
def connectLines(self, cr, points, color):
@@ -88,7 +93,19 @@
cr.line_to(x - (width / 2), y)
cr.stroke()
- def drawVelocity(self, cr, xva, start, height, skip, zero, end):
+ def drawLegend(self, cr, xva, start, height, skip, zero, end,
+ legend_entries):
+ step_size = (end - start) / len(legend_entries)
+ margin_under_x_axis = height * 0.1
+ for index, (name, color) in enumerate(legend_entries.items()):
+ set_color(cr, color)
+ cr.move_to(start + index * step_size, zero - margin_under_x_axis)
+ txt_scale = SCREEN_SIZE / 900.0
+ display_text(cr, name, txt_scale, txt_scale, 1.0 / txt_scale,
+ 1.0 / txt_scale)
+
+ def drawVelocity(self, cr, xva, start, height, skip, zero, end,
+ legend_entries):
COLOR = palette["RED"]
velocity = xva[1]
n_timesteps = len(velocity)
@@ -116,8 +133,11 @@
1.0 / txt_scale, 1.0 / txt_scale)
cr.stroke()
- def drawAcceleration(self, cr, xva, start, height, skip, zero, margin,
- end):
+ # add entry to legend
+ legend_entries["Velocity"] = COLOR
+
+ def drawAcceleration(self, cr, xva, start, height, skip, zero, margin, end,
+ legend_entries):
COLOR = palette["BLUE"]
accel = xva[2]
max_a = np.amax(accel)
@@ -146,27 +166,32 @@
1.0 / txt_scale, 1.0 / txt_scale)
cr.stroke()
- def drawVoltage(self, cr, xva, start, height, skip, traj, zero, end):
- COLOR1 = palette["GREEN"]
- COLOR2 = palette["CYAN"]
+ # draw legend
+ legend_entries["Acceleration"] = COLOR
+
+ def drawVoltage(self, cr, xva, start, height, skip, traj, zero, end,
+ legend_entries):
+ COLOR_LEFT = palette["GREEN"]
+ COLOR_RIGHT = palette["CYAN"]
poses = xva[0]
n_timesteps = len(poses)
spacing = np.abs(start - end) / float(n_timesteps)
- points1 = []
- points2 = []
+ points_left = []
+ points_right = []
for i in range(0, len(poses)):
if i % skip == 0:
+ # libspline says the order is left-right
voltage = traj.Voltage(poses[i])
- points1.append([
+ points_left.append([
start + (i * spacing),
zero + height / 2 + height * (voltage[0] / 24.0)
])
- points2.append([
+ points_right.append([
start + (i * spacing),
zero + height / 2 + height * (voltage[1] / 24.0)
])
- self.connectLines(cr, points1, COLOR1)
- self.connectLines(cr, points2, COLOR2)
+ self.connectLines(cr, points_left, COLOR_LEFT)
+ self.connectLines(cr, points_right, COLOR_RIGHT)
for i in np.linspace(-1, 1, 7):
self.HtickMark(cr, -1.0 * SCREEN_SIZE,
@@ -179,3 +204,6 @@
display_text(cr, str(round(i * 12.0, 2)), txt_scale, txt_scale,
1.0 / txt_scale, 1.0 / txt_scale)
cr.stroke()
+
+ legend_entries["Left Voltage"] = COLOR_LEFT
+ legend_entries["Right Voltage"] = COLOR_RIGHT
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index ce87aff..7620ff5 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -96,17 +96,19 @@
return self.all_controls[self.get_index_of_nearest_point()]
def draw_field_elements(self, cr):
- if FIELD == 2019:
+ if FIELD.year == 2019:
draw_HAB(cr)
draw_rockets(cr)
draw_cargo_ship(cr)
- elif FIELD == 2020:
+ elif FIELD.year == 2020:
set_color(cr, palette["BLACK"])
markers(cr)
draw_shield_generator(cr)
draw_trench_run(cr)
draw_init_lines(cr)
draw_control_panel(cr)
+ elif FIELD.year == 2021:
+ draw_at_home_grid(cr)
def draw_robot_at_point(self, cr, i, p, spline):
p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
@@ -222,18 +224,18 @@
cr.show_text('Press "i" to import')
cr.save()
- cr.translate(mToPx(WIDTH_OF_FIELD_IN_METERS) / 2.0, 0.0)
set_color(cr, palette["BLACK"])
- if FIELD == 2020:
- cr.rectangle(-mToPx(WIDTH_OF_FIELD_IN_METERS) / 2.0,
- -mToPx(LENGTH_OF_FIELD_IN_METERS) / 2.0,
- mToPx(WIDTH_OF_FIELD_IN_METERS),
- mToPx(LENGTH_OF_FIELD_IN_METERS))
- else:
+
+ if FIELD.year == 2019: # half field
cr.rectangle(0, -SCREEN_SIZE / 2, SCREEN_SIZE, SCREEN_SIZE)
+ else: # full field
+ cr.translate(mToPx(FIELD.width) / 2.0, 0.0)
+ cr.rectangle(-mToPx(FIELD.width) / 2.0, -mToPx(FIELD.length) / 2.0,
+ mToPx(FIELD.width), mToPx(FIELD.length))
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
self.draw_field_elements(cr)
+
y = 0
# update everything
@@ -319,14 +321,14 @@
def mouse_move(self, event):
old_x = self.x
old_y = self.y
- self.x = event.x - mToPx(WIDTH_OF_FIELD_IN_METERS / 2.0)
+ self.x = event.x - mToPx(FIELD.width / 2.0)
self.y = event.y
dif_x = self.x - old_x
dif_y = self.y - old_y
difs = np.array([pxToM(dif_x), pxToM(dif_y)])
if self.mode == Mode.kEditing:
- self.spline_edit = self.points.updates_for_mouse_move(
+ self.points.updates_for_mouse_move(
self.index_of_edit, self.spline_edit, self.x, self.y, difs)
def export_json(self, file_name):
@@ -406,8 +408,7 @@
self.points.setSplines(self.spline_edit, self.index_of_edit,
pxToM(self.x), pxToM(self.y))
- self.spline_edit = self.points.splineExtrapolate(
- self.spline_edit)
+ self.points.splineExtrapolate(self.spline_edit)
self.index_of_edit = -1
self.spline_edit = -1
@@ -436,6 +437,6 @@
def do_button_press(self, event):
# Be consistent with the scaling in the drawing_area
- self.x = event.x * 2 - mToPx(WIDTH_OF_FIELD_IN_METERS / 2.0)
+ self.x = event.x * 2 - mToPx(FIELD.width / 2.0)
self.y = event.y * 2
self.button_press_action()
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
index 4444a72..f42410f 100644
--- a/frc971/control_loops/python/points.py
+++ b/frc971/control_loops/python/points.py
@@ -48,24 +48,20 @@
def splineExtrapolate(self, o_spline_edit):
spline_edit = o_spline_edit
if not spline_edit == len(self.splines) - 1:
- spline_edit = spline_edit + 1
f = self.splines[spline_edit][5]
e = self.splines[spline_edit][4]
d = self.splines[spline_edit][3]
- self.splines[spline_edit][0] = f
- self.splines[spline_edit][1] = f * 2 + e * -1
- self.splines[spline_edit][2] = d + f * 4 + e * -4
+ self.splines[spline_edit + 1][0] = f
+ self.splines[spline_edit + 1][1] = f * 2 + e * -1
+ self.splines[spline_edit + 1][2] = d + f * 4 + e * -4
if not spline_edit == 0:
- spline_edit = spline_edit - 1
a = self.splines[spline_edit][0]
b = self.splines[spline_edit][1]
c = self.splines[spline_edit][2]
- self.splines[spline_edit][5] = a
- self.splines[spline_edit][4] = a * 2 + b * -1
- self.splines[spline_edit][3] = c + a * 4 + b * -4
-
- return spline_edit
+ self.splines[spline_edit - 1][5] = a
+ self.splines[spline_edit - 1][4] = a * 2 + b * -1
+ self.splines[spline_edit - 1][3] = c + a * 4 + b * -4
def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs):
if index_of_edit > -1:
@@ -97,7 +93,7 @@
index_of_edit +
1] = self.splines[spline_edit][index_of_edit + 1] + difs
- return self.splineExtrapolate(spline_edit)
+ self.splineExtrapolate(spline_edit)
def update_lib_spline(self):
self.libsplines = []
diff --git a/y2020/BUILD b/y2020/BUILD
index 63034ae..cd001a1 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -144,14 +144,6 @@
aos_config(
name = "config",
src = "y2020.json",
- flatbuffers = [
- "//aos/network:message_bridge_client_fbs",
- "//aos/network:message_bridge_server_fbs",
- "//aos/network:timestamp_fbs",
- "//y2020/vision/sift:sift_fbs",
- "//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision:vision_fbs",
- ],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
@@ -162,6 +154,14 @@
":config_pi4",
":config_roborio",
],
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//y2020/vision/sift:sift_fbs",
+ "//y2020/vision/sift:sift_training_fbs",
+ "//y2020/vision:vision_fbs",
+ ]
)
[
@@ -176,6 +176,7 @@
"//y2020/vision/sift:sift_training_fbs",
"//y2020/vision:vision_fbs",
"//aos/network:remote_message_fbs",
+ "//y2020/vision:galactic_search_path_fbs"
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index a5d47fa..187dedc 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -9,6 +9,14 @@
visibility = ["//y2020:__subpackages__"],
)
+flatbuffer_cc_library(
+ name = "galactic_search_path_fbs",
+ srcs = ["galactic_search_path.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2020:__subpackages__"],
+)
+
cc_library(
name = "v4l2_reader",
srcs = [
diff --git a/y2020/vision/ball_detection.py b/y2020/vision/ball_detection.py
new file mode 100644
index 0000000..46faccc
--- /dev/null
+++ b/y2020/vision/ball_detection.py
@@ -0,0 +1,31 @@
+#!/usr/bin/python3
+
+from rect import Rect
+
+import cv2 as cv
+import numpy as np
+
+# This function finds the percentage of yellow pixels in the rectangles
+# given that are regions of the given image. This allows us to determine
+# whether there is a ball in those rectangles
+def pct_yellow(img, rects):
+ hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
+ lower_yellow = np.array([23, 100, 75], dtype = np.uint8)
+ higher_yellow = np.array([40, 255, 255], dtype = np.uint8)
+ mask = cv.inRange(hsv, lower_yellow, higher_yellow)
+
+ pcts = np.zeros(len(rects))
+ for i in range(len(rects)):
+ rect = rects[i]
+ slice = mask[rect.y1 : rect.y2, rect.x1 : rect.x2]
+ yellow_px = np.count_nonzero(slice)
+ pcts[i] = 100 * (yellow_px / (slice.shape[0] * slice.shape[1]))
+
+ return pcts
+
+def capture_img():
+ video_stream = cv.VideoCapture(0)
+ frame = video_stream.read()[1]
+ video_stream.release()
+ frame = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
+ return frame
diff --git a/y2020/vision/galactic_search_path.fbs b/y2020/vision/galactic_search_path.fbs
new file mode 100644
index 0000000..d1e1223
--- /dev/null
+++ b/y2020/vision/galactic_search_path.fbs
@@ -0,0 +1,22 @@
+namespace y2020.vision;
+
+enum Alliance : byte {
+ kRed,
+ kBlue,
+ kUnknown
+}
+
+enum Letter : byte {
+ kA,
+ kB
+}
+
+table GalacticSearchPath {
+ // Alliance of the path
+ alliance:Alliance (id: 0);
+
+ // Letter of the path
+ letter:Letter (id: 1);
+}
+
+root_type GalacticSearchPath;
diff --git a/y2020/vision/galactic_search_path.py b/y2020/vision/galactic_search_path.py
new file mode 100644
index 0000000..e7f8402
--- /dev/null
+++ b/y2020/vision/galactic_search_path.py
@@ -0,0 +1,159 @@
+#!/usr/bin/python3
+
+from rect import Rect
+import ball_detection
+
+# Creates a UI for a user to select the regions in a camera image where the balls could be placed.
+# After the balls have been placed on the field and they submit the regions,
+# it will take another picture and based on the yellow regions in that picture it will determine where the
+# balls are. This tells us which path the current field is. It then sends the Alliance and Letter of the path
+# with aos_send to the /camera channel for the robot to excecute the spline for that path.
+
+from enum import Enum
+import glog
+import json
+import matplotlib.patches as patches
+import matplotlib.pyplot as plt
+from matplotlib.widgets import Button
+import numpy as np
+import os
+
+class Alliance(Enum):
+ kRed = "red"
+ kBlue = "blue"
+ kUnknown = None
+
+class Letter(Enum):
+ kA = "A"
+ kB = "B"
+
+
+NUM_RECTS = 4
+AOS_SEND_PATH = "bazel-bin/aos/aos_send"
+
+# The minimum percentage of yellow for a region of a image to
+# be considered to have a ball
+BALL_PCT_THRESHOLD = 50
+
+rects = [Rect(None, None, None, None)]
+
+# current index in rects list
+rect_index = 0
+
+fig, img_ax = plt.subplots()
+
+txt = img_ax.text(0, 0, "", size = 10, backgroundcolor = "white")
+
+confirm = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Confirm")
+cancel = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Cancel")
+submit = Button(plt.axes([0.4, 0.4, 0.1, 0.1]), "Submit")
+
+def draw_txt():
+ alliance = (Alliance.kRed if rect_index % 2 == 0 else Alliance.kBlue)
+ letter = (Letter.kA if rect_index < (NUM_RECTS / 2) else Letter.kB)
+ txt.set_text("Click on top left point and bottom right point for " +
+ alliance.value + ", path " + letter.value)
+ txt.set_color(alliance.value)
+
+
+def on_confirm(event):
+ global rect_index
+ if rects[rect_index].x1 != None and rects[rect_index].x2 != None:
+ confirm.ax.set_visible(False)
+ cancel.ax.set_visible(False)
+ rect_index += 1
+ clear_rect()
+ if rect_index == NUM_RECTS:
+ submit.ax.set_visible(True)
+ else:
+ draw_txt()
+ rects.append(Rect(None, None, None, None))
+ plt.show()
+
+def on_cancel(event):
+ global rect_index
+ if rect_index < NUM_RECTS:
+ confirm.ax.set_visible(False)
+ cancel.ax.set_visible(False)
+ clear_rect()
+ rects[rect_index].x1 = None
+ rects[rect_index].y1 = None
+ rects[rect_index].x2 = None
+ rects[rect_index].y2 = None
+ plt.show()
+
+def on_submit(event):
+ plt.close("all")
+ pcts = ball_detection.pct_yellow(ball_detection.capture_img(), rects)
+ if len(pcts) == len(rects):
+ paths = []
+ for i in range(len(pcts)):
+ alliance = (Alliance.kRed if i % 2 == 0 else Alliance.kBlue)
+ letter = (Letter.kA if i < NUM_RECTS / 2 else Letter.kB)
+ paths.append({"alliance" : alliance.name, "letter" : letter.name})
+ max_index = np.argmax(pcts)
+ path = paths[max_index]
+ # Make sure that exactly one percentage is >= the threshold
+ rects_with_balls = np.where(pcts >= BALL_PCT_THRESHOLD)[0].size
+ glog.info("rects_with_balls: %s" % rects_with_balls)
+ if rects_with_balls != 1:
+ path["alliance"] = Alliance.kUnknown.name
+ glog.warn("More than one ball found, path is unknown" if rects_with_balls > 1 else
+ "No balls found")
+ glog.info("Path is %s" % path)
+ os.system(AOS_SEND_PATH + " --config bazel-bin/y2020/config.json " +
+ "/pi1/camera y2020.vision.GalacticSearchPath '" + json.dumps(path) + "'")
+
+ for j in range(len(pcts)):
+ glog.info("%s: %s%% yellow" % (rects[j], pcts[j]))
+ else:
+ glog.error("Error: len of pcts (%u) != len of rects: (%u)" % (len(pcts), len(rects)))
+
+# Clears rect on screen
+def clear_rect():
+ if len(img_ax.patches) == 0:
+ glog.error("There were no patches found in img_ax")
+ else:
+ img_ax.patches[-1].remove()
+
+def on_click(event):
+ # This will get called when user clicks on Submit button, don't want to override the points on
+ # the last rect. Additionally, the event xdata or ydata will be None if the user clicks out of
+ # the bounds of the axis
+ if rect_index < NUM_RECTS and event.xdata != None and event.ydata != None:
+ if rects[rect_index].x1 == None:
+ rects[rect_index].x1, rects[rect_index].y1 = int(event.xdata), int(event.ydata)
+ elif rects[rect_index].x2 == None:
+ rects[rect_index].x2, rects[rect_index].y2 = int(event.xdata), int(event.ydata)
+ if rects[rect_index].x2 < rects[rect_index].x1:
+ rects[rect_index].x2 = rects[rect_index].x1 + (rects[rect_index].x1 - rects[rect_index].x2)
+ if rects[rect_index].y2 < rects[rect_index].y1:
+ rects[rect_index].y2 = rects[rect_index].y1 + (rects[rect_index].y1 - rects[rect_index].y2)
+
+ img_ax.add_patch(patches.Rectangle((rects[rect_index].x1, rects[rect_index].y1),
+ rects[rect_index].x2 - rects[rect_index].x1, rects[rect_index].y2 - rects[rect_index].y1,
+ edgecolor = 'r', linewidth = 1, facecolor="none"))
+ confirm.ax.set_visible(True)
+ cancel.ax.set_visible(True)
+ plt.show()
+ else:
+ glog.info("Either submitted or user pressed out of the bounds of the axis")
+
+def setup_button(button, on_clicked):
+ button.on_clicked(on_clicked)
+ button.ax.set_visible(False)
+
+def main():
+ glog.setLevel("INFO")
+
+ img_ax.imshow(ball_detection.capture_img())
+
+ fig.canvas.mpl_connect("button_press_event", on_click)
+ setup_button(confirm, on_confirm)
+ setup_button(cancel, on_cancel)
+ setup_button(submit, on_submit)
+ draw_txt()
+ plt.show()
+
+if __name__ == "__main__":
+ main()
diff --git a/y2020/vision/rect.py b/y2020/vision/rect.py
new file mode 100644
index 0000000..d57a005
--- /dev/null
+++ b/y2020/vision/rect.py
@@ -0,0 +1,13 @@
+#!/usr/bin/python3
+
+class Rect:
+
+ # x1 and y1 are top left corner, x2 and y2 are bottom right
+ def __init__(self, x1, y1, x2, y2):
+ self.x1 = x1
+ self.y1 = y1
+ self.x2 = x2
+ self.y2 = y2
+
+ def __str__(self):
+ return "({}, {}), ({}, {})".format(self.x1, self.y1, self.x2, self.y2)
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index db1de5e..d5c97d3 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -100,6 +100,18 @@
"source_node": "pi{{ NUM }}",
"frequency": 2,
"max_size": 2000000
+ },
+ {
+ "name": "/pi{{ NUM }}/camera",
+ "type": "y2020.vision.GalacticSearchPath",
+ "source_node": "pi{{ NUM }}",
+ "max_size" : 104,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "time_to_live": 100000000
+ }
+ ]
}
],
"applications": [