Merge "Just show camera color for field debug webpage"
diff --git a/y2023/BUILD b/y2023/BUILD
index dc5e708..12c0119 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -42,6 +42,7 @@
"//aos/util:foxglove_websocket",
"//y2023/vision:viewer",
"//y2023/vision:aprilrobotics",
+ "//aos/events:aos_timing_report_streamer",
"//y2023/localizer:localizer_main",
"//y2023/constants:constants_sender",
"//y2023/vision:foxglove_image_converter",
@@ -185,6 +186,7 @@
"//aos/network:timestamp_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2023/control_loops/drivetrain:target_selector_hint_fbs",
"//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 3e4ec4b..a1c5c0e 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -83,9 +83,9 @@
case kCompTeamNumber:
arm_proximal->zeroing.measured_absolute_position = 0.149079699400425;
- arm_proximal->potentiometer_offset = 0.931355973012855 + 8.6743197253382 -
- 0.101200335326309 -
- 0.0820901660993467 - 0.0703733798337964;
+ arm_proximal->potentiometer_offset =
+ 0.931355973012855 + 8.6743197253382 - 0.101200335326309 -
+ 0.0820901660993467 - 0.0703733798337964;
arm_distal->zeroing.measured_absolute_position = 0.617279923658987;
arm_distal->potentiometer_offset =
@@ -93,15 +93,16 @@
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
0.0164398318919943 - 0.145833494945215;
- roll_joint->zeroing.measured_absolute_position = 0.926576490230748;
+ roll_joint->zeroing.measured_absolute_position = 1.12525305971909;
roll_joint->potentiometer_offset =
3.87038557084874 - 0.0241774522172967 + 0.0711345168020632 -
0.866186131631967 - 0.0256788357596952 + 0.18101759154572017 -
0.0208958996127179 - 0.186395903925026 + 0.45801689548395 -
- 0.5935210745062 + 0.166256655718334;
+ 0.5935210745062 + 0.166256655718334 - 0.12591438680483 +
+ 0.11972765117321;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.183283543884167;
+ 1.9245138240575;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index 6494953..9cb851b 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -136,14 +136,14 @@
.lower_hard = -0.10, // Back Hard
.upper_hard = 2.30, // Front Hard
.lower = 0.0, // Back Soft
- .upper = 2.0, // Front Soft
+ .upper = 2.2, // Front Soft
};
}
// Rollers
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 100.0; }
- static constexpr double kRollerVoltage() { return 6.0; }
+ static constexpr double kRollerVoltage() { return 12.0; }
// Game object is fed into end effector for at least this time
static constexpr std::chrono::milliseconds kExtraIntakingTime() {
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
index d9bb4a0..7cbb37e 100644
--- a/y2023/constants/7971.json
+++ b/y2023/constants/7971.json
@@ -13,5 +13,6 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-4_cam-23-04_ext_2023-02-19.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %}
+ "target_map": {% include 'y2023/vision/maps/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
}
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 9ac8c8b..7e979c7 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -13,5 +13,6 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %}
+ "target_map": {% include 'y2023/vision/maps/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
}
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index c7c467d..743db03 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -13,5 +13,6 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %}
+ "target_map": {% include 'y2023/vision/maps/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 0f57f05..95d04a8 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -31,6 +31,7 @@
"7971.json",
"971.json",
"9971.json",
+ ":scoring_map",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
],
@@ -38,6 +39,15 @@
visibility = ["//visibility:public"],
)
+filegroup(
+ name = "scoring_map",
+ srcs = [
+ "relative_scoring_map.json",
+ "scoring_map.json",
+ ],
+ visibility = ["//y2023:__subpackages__"],
+)
+
flatbuffer_cc_library(
name = "constants_fbs",
srcs = ["constants.fbs"],
@@ -46,6 +56,7 @@
deps = [
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
+ "//y2023/localizer:scoring_map_fbs",
],
)
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index 628ea21..e874275 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -1,5 +1,6 @@
include "frc971/vision/calibration.fbs";
include "frc971/vision/target_map.fbs";
+include "y2023/localizer/scoring_map.fbs";
namespace y2023;
@@ -10,6 +11,7 @@
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
+ scoring_map:localizer.ScoringMap (id: 2);
}
root_type Constants;
diff --git a/y2023/constants/relative_scoring_map.json b/y2023/constants/relative_scoring_map.json
new file mode 100644
index 0000000..b38ff46
--- /dev/null
+++ b/y2023/constants/relative_scoring_map.json
@@ -0,0 +1,80 @@
+{
+ "substation": {
+ /* TODO: Tune substation numbers once we pick a "good" spot. */
+ "left": {
+ "x": -1.0,
+ "y": -0.255,
+ "z": 0.16
+ },
+ "right": {
+ "x": 1.0,
+ "y": -0.255,
+ "z": 0.16
+ }
+ },
+ "nominal_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -0.559,
+ "y": 0.463,
+ "z": -0.254
+ },
+ "cube": {
+ "x": 0.0,
+ "y": 0.463,
+ "z": -0.254
+ },
+ "right_cone": {
+ "x": 0.559,
+ "y": 0.463,
+ "z": -0.254
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -0.559,
+ "y": -0.407,
+ "z": 0.217
+ },
+ "cube": {
+ "x": 0.0,
+ "y": -0.407,
+ "z": 0.217
+ },
+ "right_cone": {
+ "x": 0.559,
+ "y": -0.407,
+ "z": 0.217
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -0.559,
+ "y": -0.707,
+ "z": 0.647
+ },
+ "cube": {
+ "x": 0.0,
+ "y": -0.707,
+ "z": 0.647
+ },
+ "right_cone": {
+ "x": 0.559,
+ "y": -0.707,
+ "z": 0.647
+ }
+ }
+ },
+ "red": {
+ "substation": 5,
+ "left": 3,
+ "middle": 2,
+ "right": 1
+ },
+ "blue": {
+ "substation": 4,
+ "left": 6,
+ "middle": 7,
+ "right": 8
+ }
+}
diff --git a/y2023/constants/scoring_map.json b/y2023/constants/scoring_map.json
new file mode 100644
index 0000000..ebb9b38
--- /dev/null
+++ b/y2023/constants/scoring_map.json
@@ -0,0 +1,348 @@
+{
+ "red": {
+ "substation": {
+ "left": {
+ "x": -8.068,
+ "y": 1.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": -8.068,
+ "y": 3.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": 0.973,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -0.145,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": 0.973,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -0.145,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": 0.973,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -0.145,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -0.703,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -1.821,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -0.703,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -1.821,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -0.703,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -1.821,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -2.379,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -3.497,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -2.379,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -3.497,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -2.379,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -3.497,
+ "z": 1.17
+ }
+ }
+ }
+ },
+ "blue": {
+ "substation": {
+ "left": {
+ "x": 8.069,
+ "y": 3.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": 8.069,
+ "y": 1.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -0.145,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": 0.973,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -0.145,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": 0.973,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -0.145,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": 0.973,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -1.821,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -0.703,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -1.821,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -0.703,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -1.821,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -0.703,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -3.497,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -2.379,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -3.497,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -2.379,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -3.497,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -2.379,
+ "z": 1.17
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 09e5137..416d6d9 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -122,3 +122,12 @@
gen_reflections = 1,
visibility = ["//visibility:public"],
)
+
+flatbuffer_cc_library(
+ name = "target_selector_hint_fbs",
+ srcs = [
+ ":target_selector_hint.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/control_loops/drivetrain/target_selector_hint.fbs b/y2023/control_loops/drivetrain/target_selector_hint.fbs
new file mode 100644
index 0000000..0a16a59
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -0,0 +1,33 @@
+namespace y2023.control_loops.drivetrain;
+
+// Which of the grids we are going for.
+// From the perspective of the robot!
+enum GridSelectionHint : ubyte {
+ LEFT,
+ MIDDLE,
+ RIGHT,
+}
+
+// Which level to score on.
+enum RowSelectionHint : ubyte {
+ BOTTOM,
+ MIDDLE,
+ TOP,
+}
+
+// Within a row, which spot to score in.
+// From the perspective of the robot!
+enum SpotSelectionHint : ubyte {
+ LEFT,
+ MIDDLE,
+ RIGHT,
+}
+
+
+table TargetSelectorHint {
+ grid:GridSelectionHint (id: 0);
+ row:RowSelectionHint (id: 1);
+ spot:SpotSelectionHint (id: 2);
+}
+
+root_type TargetSelectorHint;
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 011095b..13fb990 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -379,7 +379,8 @@
# Draw current spline in black
color = [0, 0, 0]
else:
- color = [0.2, random.random(), random.random()]
+ 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):
@@ -559,4 +560,5 @@
arm_ui = ArmUi()
arm_ui.segments = graph_paths.segments
+print('Starting with segment: ', arm_ui.segments[arm_ui.index].name)
basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 87ced83..c017656 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -6,37 +6,169 @@
joint_center[1] + l2 - l1,
0.0,
circular_index=1)
-neutral_to_pickup_1 = np.array([2.396694, 0.508020])
-neutral_to_pickup_2 = np.array([2.874513, 0.933160])
-pickup_pos = to_theta_with_circular_index_and_roll(0.6,
- 0.4,
- np.pi / 2.0,
- circular_index=0)
-neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.95, np.pi / 2.0)]
+# NeutralToGroundPickupBackConeUp
+neutral_to_cone_up_1 = np.array([3.170156, -0.561227])
+neutral_to_cone_up_2 = np.array([2.972776, -1.026820])
+ground_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
+ -0.913162844198605, 0.35, np.pi / 2.0, circular_index=1)
+
+# NeutralToGroundPickupBackConeDown
+neutral_to_ground_pickup_back_cone_down_1 = np.array([3.170156, -0.561227])
+neutral_to_ground_pickup_back_cone_down_2 = np.array([2.972776, -1.026820])
+ground_pickup_back_cone_down = to_theta_with_circular_index_and_roll(
+ -0.95, 0.24, np.pi / 2.0, circular_index=1)
+
+# NeutralToBackMidConeUpScore
+neutral_to_score_back_mid_cone_up_1 = np.array([0.994244, -1.417442])
+neutral_to_score_back_mid_cone_up_2 = np.array([1.711325, -0.679748])
+score_back_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
+ -1.255555, 1.10, np.pi / 2.0, circular_index=0)
+
+# NeutralToMidConeDownScore
+neutral_to_score_mid_cone_down_1 = np.array([3.394572, -0.239378])
+neutral_to_score_mid_cone_down_2 = np.array([3.654854, -0.626835])
+score_mid_cone_down_pos = to_theta_with_circular_index_and_roll(
+ -1.23, 0.74, np.pi / 2.0, circular_index=1)
+
+# NeutralToMidConeDownScore
+neutral_to_hp_pickup_back_cone_up_1 = np.array([2.0, -0.239378])
+neutral_to_hp_pickup_back_cone_up_2 = np.array([1.6, -0.626835])
+neutral_to_hp_pickup_back_cone_up_alpha_rolls = [
+ (0.7, 0.0),
+ (.9, np.pi / 2.0),
+]
+hp_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
+ -0.94, 1.31, np.pi / 2.0, circular_index=0)
+
+# NeutralToFrontHighConeUpScore
+neutral_to_score_front_high_cone_up_1 = np.array([2.594244, 0.417442])
+neutral_to_score_front_high_cone_up_2 = np.array([1.51325, 0.679748])
+score_front_high_cone_up_pos = to_theta_with_circular_index_and_roll(
+ 0.87, 1.26, -np.pi / 2.0, circular_index=0)
+
+# NeutralToFrontMidConeUpScore
+neutral_to_score_front_mid_cone_up_1 = np.array([3.0, 0.317442])
+neutral_to_score_front_mid_cone_up_2 = np.array([2.9, 0.479748])
+score_front_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
+ 0.34, 0.93, -np.pi / 2.0, circular_index=0)
+
+neutral_to_cone_down_1 = np.array([2.396694, 0.508020])
+neutral_to_cone_down_2 = np.array([2.874513, 0.933160])
+cone_down_pos = to_theta_with_circular_index_and_roll(0.7,
+ 0.11,
+ np.pi / 2.0,
+ circular_index=0)
+
+neutral_to_cube_1 = np.array([2.396694, 0.508020])
+neutral_to_cube_2 = np.array([2.874513, 0.933160])
+
+cube_pos = to_theta_with_circular_index_and_roll(0.7,
+ 0.24,
+ np.pi / 2.0,
+ circular_index=0)
+
+neutral_to_pickup_control_alpha_rolls = [
+ (0.30, 0.0),
+ (.95, np.pi / 2.0),
+]
neutral_to_score_1 = np.array([0.994244, -1.417442])
neutral_to_score_2 = np.array([1.711325, -0.679748])
-score_pos = to_theta_with_circular_index_and_roll(-1.0,
- 1.2,
- np.pi / 2.0,
- circular_index=0)
-neutral_to_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
+score_low_pos = to_theta_with_circular_index_and_roll(-(0.41 / 2 + 0.49),
+ 0 + 0.05,
+ np.pi / 2.0,
+ circular_index=1)
-# TODO(Max): Add real paths for arm.
-points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
- (score_pos, "ScorePos")]
+neutral_to_score_low_2 = np.array([3.37926599, -0.73664663])
+
+score_mid_cube_pos = to_theta_with_circular_index_and_roll(-(0.58 + 0.49),
+ 0.6 + 0.05,
+ np.pi / 2.0,
+ circular_index=0)
+
+score_high_cone_pos = to_theta_with_circular_index_and_roll(-1.01,
+ 1.17 + 0.05,
+ np.pi / 2.0,
+ circular_index=0)
+
+score_high_cube_pos = to_theta_with_circular_index_and_roll(-1.01,
+ 0.90 + 0.05,
+ np.pi / 2.0,
+ circular_index=0)
+
+neutral_to_back_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
+neutral_to_front_score_control_alpha_rolls = [(0.40, 0.0), (.95, -np.pi / 2.0)]
+
+points = [(neutral, "NeutralPos"),
+ (ground_pickup_back_cone_up, "GroundPickupBackConeUp"),
+ (ground_pickup_back_cone_down, "GroundPickupBackConeDown"),
+ (hp_pickup_back_cone_up, "HPPickupBackConeUp"),
+ (cone_down_pos, "ConeDownPos"), (score_low_pos, "ScoreLowPos"),
+ (score_back_mid_cone_up_pos, "ScoreBackMidConeUpPos"),
+ (score_front_high_cone_up_pos, "ScoreFrontHighConeUpPos"),
+ (score_front_mid_cone_up_pos, "ScoreFrontMidConeUpPos"),
+ (score_mid_cone_down_pos, "ScoreBackMidConeDownPos"),
+ (score_mid_cube_pos, "ScoreMidCubePos"),
+ (score_high_cone_pos, "ScoreHighConePos"),
+ (score_high_cube_pos, "ScoreHighCubePos"), (cube_pos, "CubePos")]
front_points = []
back_points = []
unnamed_segments = []
named_segments = [
- ThetaSplineSegment("NeutralToPickup", neutral, neutral_to_pickup_1,
- neutral_to_pickup_2, pickup_pos,
+ ThetaSplineSegment("NeutralToGroundPickupBackConeUp", neutral,
+ neutral_to_cone_up_1, neutral_to_cone_up_2,
+ ground_pickup_back_cone_up,
neutral_to_pickup_control_alpha_rolls),
- ThetaSplineSegment("NeutralToScore", neutral, neutral_to_score_1,
- neutral_to_score_2, score_pos,
- neutral_to_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToGroundPickupBackConeDown", neutral,
+ neutral_to_ground_pickup_back_cone_down_1,
+ neutral_to_ground_pickup_back_cone_down_2,
+ ground_pickup_back_cone_down,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToHPPickupBackConeUp", neutral,
+ neutral_to_hp_pickup_back_cone_up_1,
+ neutral_to_hp_pickup_back_cone_up_2,
+ hp_pickup_back_cone_up,
+ neutral_to_hp_pickup_back_cone_up_alpha_rolls),
+ ThetaSplineSegment("NeutralToFrontHighConeUpScore", neutral,
+ neutral_to_score_front_high_cone_up_1,
+ neutral_to_score_front_high_cone_up_2,
+ score_front_high_cone_up_pos,
+ neutral_to_front_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToFrontMidConeUpScore", neutral,
+ neutral_to_score_front_mid_cone_up_1,
+ neutral_to_score_front_mid_cone_up_2,
+ score_front_mid_cone_up_pos,
+ neutral_to_front_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToConeDown", neutral, neutral_to_cone_down_1,
+ neutral_to_cone_down_2, cone_down_pos,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToCube", neutral, neutral_to_cube_1,
+ neutral_to_cube_2, cube_pos,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToLowScore", neutral, neutral_to_score_1,
+ neutral_to_score_low_2, score_low_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToBackMidConeUpScore", neutral,
+ neutral_to_score_back_mid_cone_up_1,
+ neutral_to_score_back_mid_cone_up_2,
+ score_back_mid_cone_up_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToMidConeDownScore", neutral,
+ neutral_to_score_mid_cone_down_1,
+ neutral_to_score_mid_cone_down_2,
+ score_mid_cone_down_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToMidCubeScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_mid_cube_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToHighConeScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_high_cone_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToHighCubeScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_high_cube_pos,
+ neutral_to_back_score_control_alpha_rolls),
]
segments = named_segments + unnamed_segments
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index 565f497..717a29e 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -155,7 +155,7 @@
distance_step_size +
static_cast<double>(before) * distance_step_size;
CHECK_GT(alpha, 0.0);
- CHECK_LT(alpha, 1.0);
+ CHECK_LE(alpha, 1.0);
return alpha;
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 2d43d99..e2b8a34 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -510,6 +510,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -530,6 +531,7 @@
goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -752,7 +754,7 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -778,7 +780,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -795,7 +797,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -806,7 +808,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ScorePosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreLowPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 112c209..58f7df1 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -41,6 +41,14 @@
const ButtonLocation kScore(4, 4);
const ButtonLocation kSpit(4, 13);
+const ButtonLocation kMidBackTipConeScoreLeft(4, 15);
+const ButtonLocation kHighBackTipConeScoreLeft(4, 14);
+const ButtonLocation kMidBackTipConeScoreRight(3, 2);
+
+const ButtonLocation kGroundPickupConeUp(4, 7);
+const ButtonLocation kGroundPickupConeDown(4, 8);
+const ButtonLocation kHPConePickup(4, 6);
+
const ButtonLocation kSuck(4, 12);
const ButtonLocation kWrist(4, 10);
@@ -48,6 +56,66 @@
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
+enum class GamePiece {
+ CONE_UP = 0,
+ CONE_DOWN = 1,
+ CUBE = 2,
+};
+
+struct ArmSetpoint {
+ uint32_t index;
+ double wrist_goal;
+ std::optional<double> score_wrist_goal = std::nullopt;
+ GamePiece game_piece;
+ ButtonLocation button;
+};
+
+const std::vector<ArmSetpoint> setpoints = {
+ {
+ .index = arm::GroundPickupBackConeUpIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kGroundPickupConeUp,
+ },
+ {
+ .index = arm::GroundPickupBackConeDownIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .button = kGroundPickupConeDown,
+ },
+ {
+ .index = arm::ScoreBackMidConeUpPosIndex(),
+ .wrist_goal = 0.55,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kMidBackTipConeScoreRight,
+ },
+ {
+ .index = arm::ScoreBackMidConeDownPosIndex(),
+ .wrist_goal = 2.2,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .button = kMidBackTipConeScoreRight,
+ },
+ {
+ .index = arm::HPPickupBackConeUpIndex(),
+ .wrist_goal = 0.2,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kHPConePickup,
+ },
+ {
+ .index = arm::ScoreFrontHighConeUpPosIndex(),
+ .wrist_goal = 0.05,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kHighBackTipConeScoreLeft,
+ },
+ {
+ .index = arm::ScoreFrontMidConeUpPosIndex(),
+ .wrist_goal = 0.05,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kMidBackTipConeScoreLeft,
+ },
+};
+
class Reader : public ::frc971::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
@@ -63,6 +131,8 @@
void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+ GamePiece current_game_piece_ = GamePiece::CONE_UP;
+
void HandleTeleop(
const ::frc971::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
@@ -71,39 +141,61 @@
return;
}
- RollerGoal roller_goal = RollerGoal::IDLE;
+ if (!superstructure_status_fetcher_->has_wrist()) {
+ AOS_LOG(ERROR, "Got no superstructure status message.\n");
+ return;
+ }
- // TODO(milind): add more actions and paths
- if (data.IsPressed(kIntake)) {
- arm_goal_position_ = arm::ScorePosIndex();
- } else if (data.IsPressed(kScore)) {
- arm_goal_position_ = arm::ScorePosIndex();
- } else {
- arm_goal_position_ = arm::NeutralPosIndex();
+ double wrist_goal = 0.0;
+ RollerGoal roller_goal = RollerGoal::IDLE;
+ arm_goal_position_ = arm::NeutralPosIndex();
+ std::optional<double> score_wrist_goal = std::nullopt;
+
+ if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
+ roller_goal = RollerGoal::INTAKE;
+ current_game_piece_ = GamePiece::CONE_UP;
+ } else if (data.IsPressed(kGroundPickupConeDown)) {
+ roller_goal = RollerGoal::INTAKE;
+ current_game_piece_ = GamePiece::CONE_DOWN;
+ }
+
+ // Search for the active setpoint.
+ for (const ArmSetpoint &setpoint : setpoints) {
+ if (data.IsPressed(setpoint.button)) {
+ if (setpoint.game_piece == current_game_piece_) {
+ wrist_goal = setpoint.wrist_goal;
+ arm_goal_position_ = setpoint.index;
+ score_wrist_goal = setpoint.score_wrist_goal;
+ break;
+ }
+ }
}
if (data.IsPressed(kSuck)) {
roller_goal = RollerGoal::INTAKE;
} else if (data.IsPressed(kSpit)) {
- roller_goal = RollerGoal::SPIT;
- }
+ if (score_wrist_goal.has_value()) {
+ wrist_goal = score_wrist_goal.value();
- double wrist_goal = 0.1;
-
- if (data.IsPressed(kWrist)) {
- wrist_goal = 1.5;
- } else {
- wrist_goal = 0.1;
+ // If we are supposed to dunk it, wait until we are close enough to
+ // spit.
+ if (std::abs(score_wrist_goal.value() -
+ superstructure_status_fetcher_->wrist()->position()) <
+ 0.1) {
+ roller_goal = RollerGoal::SPIT;
+ }
+ } else {
+ roller_goal = RollerGoal::SPIT;
+ }
}
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
- wrist_offset =
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), wrist_goal,
- CreateProfileParameters(*builder.fbb(), 12.0, 90.0));
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), wrist_goal,
+ CreateProfileParameters(*builder.fbb(), 12.0, 90.0));
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index 8f1237c..2345900 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -31,7 +31,6 @@
"visualization.fbs",
],
gen_reflections = True,
- target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
":status_fbs",
@@ -47,12 +46,87 @@
],
)
+flatbuffer_cc_library(
+ name = "scoring_map_fbs",
+ srcs = [
+ "scoring_map.fbs",
+ ],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:target_map_fbs",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "relative_scoring_map_fbs",
+ srcs = [
+ "relative_scoring_map.fbs",
+ ],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = [
+ ":scoring_map_fbs",
+ ],
+)
+
+cc_library(
+ name = "utils",
+ srcs = ["utils.cc"],
+ hdrs = ["utils.h"],
+ deps = [
+ "//frc971/vision:target_map_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "map_expander_lib",
+ srcs = ["map_expander_lib.cc"],
+ hdrs = ["map_expander_lib.h"],
+ deps = [
+ ":relative_scoring_map_fbs",
+ ":scoring_map_fbs",
+ ":utils",
+ "//aos:flatbuffers",
+ "//aos:json_to_flatbuffer",
+ ],
+)
+
+cc_binary(
+ name = "map_expander",
+ srcs = ["map_expander.cc"],
+ data = [
+ "//y2023/constants:scoring_map",
+ "//y2023/vision/maps",
+ ],
+ deps = [
+ ":map_expander_lib",
+ "//aos:init",
+ ],
+)
+
+cc_test(
+ name = "map_expander_lib_test",
+ srcs = ["map_expander_lib_test.cc"],
+ data = [
+ "//y2023/constants:scoring_map",
+ "//y2023/vision/maps",
+ ],
+ deps = [
+ ":map_expander_lib",
+ "//aos/testing:flatbuffer_eq",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_library(
name = "localizer",
srcs = ["localizer.cc"],
hdrs = ["localizer.h"],
deps = [
":status_fbs",
+ ":utils",
":visualization_fbs",
"//aos/containers:sized_array",
"//aos/events:event_loop",
@@ -77,6 +151,7 @@
deps = [
":localizer",
":status_fbs",
+ ":utils",
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_writer",
"//aos/testing:googletest",
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 1d78528..4378f61 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -4,6 +4,7 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/pose.h"
#include "y2023/constants.h"
+#include "y2023/localizer/utils.h"
namespace y2023::localizer {
namespace {
@@ -36,16 +37,6 @@
}
} // namespace
-Localizer::Transform PoseToTransform(
- const frc971::vision::TargetPoseFbs *pose) {
- const frc971::vision::Position *position = pose->position();
- const frc971::vision::Quaternion *quaternion = pose->orientation();
- return (Eigen::Translation3d(
- Eigen::Vector3d(position->x(), position->y(), position->z())) *
- Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
- quaternion->z()))
- .matrix();
-}
std::array<Localizer::CameraState, Localizer::kNumCameras>
Localizer::MakeCameras(const Constants &constants, aos::EventLoop *event_loop) {
@@ -317,6 +308,7 @@
++total_accepted_targets_;
++cameras_.at(camera_index).total_accepted_targets;
VLOG(1) << "new state " << ekf_.X_hat().topRows<3>().transpose();
+ builder.add_accepted(true);
return builder.Finish();
}
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index 8c53467..ba5b1cf 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -116,8 +116,5 @@
// For the status message.
std::optional<Eigen::Vector2d> last_encoder_readings_;
};
-
-// Converts a TargetPoseFbs into a transformation matrix.
-Localizer::Transform PoseToTransform(const frc971::vision::TargetPoseFbs *pose);
} // namespace y2023::localizer
#endif // Y2023_LOCALIZER_LOCALIZER_H_
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 77af69b..554ab5d 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -10,6 +10,7 @@
#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
#include "y2023/localizer/status_generated.h"
+#include "y2023/localizer/utils.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
diff --git a/y2023/localizer/map_expander.cc b/y2023/localizer/map_expander.cc
new file mode 100644
index 0000000..e13c0a1
--- /dev/null
+++ b/y2023/localizer/map_expander.cc
@@ -0,0 +1,23 @@
+#include "aos/init.h"
+#include "aos/util/file.h"
+#include "gflags/gflags.h"
+#include "y2023/localizer/map_expander_lib.h"
+
+DEFINE_string(target_map, "y2023/vision/maps/target_map.json",
+ "Path to the target map JSON file.");
+DEFINE_string(relative_map, "y2023/constants/relative_scoring_map.json",
+ "Path to the relative scoring map JSON file.");
+DEFINE_string(output, "y2023/constants/scoring_map.json",
+ "Path to the output scoring map JSON file.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<y2023::localizer::ScoringMap> map =
+ y2023::localizer::ExpandMap(
+ aos::util::ReadFileToStringOrDie(FLAGS_relative_map),
+ aos::util::ReadFileToStringOrDie(FLAGS_target_map));
+ aos::util::WriteStringToFileOrDie(
+ FLAGS_output,
+ aos::FlatbufferToJson(map, {.multi_line = true, .max_multi_line = true}));
+ return EXIT_SUCCESS;
+}
diff --git a/y2023/localizer/map_expander_lib.cc b/y2023/localizer/map_expander_lib.cc
new file mode 100644
index 0000000..5a94985
--- /dev/null
+++ b/y2023/localizer/map_expander_lib.cc
@@ -0,0 +1,124 @@
+#include "y2023/localizer/map_expander_lib.h"
+
+#include "y2023/localizer/utils.h"
+
+namespace y2023::localizer {
+namespace {
+flatbuffers::Offset<frc971::vision::Position> AbsolutePositionForTagAndRelative(
+ const Eigen::Affine3d &tag, const frc971::vision::Position *relative,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const Eigen::Vector3d absolute =
+ tag * Eigen::Vector3d(relative->x(), relative->y(), relative->z());
+ return frc971::vision::CreatePosition(*fbb, absolute.x(), absolute.y(),
+ absolute.z());
+}
+
+flatbuffers::Offset<ScoringRow> AbsoluteRowForTagAndRelative(
+ const Eigen::Affine3d &tag, const ScoringRow *relative_row,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto left_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_row->left_cone(), fbb);
+ auto cube_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_row->cube(), fbb);
+ auto right_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_row->right_cone(), fbb);
+ return CreateScoringRow(*fbb, left_offset, cube_offset, right_offset);
+}
+
+flatbuffers::Offset<ScoringGrid> AbsoluteGridForTagAndRelative(
+ const Eigen::Affine3d &tag, const ScoringGrid *relative_grid,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto bottom_offset =
+ AbsoluteRowForTagAndRelative(tag, relative_grid->bottom(), fbb);
+ auto middle_offset =
+ AbsoluteRowForTagAndRelative(tag, relative_grid->middle(), fbb);
+ auto top_offset =
+ AbsoluteRowForTagAndRelative(tag, relative_grid->top(), fbb);
+ return CreateScoringGrid(*fbb, bottom_offset, middle_offset, top_offset);
+}
+
+flatbuffers::Offset<ScoringGrid> RelativeGridForTagAndAbsolute(
+ const Eigen::Affine3d &tag, const ScoringGrid *absolute_grid,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ return AbsoluteGridForTagAndRelative(tag.inverse(), absolute_grid, fbb);
+}
+
+flatbuffers::Offset<DoubleSubstation> AbsoluteSubstationForTagAndRelative(
+ const Eigen::Affine3d &tag, const DoubleSubstation *relative_station,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto left_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_station->left(), fbb);
+ auto right_offset =
+ AbsolutePositionForTagAndRelative(tag, relative_station->right(), fbb);
+ return CreateDoubleSubstation(*fbb, left_offset, right_offset);
+}
+
+flatbuffers::Offset<DoubleSubstation> RelativeSubstationForTagAndAbsolute(
+ const Eigen::Affine3d &tag, const DoubleSubstation *absolute_station,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ return AbsoluteSubstationForTagAndRelative(tag.inverse(), absolute_station,
+ fbb);
+}
+
+flatbuffers::Offset<HalfField> AbsoluteHalfForTagsAndRelative(
+ const std::map<uint64_t, Eigen::Affine3d> &april_tags,
+ const RelativeHalfField *relative_half, const RelativeScoringMap *map,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ auto substation_offset = AbsoluteSubstationForTagAndRelative(
+ april_tags.at(relative_half->substation()), map->substation(), fbb);
+ auto left_offset = AbsoluteGridForTagAndRelative(
+ april_tags.at(relative_half->left()), map->nominal_grid(), fbb);
+ auto middle_offset = AbsoluteGridForTagAndRelative(
+ april_tags.at(relative_half->middle()), map->nominal_grid(), fbb);
+ auto right_offset = AbsoluteGridForTagAndRelative(
+ april_tags.at(relative_half->right()), map->nominal_grid(), fbb);
+ return CreateHalfField(*fbb, substation_offset, left_offset, middle_offset,
+ right_offset);
+}
+} // namespace
+
+std::map<uint64_t, Eigen::Affine3d> AprilTagPoses(
+ const frc971::vision::TargetMap *map) {
+ std::map<uint64_t, Eigen::Affine3d> april_tags;
+ for (const frc971::vision::TargetPoseFbs *target : *map->target_poses()) {
+ const uint64_t id = target->id();
+ CHECK(april_tags.count(id) == 0);
+ april_tags[id] = PoseToTransform(target);
+ }
+ return april_tags;
+}
+
+aos::FlatbufferDetachedBuffer<ScoringMap> ExpandMap(
+ const RelativeScoringMap *relative_map,
+ const frc971::vision::TargetMap *target_map) {
+ std::map<uint64_t, Eigen::Affine3d> april_tags = AprilTagPoses(target_map);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateScoringMap(
+ fbb,
+ AbsoluteHalfForTagsAndRelative(april_tags, relative_map->red(),
+ relative_map, &fbb),
+ AbsoluteHalfForTagsAndRelative(april_tags, relative_map->blue(),
+ relative_map, &fbb)));
+ return fbb.Release();
+}
+
+aos::FlatbufferDetachedBuffer<ScoringGrid> RelativeGridForTag(
+ const ScoringGrid *absolute_grid,
+ const frc971::vision::TargetMap *target_map, uint64_t tag) {
+ const Eigen::Affine3d tag_pose = AprilTagPoses(target_map).at(tag);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(RelativeGridForTagAndAbsolute(tag_pose, absolute_grid, &fbb));
+ return fbb.Release();
+}
+
+aos::FlatbufferDetachedBuffer<DoubleSubstation> RelativeSubstationForTag(
+ const DoubleSubstation *absolute_substation,
+ const frc971::vision::TargetMap *target_map, uint64_t tag) {
+ const Eigen::Affine3d tag_pose = AprilTagPoses(target_map).at(tag);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(
+ RelativeSubstationForTagAndAbsolute(tag_pose, absolute_substation, &fbb));
+ return fbb.Release();
+}
+
+} // namespace y2023::localizer
diff --git a/y2023/localizer/map_expander_lib.h b/y2023/localizer/map_expander_lib.h
new file mode 100644
index 0000000..773a769
--- /dev/null
+++ b/y2023/localizer/map_expander_lib.h
@@ -0,0 +1,49 @@
+#ifndef Y2023_LOCALIZER_MAP_EXPANDER_LIB_H_
+#define Y2023_LOCALIZER_MAP_EXPANDER_LIB_H_
+
+#include <Eigen/Dense>
+#include <map>
+
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/vision/target_map_generated.h"
+#include "y2023/localizer/relative_scoring_map_generated.h"
+#include "y2023/localizer/scoring_map_generated.h"
+
+namespace y2023::localizer {
+
+// This function takes a RelativeScoringMap plus a TargetMap with april tag
+// locations and uses it to generate a map of the various scoring locations on
+// the field.
+// This allows us to use a condensed JSON file (the RelativeScoringMap) to
+// generate the more verbose ScoringMap, making it so that humans don't have to
+// modify every single location in the verbose ScoringMap should something
+// change. However, we do still want to have the full ScoringMap available
+// for modifications should we discover that, e.g., an individual field has
+// one individual pole or april tag or something out of aligment.
+aos::FlatbufferDetachedBuffer<ScoringMap> ExpandMap(
+ const RelativeScoringMap *relative_map,
+ const frc971::vision::TargetMap *target_map);
+
+inline aos::FlatbufferDetachedBuffer<ScoringMap> ExpandMap(
+ std::string_view relative_map_json, std::string_view target_map_json) {
+ aos::FlatbufferDetachedBuffer<RelativeScoringMap> relative =
+ aos::JsonToFlatbuffer<RelativeScoringMap>(relative_map_json);
+ aos::FlatbufferDetachedBuffer<frc971::vision::TargetMap> target_map =
+ aos::JsonToFlatbuffer<frc971::vision::TargetMap>(target_map_json);
+ return ExpandMap(&relative.message(), &target_map.message());
+}
+
+aos::FlatbufferDetachedBuffer<ScoringGrid> RelativeGridForTag(
+ const ScoringGrid *absolute_grid,
+ const frc971::vision::TargetMap *target_map, uint64_t tag);
+
+aos::FlatbufferDetachedBuffer<DoubleSubstation> RelativeSubstationForTag(
+ const DoubleSubstation *absolute_substation,
+ const frc971::vision::TargetMap *target_map, uint64_t tag);
+
+std::map<uint64_t, Eigen::Affine3d> AprilTagPoses(
+ const frc971::vision::TargetMap *map);
+
+} // namespace y2023::localizer
+#endif // Y2023_LOCALIZER_MAP_EXPANDER_LIB_H_
diff --git a/y2023/localizer/map_expander_lib_test.cc b/y2023/localizer/map_expander_lib_test.cc
new file mode 100644
index 0000000..5b353a1
--- /dev/null
+++ b/y2023/localizer/map_expander_lib_test.cc
@@ -0,0 +1,54 @@
+#include "y2023/localizer/map_expander_lib.h"
+
+#include "gtest/gtest.h"
+#include "aos/testing/flatbuffer_eq.h"
+
+namespace y2023::localizer::testing {
+class MapExpanderTest : public ::testing::Test {
+ protected:
+ MapExpanderTest()
+ : relative_map_(aos::JsonFileToFlatbuffer<RelativeScoringMap>(
+ "y2023/constants/relative_scoring_map.json")),
+ target_map_(aos::JsonFileToFlatbuffer<frc971::vision::TargetMap>(
+ "y2023/vision/maps/target_map.json")),
+ absolute_map_(
+ ExpandMap(&relative_map_.message(), &target_map_.message())) {}
+ const aos::FlatbufferDetachedBuffer<RelativeScoringMap> relative_map_;
+ const aos::FlatbufferDetachedBuffer<frc971::vision::TargetMap> target_map_;
+ const aos::FlatbufferDetachedBuffer<ScoringMap> absolute_map_;
+};
+TEST_F(MapExpanderTest, BackAndForthConsistent) {
+ // Use FlatbufferToJson instead of FlatbufferEq because we don't want
+ // equivalent but different encoded floating point numbers to get
+ // evaluated differently.
+#define CHECK_REVERSE(color, grid) \
+ { \
+ ASSERT_TRUE(absolute_map_.message().has_##color()); \
+ auto half = absolute_map_.message().color(); \
+ ASSERT_TRUE(half->has_##grid##_grid()); \
+ auto grid = half->grid##_grid(); \
+ auto relative_grid = \
+ RelativeGridForTag(grid, &target_map_.message(), \
+ relative_map_.message().color()->grid()); \
+ EXPECT_EQ(aos::FlatbufferToJson(relative_map_.message().nominal_grid()), \
+ aos::FlatbufferToJson(&relative_grid.message())); \
+ }
+ CHECK_REVERSE(blue, left);
+ CHECK_REVERSE(blue, middle);
+ CHECK_REVERSE(blue, right);
+ CHECK_REVERSE(red, left);
+ CHECK_REVERSE(red, middle);
+ CHECK_REVERSE(red, right);
+}
+
+// Test that the currently checked-in map is consistent with the results of
+// ExpandMap.
+TEST_F(MapExpanderTest, ExpandMap) {
+ const std::string stored_map =
+ aos::util::ReadFileToStringOrDie("y2023/constants/scoring_map.json");
+ // TODO: Provide coherent error messages so that changes can be accommodated.
+ EXPECT_EQ(aos::FlatbufferToJson(absolute_map_,
+ {.multi_line = true, .max_multi_line = true}),
+ stored_map);
+}
+} // namespace y2023::localizer::testing
diff --git a/y2023/localizer/relative_scoring_map.fbs b/y2023/localizer/relative_scoring_map.fbs
new file mode 100644
index 0000000..9d1c979
--- /dev/null
+++ b/y2023/localizer/relative_scoring_map.fbs
@@ -0,0 +1,38 @@
+include "y2023/localizer/scoring_map.fbs";
+
+namespace y2023.localizer;
+
+// This file provides a schema for a more stripped-down version of the
+// ScoringMap. In this version, we try to deduplicate as much information
+// as is reasonable so that it is easier for humans to modify and verify the
+// numbers.
+// In particular, we:
+// * Assume that each batch of 9 scoring grid is identical with respect to
+// the april tag for that scoring grid.
+// * Provide all coordinates relative to the april tags in question.
+//
+// Note that when we say "relative to the april tags," that does mean in the
+// april tag frame. As such, the Z of the relative position will be how far
+// behind the april tag the scoring position is (i.e., the X offset), the
+// relative X will be to the right of the target, and the Y will be how far
+// below the center of the target the scoring position is.
+
+// The april tag ids for each field component on one half of the field.
+table RelativeHalfField {
+ substation:uint64 (id: 0);
+ left:uint64 (id: 1);
+ middle:uint64 (id: 2);
+ right:uint64 (id: 3);
+}
+
+table RelativeScoringMap {
+ // Substation numbers are relative to the april tag.
+ substation:DoubleSubstation (id: 0);
+ // The nominal grid will provide positions that are all relative to the
+ // coordinates of the april tag for that grid.
+ nominal_grid:ScoringGrid (id: 1);
+ red:RelativeHalfField (id: 2);
+ blue:RelativeHalfField (id: 3);
+}
+
+root_type RelativeScoringMap;
diff --git a/y2023/localizer/scoring_map.fbs b/y2023/localizer/scoring_map.fbs
new file mode 100644
index 0000000..92b48ca
--- /dev/null
+++ b/y2023/localizer/scoring_map.fbs
@@ -0,0 +1,44 @@
+include "frc971/vision/target_map.fbs";
+
+namespace y2023.localizer;
+
+// "left" and "right" in this file are taken from the perspective of a robot
+// on the field.
+
+// A row of three scoring positions, where the cube scoring locations will be
+// in the middle.
+table ScoringRow {
+ left_cone:frc971.vision.Position (id: 0);
+ cube:frc971.vision.Position (id: 1);
+ right_cone:frc971.vision.Position (id: 2);
+}
+
+// A batch of 9 scoring locations, anchored by a single april tag target.
+table ScoringGrid {
+ // Either game piece type can be scored in all the locations on the bottom
+ // row.
+ bottom:ScoringRow (id: 0);
+ middle:ScoringRow (id: 1);
+ top:ScoringRow (id: 2);
+}
+
+// Location where we can retrieve game pieces from the human player.
+table DoubleSubstation {
+ left:frc971.vision.Position (id: 0);
+ right:frc971.vision.Position (id: 1);
+}
+
+table HalfField {
+ // The substation will be across the field from the grids.
+ substation:DoubleSubstation (id: 0);
+ left_grid:ScoringGrid (id: 1);
+ middle_grid:ScoringGrid (id: 2);
+ right_grid:ScoringGrid (id: 3);
+}
+
+table ScoringMap {
+ red:HalfField (id: 0);
+ blue:HalfField (id: 1);
+}
+
+root_type ScoringMap;
diff --git a/y2023/localizer/utils.cc b/y2023/localizer/utils.cc
new file mode 100644
index 0000000..7faca1f
--- /dev/null
+++ b/y2023/localizer/utils.cc
@@ -0,0 +1,14 @@
+#include "y2023/localizer/utils.h"
+
+namespace y2023::localizer {
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose) {
+ const frc971::vision::Position *position = pose->position();
+ const frc971::vision::Quaternion *quaternion = pose->orientation();
+ return (Eigen::Translation3d(
+ Eigen::Vector3d(position->x(), position->y(), position->z())) *
+ Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
+ quaternion->z()))
+ .matrix();
+}
+} // namespace y2023::localizer
diff --git a/y2023/localizer/utils.h b/y2023/localizer/utils.h
new file mode 100644
index 0000000..8241cf8
--- /dev/null
+++ b/y2023/localizer/utils.h
@@ -0,0 +1,14 @@
+#ifndef Y2023_LOCALIZER_UTILS_H_
+#define Y2023_LOCALIZER_UTILS_H_
+
+#include <Eigen/Dense>
+
+#include "frc971/vision/target_map_generated.h"
+
+namespace y2023::localizer {
+// Converts a TargetPoseFbs into a transformation matrix.
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose);
+} // namespace y2023::localizer
+
+#endif // Y2023_LOCALIZER_UTILS_H_
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 4c07034..c2e063a 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -8,6 +8,8 @@
DEFINE_double(min_decision_margin, 75.0,
"Minimum decision margin (confidence) for an apriltag detection");
+DEFINE_int32(pixel_border, 3,
+ "Size of image border within which to reject detected corners");
namespace y2023 {
namespace vision {
@@ -18,13 +20,12 @@
std::string_view channel_name)
: calibration_data_(event_loop),
ftrace_(),
- image_callback_(
- event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(5)),
+ image_callback_(event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
@@ -103,7 +104,9 @@
const auto position_offset =
frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
- const auto orientation = Eigen::Quaterniond(Eigen::Matrix3d(pose.R->data));
+ // Aprilrobotics stores the rotation matrix in row-major order
+ const auto orientation = Eigen::Quaterniond(
+ Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(pose.R->data));
const auto orientation_offset = frc971::vision::CreateQuaternion(
*fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());
@@ -145,6 +148,8 @@
.stride = image.cols,
.buf = image.data,
};
+ const uint32_t min_x = FLAGS_pixel_border;
+ const uint32_t max_x = image.cols - FLAGS_pixel_border;
ftrace_.FormatMessage("Starting detect\n");
zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
@@ -152,6 +157,7 @@
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+ std::vector<std::vector<cv::Point2f>> orig_corners_vector;
std::vector<std::vector<cv::Point2f>> corners_vector;
auto builder = image_annotations_sender_.MakeBuilder();
@@ -161,6 +167,13 @@
zarray_get(detections, i, &det);
if (det->decision_margin > FLAGS_min_decision_margin) {
+ if (det->p[0][0] < min_x || det->p[0][0] > max_x ||
+ det->p[1][0] < min_x || det->p[1][0] > max_x ||
+ det->p[2][0] < min_x || det->p[2][0] > max_x ||
+ det->p[3][0] < min_x || det->p[3][0] > max_x) {
+ VLOG(1) << "Rejecting detection because corner is outside pixel border";
+ continue;
+ }
VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
<< " margin: " << det->decision_margin;
@@ -177,10 +190,20 @@
info.cx = intrinsics_.at<double>(0, 2);
info.cy = intrinsics_.at<double>(1, 2);
+ // Store out the original, pre-undistortion corner points for sending
+ std::vector<cv::Point2f> orig_corner_points;
+ orig_corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+ orig_corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+ orig_corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+ orig_corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+ orig_corners_vector.emplace_back(orig_corner_points);
+
+ UndistortDetection(det);
+
apriltag_pose_t pose;
double err = estimate_tag_pose(&info, &pose);
- UndistortDetection(det);
VLOG(1) << "err: " << err;
results.emplace_back(*det, pose);
@@ -204,8 +227,8 @@
}
}
- const auto annotations_offset =
- frc971::vision::BuildAnnotations(eof, corners_vector, 5.0, builder.fbb());
+ const auto annotations_offset = frc971::vision::BuildAnnotations(
+ eof, orig_corners_vector, 5.0, builder.fbb());
builder.CheckOk(builder.Send(annotations_offset));
apriltag_detections_destroy(detections);
diff --git a/y2023/vision/maps/target_map.json b/y2023/vision/maps/target_map.json
index 8d971f3..07d3464 100644
--- a/y2023/vision/maps/target_map.json
+++ b/y2023/vision/maps/target_map.json
@@ -1,4 +1,8 @@
{
+/* Targets have positive Z axis pointing into the board, positive X to the right
+ when looking at the board, and positive Y is down when looking at the board.
+ This means that you will get an identity rotation from the camera to target
+ frame when the target is upright, flat, and centered in the camera's view.*/
"target_poses": [
{
"id": 1,
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index 68dc14a..07c3b08 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -344,7 +344,7 @@
"source_node": "imu",
"frequency": 1,
"num_senders": 2,
- "max_size": 4096
+ "max_size": 65536
}
],
"applications": [
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index bb78bc5..840ac23 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -408,7 +408,7 @@
"source_node": "logger",
"frequency": 1,
"num_senders": 2,
- "max_size": 4096
+ "max_size": 65536
}
],
"maps": [
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index e125ebe..baf7031 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -174,7 +174,7 @@
"type": "foxglove.CompressedImage",
"source_node": "pi{{ NUM }}",
"frequency": 40,
- "max_size": 300000
+ "max_size": 622384
},
{
"name": "/pi{{ NUM }}/camera",
@@ -336,7 +336,7 @@
"source_node": "pi{{ NUM }}",
"frequency": 1,
"num_senders": 2,
- "max_size": 4096
+ "max_size": 65536
},
{
"name": "/pi{{ NUM }}/camera",
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 70a6685..8ef43d3 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -450,7 +450,12 @@
"source_node": "roborio",
"frequency": 1,
"num_senders": 2,
- "max_size": 4096
+ "max_size": 65536
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2023.control_loops.drivetrain.TargetSelectorHint",
+ "source_node": "roborio"
}
],
"applications": [