Merge "Refactor game piece position code & latch in target selector"
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 3a61b5e..7bc45db 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -36,7 +36,7 @@
Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
Robot2021 = Robot2020
Robot2022 = RobotType(width=0.8763, length=0.96647)
-Robot2023 = RobotType(width=0.8763, length=0.96647)
+Robot2023 = RobotType(width=0.6061, length=0.77581)
FIELDS = {
"2019 Field":
@@ -137,6 +137,8 @@
return "y2020/actors/splines"
elif field.year == 2022:
return "y2022/actors/splines"
+ elif field.year == 2023:
+ return "y2023/autonomous/splines"
else:
return "frc971/control_loops/python/spline_jsons"
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index ac708f6..89ebe19 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -24,6 +24,8 @@
DEFINE_uint32(
min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
+DEFINE_uint32(min_id, 12, "Minimum valid charuco id");
+DEFINE_uint32(max_id, 15, "Minimum valid charuco id");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
DEFINE_bool(
draw_axes, false,
@@ -426,24 +428,47 @@
square_length_ / marker_length_,
diamond_corners, diamond_ids);
- // Check to see if we found any diamond targets
- if (diamond_ids.size() > 0) {
- cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
- diamond_ids);
+ // Check that we have exactly one charuco diamond. For calibration, we
+ // can constrain things so that this is the case
+ if (diamond_ids.size() == 1) {
+ // TODO<Jim>: Could probably make this check more general than requiring
+ // range of ids
+ bool all_valid_ids = true;
+ for (uint i = 0; i < 4; i++) {
+ uint id = diamond_ids[0][i];
+ if ((id < FLAGS_min_id) || (id > FLAGS_max_id)) {
+ all_valid_ids = false;
+ LOG(INFO) << "Got invalid charuco id: " << id;
+ }
+ }
+ if (all_valid_ids) {
+ cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
+ diamond_ids);
- // estimate pose for diamonds doesn't return valid, so marking true
- valid = true;
- std::vector<cv::Vec3d> rvecs, tvecs;
- cv::aruco::estimatePoseSingleMarkers(
- diamond_corners, square_length_, calibration_.CameraIntrinsics(),
- calibration_.CameraDistCoeffs(), rvecs, tvecs);
- DrawTargetPoses(rgb_image, rvecs, tvecs);
+ // estimate pose for diamonds doesn't return valid, so marking true
+ valid = true;
+ std::vector<cv::Vec3d> rvecs, tvecs;
+ cv::aruco::estimatePoseSingleMarkers(
+ diamond_corners, square_length_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs, tvecs);
+ DrawTargetPoses(rgb_image, rvecs, tvecs);
- PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
- result_ids = diamond_ids;
- result_corners = diamond_corners;
+ PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
+ result_ids = diamond_ids;
+ result_corners = diamond_corners;
+ } else {
+ LOG(INFO) << "Not all charuco ids were valid, so skipping";
+ }
} else {
- VLOG(2) << "Found aruco markers, but no charuco diamond targets";
+ if (diamond_ids.size() == 0) {
+ // OK to not see any markers sometimes
+ VLOG(2)
+ << "Found aruco markers, but no valid charuco diamond targets";
+ } else {
+ // But should never detect multiple
+ LOG(FATAL) << "Found multiple charuco diamond markers. Should only "
+ "be one";
+ }
}
} else {
LOG(FATAL) << "Unknown target type: "
diff --git a/y2023/autonomous/splines/README.md b/y2023/autonomous/splines/README.md
new file mode 100644
index 0000000..c655416
--- /dev/null
+++ b/y2023/autonomous/splines/README.md
@@ -0,0 +1,3 @@
+# Spline Descriptions
+This folder contains reference material for what each spline does
+
diff --git a/y2023/autonomous/splines/spline_1.json b/y2023/autonomous/splines/spline_1.json
new file mode 100644
index 0000000..e6e24ed
--- /dev/null
+++ b/y2023/autonomous/splines/spline_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450466090790975, -6.021160733648118, -5.591855376505261, -3.3652785421474576, -2.7749836760760287, -1.7732711760760287], "spline_y": [0.9493418961252269, 0.9493418961252269, 0.9314541729109411, 0.5975544198946889, 0.5975544198946889, 0.5796666966804032], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_2.json b/y2023/autonomous/splines/spline_2.json
new file mode 100644
index 0000000..032a081
--- /dev/null
+++ b/y2023/autonomous/splines/spline_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-1.7732711760760287, -2.7749836760760287, -3.3652785421474576, -5.591855376505261, -6.021160733648118, -6.450466090790975], "spline_y": [0.5796666966804032, 0.5975544198946889, 0.5975544198946889, 0.40105062588141127, 0.41893834909569705, 0.41893834909569705], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_3.json b/y2023/autonomous/splines/spline_3.json
new file mode 100644
index 0000000..4ca06a8
--- /dev/null
+++ b/y2023/autonomous/splines/spline_3.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450466090790975, -6.021160733648118, -5.591855376505261, -3.605574338541678, -3.0269522872367363, -1.6929070022836754], "spline_y": [0.41893834909569705, 0.41893834909569705, 0.40105062588141127, 0.5475210271634618, 0.515375357646521, -0.3364848845524211], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_4.json b/y2023/autonomous/splines/spline_4.json
new file mode 100644
index 0000000..a56d24e
--- /dev/null
+++ b/y2023/autonomous/splines/spline_4.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-1.6929070022836754, -3.0269522872367363, -3.605574338541678, -5.591855376505261, -6.021160733648118, -6.450466090790975], "spline_y": [-0.3364848845524211, 0.515375357646521, 0.5475210271634618, 0.40105062588141127, 0.41893834909569705, 0.41893834909569705], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_5.json b/y2023/autonomous/splines/spline_5.json
new file mode 100644
index 0000000..4eee822
--- /dev/null
+++ b/y2023/autonomous/splines/spline_5.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450466090790975, -6.448323209188465, -6.468936183333308, -5.63485982210851, -5.224861021501398, -4.383040925048516], "spline_y": [0.41893834909569705, -0.2089748700255587, -1.0435424455861884, -0.6390449134590346, -0.8779649804883709, -0.8766708249234052], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/test_spline.json b/y2023/autonomous/splines/test_spline.json
index 7672596..733d516 100644
--- a/y2023/autonomous/splines/test_spline.json
+++ b/y2023/autonomous/splines/test_spline.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [6.22420997455908, 6.1347950111487386, 6.080329974810555, 6.023577036950107, 5.9617203084135255, 5.81469341092744], "spline_y": [-2.63127733767268, -2.63127733767268, -2.656484781970896, -2.656484781970896, -2.6668098529078925, -2.6448802602350456], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 2}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 4}]}
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 287c5b0..e47128a 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -285,6 +285,10 @@
self.segment_selector = SegmentSelector(self.segments)
self.segment_selector.show()
+ self.show_indicators = True
+ # Lets you only view selected path
+ self.view_current = False
+
def _do_button_press_internal(self, event):
o_x = event.x
o_y = event.y
@@ -410,23 +414,26 @@
self.outline.draw_theta(cr)
set_color(cr, Color(0.0, 0.5, 1.0))
- for i in range(len(self.segments)):
- color = None
- if i == self.index:
- continue
- color = [0, random.random(), 1]
- random.shuffle(color)
- set_color(cr, Color(color[0], color[1], color[2]))
- self.segments[i].DrawTo(cr, self.theta_version)
+ if not self.view_current:
+ for i in range(len(self.segments)):
+ color = None
+ if i == self.index:
+ continue
+ color = [0, random.random(), 1]
+ random.shuffle(color)
+ set_color(cr, Color(color[0], color[1], color[2]))
+ self.segments[i].DrawTo(cr, self.theta_version)
- with px(cr):
- cr.stroke()
+ with px(cr):
+ cr.stroke()
# Draw current spline in black
color = [0, 0, 0]
set_color(cr, Color(color[0], color[1], color[2]))
self.segments[self.index].DrawTo(cr, self.theta_version)
+ with px(cr):
+ cr.stroke()
control1 = get_xy(self.segments[self.index].control1)
control2 = get_xy(self.segments[self.index].control2)
@@ -434,10 +441,15 @@
control1 = shift_angles(self.segments[self.index].control1)
control2 = shift_angles(self.segments[self.index].control2)
- cr.move_to(control1[0] + 0.02, control1[1])
- cr.arc(control1[0], control1[1], 0.02, 0, 2.0 * np.pi)
- cr.move_to(control2[0] + 0.02, control2[1])
- cr.arc(control2[0], control2[1], 0.02, 0, 2.0 * np.pi)
+ if self.show_indicators:
+ set_color(cr, Color(1.0, 0.0, 1.0))
+ cr.move_to(control1[0] + 0.02, control1[1])
+ cr.arc(control1[0], control1[1], 0.02, 0, 2.0 * np.pi)
+ with px(cr):
+ cr.stroke()
+ set_color(cr, Color(1.0, 0.7, 0.0))
+ cr.move_to(control2[0] + 0.02, control2[1])
+ cr.arc(control2[0], control2[1], 0.02, 0, 2.0 * np.pi)
with px(cr):
cr.stroke()
@@ -552,12 +564,18 @@
print("Switched to segment:", self.segments[self.index].name)
self.segments[self.index].Print(graph_paths.points)
+ elif keyval == Gdk.KEY_i:
+ self.show_indicators = not self.show_indicators
+
elif keyval == Gdk.KEY_n:
self.index += 1
self.index = self.index % len(self.segments)
print("Switched to segment:", self.segments[self.index].name)
self.segments[self.index].Print(graph_paths.points)
+ elif keyval == Gdk.KEY_l:
+ self.view_current = not self.view_current
+
elif keyval == Gdk.KEY_t:
# Toggle between theta and xy renderings
if self.theta_version: