Merge "Refactor GetChannel in event_loop.h"
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index ce87aff..f198299 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -326,7 +326,7 @@
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 +406,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
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..b7f1c53 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -100,6 +100,12 @@
"source_node": "pi{{ NUM }}",
"frequency": 2,
"max_size": 2000000
+ },
+ {
+ "name": "/pi{{ NUM }}/camera",
+ "type": "y2020.vision.GalacticSearchPath",
+ "source_node": "pi{{ NUM }}",
+ "max_size" : 104
}
],
"applications": [