Merge "Track matching forwarded times for log file sorting"
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 1b83713..c974d68 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -2,6 +2,9 @@
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
import numpy as np
+import queue
+import threading
+import copy
from points import Points
from libspline import Spline, DistanceSpline, Trajectory
@@ -9,7 +12,6 @@
FigureCanvas)
from matplotlib.figure import Figure
-
class Graph(Gtk.Bin):
def __init__(self):
super(Graph, self).__init__()
@@ -19,10 +21,31 @@
canvas.set_vexpand(True)
canvas.set_size_request(800, 250)
self.add(canvas)
+ self.queue = queue.Queue(maxsize=1)
+
+ thread = threading.Thread(target=self.worker)
+ thread.daemon = True
+ thread.start()
+
+ def schedule_recalculate(self, points):
+ if not points.getLibsplines() or self.queue.full(): return
+ new_copy = copy.deepcopy(points)
+
+ # empty the queue
+ try:
+ self.queue.get_nowait()
+ except queue.Empty:
+ pass # was already empty
+
+ # replace with new request
+ self.queue.put_nowait(new_copy)
+
+ def worker(self):
+ while True:
+ self.recalculate_graph(self.queue.get())
def recalculate_graph(self, points):
if not points.getLibsplines(): return
-
# set the size of a timestep
dt = 0.00505
@@ -32,11 +55,12 @@
points.addConstraintsToTrajectory(traj)
traj.Plan()
XVA = traj.GetPlanXVA(dt)
+ if XVA is None: return
# extract values to be graphed
total_steps_taken = XVA.shape[1]
total_time = dt * total_steps_taken
- time = np.arange(total_time, step=dt)
+ time = np.linspace(0, total_time, num=total_steps_taken)
position, velocity, acceleration = XVA
left_voltage, right_voltage = zip(*(traj.Voltage(x) for x in position))
@@ -53,3 +77,6 @@
# renumber the x-axis to include the last point,
# the total time to drive the spline
self.axis.xaxis.set_ticks(np.linspace(0, total_time, num=8))
+
+ # ask to be redrawn
+ self.queue_draw()
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 3386bc6..50400b9 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -223,8 +223,6 @@
draw_px_cross(cr, self.mousex, self.mousey, 10)
cr.restore()
- if self.points.getLibsplines():
- self.graph.recalculate_graph(self.points)
print("spent {:.2f} ms drawing the field widget".format(1000 * (time.perf_counter() - start_time)))
@@ -249,18 +247,21 @@
def mouse_move(self, event):
old_x = self.mousex
old_y = self.mousey
- self.mousex = event.x
- self.mousey = event.y
+ self.mousex, self.mousey = event.x, event.y
dif_x = self.mousex - old_x
dif_y = self.mousey - old_y
difs = np.array([pxToM(dif_x), pxToM(dif_y)])
- if self.mode == Mode.kEditing:
+ if self.mode == Mode.kEditing and self.spline_edit != -1:
self.points.updates_for_mouse_move(self.index_of_edit,
self.spline_edit,
pxToM(self.mousex),
pxToM(self.mousey), difs)
+ self.points.update_lib_spline()
+ self.graph.schedule_recalculate(self.points)
+ self.queue_draw()
+
def export_json(self, file_name):
self.path_to_export = os.path.join(
self.module_path, # position of the python
@@ -310,7 +311,7 @@
print("SPLINES LOADED")
self.mode = Mode.kEditing
- def key_press(self, event, file_name):
+ def key_press(self, event):
keyval = Gdk.keyval_to_lower(event.keyval)
# TODO: This should be a button
@@ -325,10 +326,10 @@
self.points.getSplines()[len(self.points.getSplines()) - 1][5],
self.points.getSplines()[len(self.points.getSplines()) - 1][4],
self.points.getSplines()[len(self.points.getSplines()) - 1][3])
+ self.queue_draw()
def button_press(self, event):
- self.mousex = event.x
- self.mousey = event.y
+ self.mousex, self.mousey = event.x, event.y
if self.mode == Mode.kPlacing:
if self.points.add_point(
@@ -337,15 +338,7 @@
elif self.mode == Mode.kEditing:
# Now after index_of_edit is not -1, the point is selected, so
# user can click for new point
- if self.index_of_edit > -1 and self.held_x != self.mousex:
- self.points.setSplines(self.spline_edit, self.index_of_edit,
- pxToM(self.mousex), pxToM(self.mousey))
-
- self.points.splineExtrapolate(self.spline_edit)
-
- self.index_of_edit = -1
- self.spline_edit = -1
- else:
+ if self.index_of_edit == -1:
# Get clicked point
# Find nearest
# Move nearest to clicked
@@ -367,3 +360,21 @@
self.index_of_edit = index_of_closest
self.spline_edit = index_splines
self.held_x = self.mousex
+ self.queue_draw()
+
+ def button_release(self, event):
+ self.mousex, self.mousey = event.x, event.y
+ if self.mode == Mode.kEditing:
+ if self.index_of_edit > -1 and self.held_x != self.mousex:
+
+ self.points.setSplines(self.spline_edit, self.index_of_edit,
+ pxToM(self.mousex),
+ pxToM(self.mousey))
+
+ self.points.splineExtrapolate(self.spline_edit)
+
+ self.points.update_lib_spline()
+ self.graph.schedule_recalculate(self.points)
+
+ 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 d874306..5fc4c4a 100644
--- a/frc971/control_loops/python/points.py
+++ b/frc971/control_loops/python/points.py
@@ -1,7 +1,7 @@
from constants import *
import numpy as np
from libspline import Spline, DistanceSpline, Trajectory
-
+import copy
class Points():
def __init__(self):
@@ -21,6 +21,14 @@
}
]
+ def __deepcopy__(self, memo):
+ new_copy = Points()
+ new_copy.points = copy.deepcopy(self.points, memo)
+ new_copy.splines = copy.deepcopy(self.splines, memo)
+ new_copy.constraints = copy.deepcopy(self.constraints, memo)
+ new_copy.update_lib_spline()
+ return new_copy
+
def getPoints(self):
return self.points
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index 1378e8f..f56b0d3 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -20,17 +20,6 @@
self.connect(event, handler)
- def mouse_move(self, event):
- self.field.mouse_move(event)
- self.queue_draw()
-
- def button_press(self, event):
- self.field.button_press(event)
-
- def key_press(self, event):
- self.field.key_press(event, self.file_name_box.get_text())
- self.queue_draw()
-
def configure(self, event):
self.field.window_shape = (event.width, event.height)
@@ -77,6 +66,7 @@
self.eventBox = Gtk.EventBox()
self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+ | Gdk.EventMask.BUTTON_PRESS_MASK
| Gdk.EventMask.BUTTON_RELEASE_MASK
| Gdk.EventMask.POINTER_MOTION_MASK
| Gdk.EventMask.SCROLL_MASK
@@ -85,10 +75,11 @@
self.field = FieldWidget()
self.method_connect("delete-event", basic_window.quit_main_loop)
- self.method_connect("key-release-event", self.key_press)
- self.method_connect("button-release-event", self.button_press)
+ self.method_connect("key-release-event", self.field.key_press)
+ self.method_connect("button-press-event", self.field.button_press)
+ self.method_connect("button-release-event", self.field.button_release)
self.method_connect("configure-event", self.configure)
- self.method_connect("motion_notify_event", self.mouse_move)
+ self.method_connect("motion_notify_event", self.field.mouse_move)
self.file_name_box = Gtk.Entry()
self.file_name_box.set_size_request(200, 40)
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index a3e7d4f..7d12fd5 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -198,9 +198,8 @@
rotation_state_ = RotationState::ROTATING_LEFT;
rotation_count_ = kReverseTime;
break;
- } else {
- break;
}
+ [[fallthrough]];
case RotationState::STUCK: {
// Latch being stuck for 80 ms so we kick the box out far enough.
if (last_stuck_time_ + chrono::milliseconds(80) < monotonic_now) {
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index 6223d76..a867578 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -1,14 +1,77 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
load("//tools:platforms.bzl", "platforms")
+py_library(
+ name = "train_and_match",
+ srcs = ["train_and_match.py"],
+ data = [
+ "@python_repo//:scipy",
+ ],
+ deps = [
+ "//external:python-glog",
+ "@opencv_contrib_nonfree_amd64//:python_opencv",
+ ],
+)
+
+py_library(
+ name = "define_training_data",
+ srcs = [
+ "define_training_data.py",
+ ],
+ data = [
+ "@python_repo//:scipy",
+ ],
+ deps = [
+ ":train_and_match",
+ "//external:python-glog",
+ "@opencv_contrib_nonfree_amd64//:python_opencv",
+ ],
+)
+
+py_library(
+ name = "camera_definition",
+ srcs = [
+ "camera_definition.py",
+ ],
+ deps = [
+ ":define_training_data",
+ "//external:python-glog",
+ ],
+)
+
+py_library(
+ name = "target_definition",
+ srcs = [
+ "target_definition.py",
+ ],
+ deps = [
+ ":camera_definition",
+ ":define_training_data",
+ ":train_and_match",
+ "//external:python-glog",
+ "@opencv_contrib_nonfree_amd64//:python_opencv",
+ ],
+)
+
+py_binary(
+ name = "target_definition_main",
+ srcs = ["target_definition.py"],
+ data = glob(["calib_files/*.json"]) + glob([
+ "test_images/*.png",
+ ]),
+ main = "target_definition.py",
+ python_version = "PY3",
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":target_definition",
+ "@bazel_tools//tools/python/runfiles",
+ ],
+)
+
py_binary(
name = "load_sift_training",
srcs = [
- "camera_definition.py",
- "define_training_data.py",
"load_sift_training.py",
- "target_definition.py",
- "train_and_match.py",
],
args = [
"sift_training_data.h",
@@ -17,9 +80,30 @@
"test_images/*.png",
]),
python_version = "PY3",
- srcs_version = "PY2AND3",
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ ":camera_definition",
+ ":target_definition",
+ "//external:python-glog",
+ "//y2020/vision/sift:sift_fbs_python",
+ "@bazel_tools//tools/python/runfiles",
+ "@opencv_contrib_nonfree_amd64//:python_opencv",
+ ],
+)
+
+py_binary(
+ name = "image_match_test",
+ srcs = [
+ "image_match_test.py",
+ ],
+ data = glob(["calib_files/*.json"]) + glob([
+ "test_images/*.png",
+ ]),
+ python_version = "PY3",
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":camera_definition",
+ ":target_definition",
"//external:python-glog",
"//y2020/vision/sift:sift_fbs_python",
"@bazel_tools//tools/python/runfiles",
diff --git a/y2020/vision/tools/python_code/image_match_test.py b/y2020/vision/tools/python_code/image_match_test.py
index b085166..b6d3c1d 100644
--- a/y2020/vision/tools/python_code/image_match_test.py
+++ b/y2020/vision/tools/python_code/image_match_test.py
@@ -9,8 +9,6 @@
import camera_definition
### DEFINITIONS
-target_definition.USE_BAZEL = False
-camera_definition.USE_BAZEL = False
target_list = target_definition.compute_target_definition()
camera_list = camera_definition.load_camera_definitions()