Merge changes I419808b1,Iaeeb0231,Iec2c0dcf,I6540f960
* changes:
Deploy the Julia runtime to the scouting server
Parametrize the DriverRank.jl script
Upgrade rules_pkg
Deploy the DriverRank.jl script as part of the scouting app
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index eca86b9..ca6cc89 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -28,7 +28,7 @@
dt_config_(dt_config),
initial_drivetrain_({0.0, 0.0}),
target_selector_hint_sender_(
- event_loop->MakeSender<
+ event_loop->TryMakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
"/drivetrain")),
drivetrain_goal_sender_(
@@ -456,7 +456,8 @@
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
- {
+ if (target_selector_hint_sender_) {
+ // TODO(james): 2019? Seriously?
auto builder = target_selector_hint_sender_.MakeBuilder();
::y2019::control_loops::drivetrain::TargetSelectorHint::Builder
target_hint_builder = builder.MakeBuilder<
diff --git a/frc971/vision/foxglove_image_converter_lib.cc b/frc971/vision/foxglove_image_converter_lib.cc
index f5ecb40..920eaf7 100644
--- a/frc971/vision/foxglove_image_converter_lib.cc
+++ b/frc971/vision/foxglove_image_converter_lib.cc
@@ -3,7 +3,7 @@
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
-DEFINE_int32(jpeg_quality, 95,
+DEFINE_int32(jpeg_quality, 60,
"Compression quality of JPEGs, 0-100; lower numbers mean lower "
"quality and resulting image sizes.");
diff --git a/frc971/vision/foxglove_image_converter_test.cc b/frc971/vision/foxglove_image_converter_test.cc
index e027de2..7a3f786 100644
--- a/frc971/vision/foxglove_image_converter_test.cc
+++ b/frc971/vision/foxglove_image_converter_test.cc
@@ -6,6 +6,8 @@
#include "aos/testing/tmpdir.h"
#include "gtest/gtest.h"
+DECLARE_int32(jpeg_quality);
+
namespace frc971::vision {
std::ostream &operator<<(std::ostream &os, ImageCompression compression) {
os << ExtensionForCompression(compression);
@@ -29,6 +31,10 @@
GetParam()),
output_path_(absl::StrCat(aos::testing::TestTmpDir(), "/test.",
ExtensionForCompression(GetParam()))) {
+ // Because our test image for comparison was generated with a JPEG quality
+ // of 95, we need to use that for the test to work. This also protects the
+ // tests against future changes to the default JPEG quality.
+ FLAGS_jpeg_quality = 95;
test_event_loop_->OnRun(
[this]() { image_sender_.CheckOk(image_sender_.Send(camera_image_)); });
test_event_loop_->MakeWatcher(
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 01a8df0..dd332fc 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -7,6 +7,9 @@
DEFINE_uint64(max_num_iterations, 100,
"Maximum number of iterations for the ceres solver");
+DEFINE_double(distortion_noise_scalar, 1.0,
+ "Scale the target pose distortion factor by this when computing "
+ "the noise.");
namespace frc971::vision {
@@ -101,36 +104,33 @@
// Match consecutive detections
ceres::examples::VectorOfConstraints target_constraints;
- for (auto it = timestamped_target_detections.begin() + 1;
- it < timestamped_target_detections.end(); it++) {
- auto last_detection = *(it - 1);
+ for (auto detection = timestamped_target_detections.begin() + 1;
+ detection < timestamped_target_detections.end(); detection++) {
+ auto last_detection = detection - 1;
// Skip two consecutive detections of the same target, because the solver
// doesn't allow this
- if (it->id == last_detection.id) {
+ if (detection->id == last_detection->id) {
continue;
}
// Don't take into account constraints too far apart in time, because the
// recording device could have moved too much
- if ((it->time - last_detection.time) > max_dt) {
+ if ((detection->time - last_detection->time) > max_dt) {
continue;
}
- auto confidence = ComputeConfidence(last_detection.time, it->time,
- last_detection.distance_from_camera,
- it->distance_from_camera);
+ auto confidence = ComputeConfidence(*last_detection, *detection);
target_constraints.emplace_back(
- ComputeTargetConstraint(last_detection, *it, confidence));
+ ComputeTargetConstraint(*last_detection, *detection, confidence));
}
return target_constraints;
}
TargetMapper::ConfidenceMatrix DataAdapter::ComputeConfidence(
- aos::distributed_clock::time_point start,
- aos::distributed_clock::time_point end, double distance_from_camera_start,
- double distance_from_camera_end) {
+ const TimestampedDetection &detection_start,
+ const TimestampedDetection &detection_end) {
constexpr size_t kX = 0;
constexpr size_t kY = 1;
constexpr size_t kZ = 2;
@@ -153,7 +153,8 @@
Q_odometry(kOrientation3, kOrientation3) = std::pow(0.01, 2);
// Add uncertainty for robot position measurements from start to end
- int iterations = (end - start) / frc971::controls::kLoopFrequency;
+ int iterations = (detection_end.time - detection_start.time) /
+ frc971::controls::kLoopFrequency;
P += static_cast<double>(iterations) * Q_odometry;
}
@@ -171,8 +172,12 @@
Q_vision(kOrientation3, kOrientation3) = std::pow(0.02, 2);
// Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
- P += Q_vision * std::pow(distance_from_camera_start, 2);
- P += Q_vision * std::pow(distance_from_camera_end, 2);
+ P += Q_vision * std::pow(detection_start.distance_from_camera, 2) *
+ (1.0 +
+ FLAGS_distortion_noise_scalar * detection_start.distortion_factor);
+ P +=
+ Q_vision * std::pow(detection_end.distance_from_camera, 2) *
+ (1.0 + FLAGS_distortion_noise_scalar * detection_end.distortion_factor);
}
return P.inverse();
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 9d943c9..3724832 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -111,6 +111,8 @@
// Horizontal distance from camera to target, used for confidence
// calculation
double distance_from_camera;
+ // A measure of how much distortion affected this detection from 0-1.
+ double distortion_factor;
TargetMapper::TargetId id;
};
@@ -128,9 +130,8 @@
// detection between robot movement over the given time period. Ceres calls
// this matrix the "information"
static TargetMapper::ConfidenceMatrix ComputeConfidence(
- aos::distributed_clock::time_point start,
- aos::distributed_clock::time_point end, double distance_from_camera_start,
- double distance_from_camera_end);
+ const TimestampedDetection &detection_start,
+ const TimestampedDetection &detection_end);
private:
// Computes the constraint between the start and end pose of the targets: the
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 271f787..cd7d18b 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -95,12 +95,28 @@
// Assumes camera and robot origin are the same
DataAdapter::TimestampedDetection MakeTimestampedDetection(
aos::distributed_clock::time_point time, Eigen::Affine3d H_robot_target,
- TargetMapper::TargetId id) {
+ TargetMapper::TargetId id, double distortion_factor = 0.001) {
auto target_pose = PoseUtils::Affine3dToPose3d(H_robot_target);
return DataAdapter::TimestampedDetection{
- time, H_robot_target,
- std::sqrt(std::pow(target_pose.p(0), 2) + std::pow(target_pose.p(1), 2)),
- id};
+ .time = time,
+ .H_robot_target = H_robot_target,
+ .distance_from_camera = target_pose.p.norm(),
+ .distortion_factor = distortion_factor,
+ .id = id};
+}
+
+DataAdapter::TimestampedDetection MakeTimestampedDetectionForConfidence(
+ aos::distributed_clock::time_point time, TargetMapper::TargetId id,
+ double distance_from_camera, double distortion_factor) {
+ auto target_pose = ceres::examples::Pose3d{
+ .p = Eigen::Vector3d(distance_from_camera, 0.0, 0.0),
+ .q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
+ return DataAdapter::TimestampedDetection{
+ .time = time,
+ .H_robot_target = PoseUtils::Pose3dToAffine3d(target_pose),
+ .distance_from_camera = target_pose.p.norm(),
+ .distortion_factor = distortion_factor,
+ .id = id};
}
bool TargetIsInView(TargetMapper::TargetPose target_detection) {
@@ -170,17 +186,25 @@
// Check the confidence matrices. Don't check the actual values
// in case the constants change, just check that the confidence of contraints
// decreases as time period or distances from camera increase.
+
+ constexpr size_t kIdStart = 0;
+ constexpr size_t kIdEnd = 1;
+
{
// Vary time period
constexpr double kDistanceStart = 0.5;
constexpr double kDistanceEnd = 2.0;
+ constexpr double kDistortionFactor = 0.001;
TargetMapper::ConfidenceMatrix last_confidence =
TargetMapper::ConfidenceMatrix::Zero();
for (size_t dt = 0; dt < 15; dt++) {
TargetMapper::ConfidenceMatrix confidence =
- DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(dt),
- kDistanceStart, kDistanceEnd);
+ DataAdapter::ComputeConfidence(
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(0), kIdStart, kDistanceStart, kDistortionFactor),
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(dt), kIdEnd, kDistanceEnd, kDistortionFactor));
if (dt != 0) {
// Confidence only decreases every 5ms (control loop period)
@@ -198,13 +222,19 @@
// Vary distance at start
constexpr int kDt = 3;
constexpr double kDistanceEnd = 1.5;
+ constexpr double kDistortionFactor = 0.001;
+
TargetMapper::ConfidenceMatrix last_confidence =
TargetMapper::ConfidenceMatrix::Zero();
for (double distance_start = 0.0; distance_start < 3.0;
distance_start += 0.5) {
TargetMapper::ConfidenceMatrix confidence =
- DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(kDt),
- distance_start, kDistanceEnd);
+ DataAdapter::ComputeConfidence(
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(0), kIdStart, distance_start, kDistortionFactor),
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(kDt), kIdEnd, kDistanceEnd, kDistortionFactor));
+
if (distance_start != 0.0) {
EXPECT_CONFIDENCE_GT(last_confidence, confidence);
}
@@ -216,18 +246,48 @@
// Vary distance at end
constexpr int kDt = 2;
constexpr double kDistanceStart = 2.5;
+ constexpr double kDistortionFactor = 0.001;
+
TargetMapper::ConfidenceMatrix last_confidence =
TargetMapper::ConfidenceMatrix::Zero();
for (double distance_end = 0.0; distance_end < 3.0; distance_end += 0.5) {
TargetMapper::ConfidenceMatrix confidence =
- DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(kDt),
- kDistanceStart, distance_end);
+ DataAdapter::ComputeConfidence(
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(0), kIdStart, kDistanceStart, kDistortionFactor),
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(kDt), kIdEnd, distance_end, kDistortionFactor));
+
if (distance_end != 0.0) {
EXPECT_CONFIDENCE_GT(last_confidence, confidence);
}
last_confidence = confidence;
}
}
+
+ {
+ // Vary distortion factor
+ constexpr int kDt = 2;
+ constexpr double kDistanceStart = 2.5;
+ constexpr double kDistanceEnd = 1.5;
+
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
+ for (double distortion_factor = 0.0; distortion_factor <= 1.0;
+ distortion_factor += 0.01) {
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(0), kIdStart, kDistanceStart, distortion_factor),
+ MakeTimestampedDetectionForConfidence(
+ TimeInMs(kDt), kIdEnd, kDistanceEnd, distortion_factor));
+
+ if (distortion_factor != 0.0) {
+ EXPECT_CONFIDENCE_GT(last_confidence, confidence);
+ }
+ last_confidence = confidence;
+ }
+ }
}
TEST(DataAdapterTest, MatchTargetDetections) {
@@ -386,10 +446,18 @@
target_detection.pose.q.y() += dist(generator_) * kNoiseScaleOrientation;
target_detection.pose.q.z() += dist(generator_) * kNoiseScaleOrientation;
+ // Get most distortion factors close to zero, but have a few outliers
+ const std::vector<double> kDistortionFactorIntervals = {0.0, 0.01, 1.0};
+ const std::vector<double> kDistortionFactorWeights = {0.9, 0.1};
+ std::piecewise_constant_distribution<double> distortion_factor_dist(
+ kDistortionFactorIntervals.begin(), kDistortionFactorIntervals.end(),
+ kDistortionFactorWeights.begin());
+ double distortion_factor = distortion_factor_dist(generator_);
+
auto time_point = TimeInMs(t);
return MakeTimestampedDetection(
time_point, PoseUtils::Pose3dToAffine3d(target_detection.pose),
- target_detection.id);
+ target_detection.id, distortion_factor);
}
return std::nullopt;
diff --git a/y2023/BUILD b/y2023/BUILD
index 8108a39..a6fec71 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -204,6 +204,7 @@
"//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:target_selector_status_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/7971.json b/y2023/constants/7971.json
index 7cbb37e..a3bd130 100644
--- a/y2023/constants/7971.json
+++ b/y2023/constants/7971.json
@@ -13,6 +13,5 @@
"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' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ {% include 'y2023/constants/common.json' %}
}
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 7e979c7..2f91d1b 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -1,7 +1,7 @@
{
"cameras": [
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-02-22.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-03-05.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-02-22.json' %}
@@ -13,6 +13,19 @@
"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' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "robot": {
+ "tof": {
+ "interpolation_table": [
+ {
+ "tof_reading": 0.05,
+ "lateral_position": 0.23
+ },
+ {
+ "tof_reading": 0.90,
+ "lateral_position": -0.23
+ }
+ ]
+ }
+ },
+ {% include 'y2023/constants/common.json' %}
}
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index 356b232..4795d61 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -4,7 +4,7 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-05.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json' %}
@@ -13,6 +13,19 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "robot": {
+ "tof": {
+ "interpolation_table": [
+ {
+ "tof_reading": 0.05,
+ "lateral_position": 0.23
+ },
+ {
+ "tof_reading": 0.90,
+ "lateral_position": -0.23
+ }
+ ]
+ }
+ },
+ {% include 'y2023/constants/common.json' %}
}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 95d04a8..c91aa56 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -31,6 +31,7 @@
"7971.json",
"971.json",
"9971.json",
+ "common.json",
":scoring_map",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
new file mode 100644
index 0000000..c559810
--- /dev/null
+++ b/y2023/constants/common.json
@@ -0,0 +1,2 @@
+ "target_map": {% include 'y2023/vision/maps/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index e874275..bd038b8 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -8,10 +8,32 @@
calibration:frc971.vision.calibration.CameraCalibration (id: 0);
}
+// Data point for a single time of flight sensor reading. Used in a linear
+// interpolation table.
+table TimeOfFlightDatum {
+ // Time-of-flight sensor reading for the datum.
+ tof_reading:double (id: 0);
+ // Where the game piece is laterally in the robot frame. 0 = centered;
+ // positive = to the left of the robot.
+ // In meters.
+ lateral_position:double (id: 1);
+}
+
+table TimeOfFlight {
+ interpolation_table:[TimeOfFlightDatum] (id: 0);
+}
+
+table RobotConstants {
+ // Table of time-of-flight reading positions. Until we bother using one
+ // of our interpolation classes, should just contain two values.
+ tof:TimeOfFlight (id: 0);
+}
+
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
scoring_map:localizer.ScoringMap (id: 2);
+ robot:RobotConstants (id: 3);
}
root_type Constants;
diff --git a/y2023/constants/relative_scoring_map.json b/y2023/constants/relative_scoring_map.json
index b38ff46..e52fcef 100644
--- a/y2023/constants/relative_scoring_map.json
+++ b/y2023/constants/relative_scoring_map.json
@@ -14,7 +14,7 @@
},
"nominal_grid": {
"bottom": {
- "left_cone": {
+ "right_cone": {
"x": -0.559,
"y": 0.463,
"z": -0.254
@@ -24,14 +24,14 @@
"y": 0.463,
"z": -0.254
},
- "right_cone": {
+ "left_cone": {
"x": 0.559,
"y": 0.463,
"z": -0.254
}
},
"middle": {
- "left_cone": {
+ "right_cone": {
"x": -0.559,
"y": -0.407,
"z": 0.217
@@ -41,14 +41,14 @@
"y": -0.407,
"z": 0.217
},
- "right_cone": {
+ "left_cone": {
"x": 0.559,
"y": -0.407,
"z": 0.217
}
},
"top": {
- "left_cone": {
+ "right_cone": {
"x": -0.559,
"y": -0.707,
"z": 0.647
@@ -58,7 +58,7 @@
"y": -0.707,
"z": 0.647
},
- "right_cone": {
+ "left_cone": {
"x": 0.559,
"y": -0.707,
"z": 0.647
@@ -67,14 +67,14 @@
},
"red": {
"substation": 5,
- "left": 3,
+ "right": 3,
"middle": 2,
- "right": 1
+ "left": 1
},
"blue": {
"substation": 4,
- "left": 6,
+ "right": 6,
"middle": 7,
- "right": 8
+ "left": 8
}
}
diff --git a/y2023/constants/scoring_map.json b/y2023/constants/scoring_map.json
index ebb9b38..d0fd408 100644
--- a/y2023/constants/scoring_map.json
+++ b/y2023/constants/scoring_map.json
@@ -16,51 +16,51 @@
"bottom": {
"left_cone": {
"x": 6.99,
- "y": 0.973,
+ "y": -3.497,
"z": 0.0
},
"cube": {
"x": 6.99,
- "y": 0.414,
+ "y": -2.938,
"z": 0.0
},
"right_cone": {
"x": 6.99,
- "y": -0.145,
+ "y": -2.379,
"z": 0.0
}
},
"middle": {
"left_cone": {
"x": 7.461,
- "y": 0.973,
+ "y": -3.497,
"z": 0.87
},
"cube": {
"x": 7.461,
- "y": 0.414,
+ "y": -2.938,
"z": 0.87
},
"right_cone": {
"x": 7.461,
- "y": -0.145,
+ "y": -2.379,
"z": 0.87
}
},
"top": {
"left_cone": {
"x": 7.891,
- "y": 0.973,
+ "y": -3.497,
"z": 1.17
},
"cube": {
"x": 7.891,
- "y": 0.414,
+ "y": -2.938,
"z": 1.17
},
"right_cone": {
"x": 7.891,
- "y": -0.145,
+ "y": -2.379,
"z": 1.17
}
}
@@ -69,7 +69,7 @@
"bottom": {
"left_cone": {
"x": 6.99,
- "y": -0.703,
+ "y": -1.821,
"z": 0.0
},
"cube": {
@@ -79,14 +79,14 @@
},
"right_cone": {
"x": 6.99,
- "y": -1.821,
+ "y": -0.703,
"z": 0.0
}
},
"middle": {
"left_cone": {
"x": 7.461,
- "y": -0.703,
+ "y": -1.821,
"z": 0.87
},
"cube": {
@@ -96,14 +96,14 @@
},
"right_cone": {
"x": 7.461,
- "y": -1.821,
+ "y": -0.703,
"z": 0.87
}
},
"top": {
"left_cone": {
"x": 7.891,
- "y": -0.703,
+ "y": -1.821,
"z": 1.17
},
"cube": {
@@ -113,7 +113,7 @@
},
"right_cone": {
"x": 7.891,
- "y": -1.821,
+ "y": -0.703,
"z": 1.17
}
}
@@ -122,51 +122,51 @@
"bottom": {
"left_cone": {
"x": 6.99,
- "y": -2.379,
+ "y": -0.145,
"z": 0.0
},
"cube": {
"x": 6.99,
- "y": -2.938,
+ "y": 0.414,
"z": 0.0
},
"right_cone": {
"x": 6.99,
- "y": -3.497,
+ "y": 0.973,
"z": 0.0
}
},
"middle": {
"left_cone": {
"x": 7.461,
- "y": -2.379,
+ "y": -0.145,
"z": 0.87
},
"cube": {
"x": 7.461,
- "y": -2.938,
+ "y": 0.414,
"z": 0.87
},
"right_cone": {
"x": 7.461,
- "y": -3.497,
+ "y": 0.973,
"z": 0.87
}
},
"top": {
"left_cone": {
"x": 7.891,
- "y": -2.379,
+ "y": -0.145,
"z": 1.17
},
"cube": {
"x": 7.891,
- "y": -2.938,
+ "y": 0.414,
"z": 1.17
},
"right_cone": {
"x": 7.891,
- "y": -3.497,
+ "y": 0.973,
"z": 1.17
}
}
@@ -189,51 +189,51 @@
"bottom": {
"left_cone": {
"x": -6.989,
- "y": -0.145,
+ "y": -2.379,
"z": 0.0
},
"cube": {
"x": -6.989,
- "y": 0.414,
+ "y": -2.938,
"z": 0.0
},
"right_cone": {
"x": -6.989,
- "y": 0.973,
+ "y": -3.497,
"z": 0.0
}
},
"middle": {
"left_cone": {
"x": -7.46,
- "y": -0.145,
+ "y": -2.379,
"z": 0.87
},
"cube": {
"x": -7.46,
- "y": 0.414,
+ "y": -2.938,
"z": 0.87
},
"right_cone": {
"x": -7.46,
- "y": 0.973,
+ "y": -3.497,
"z": 0.87
}
},
"top": {
"left_cone": {
"x": -7.89,
- "y": -0.145,
+ "y": -2.379,
"z": 1.17
},
"cube": {
"x": -7.89,
- "y": 0.414,
+ "y": -2.938,
"z": 1.17
},
"right_cone": {
"x": -7.89,
- "y": 0.973,
+ "y": -3.497,
"z": 1.17
}
}
@@ -242,7 +242,7 @@
"bottom": {
"left_cone": {
"x": -6.989,
- "y": -1.821,
+ "y": -0.703,
"z": 0.0
},
"cube": {
@@ -252,14 +252,14 @@
},
"right_cone": {
"x": -6.989,
- "y": -0.703,
+ "y": -1.821,
"z": 0.0
}
},
"middle": {
"left_cone": {
"x": -7.46,
- "y": -1.821,
+ "y": -0.703,
"z": 0.87
},
"cube": {
@@ -269,14 +269,14 @@
},
"right_cone": {
"x": -7.46,
- "y": -0.703,
+ "y": -1.821,
"z": 0.87
}
},
"top": {
"left_cone": {
"x": -7.89,
- "y": -1.821,
+ "y": -0.703,
"z": 1.17
},
"cube": {
@@ -286,7 +286,7 @@
},
"right_cone": {
"x": -7.89,
- "y": -0.703,
+ "y": -1.821,
"z": 1.17
}
}
@@ -295,51 +295,51 @@
"bottom": {
"left_cone": {
"x": -6.989,
- "y": -3.497,
+ "y": 0.973,
"z": 0.0
},
"cube": {
"x": -6.989,
- "y": -2.938,
+ "y": 0.414,
"z": 0.0
},
"right_cone": {
"x": -6.989,
- "y": -2.379,
+ "y": -0.145,
"z": 0.0
}
},
"middle": {
"left_cone": {
"x": -7.46,
- "y": -3.497,
+ "y": 0.973,
"z": 0.87
},
"cube": {
"x": -7.46,
- "y": -2.938,
+ "y": 0.414,
"z": 0.87
},
"right_cone": {
"x": -7.46,
- "y": -2.379,
+ "y": -0.145,
"z": 0.87
}
},
"top": {
"left_cone": {
"x": -7.89,
- "y": -3.497,
+ "y": 0.973,
"z": 1.17
},
"cube": {
"x": -7.89,
- "y": -2.938,
+ "y": 0.414,
"z": 1.17
},
"right_cone": {
"x": -7.89,
- "y": -2.379,
+ "y": -0.145,
"z": 1.17
}
}
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index a12326c..521fc09 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -125,6 +125,15 @@
)
flatbuffer_cc_library(
+ name = "target_selector_status_fbs",
+ srcs = [
+ ":target_selector_status.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
name = "target_selector_hint_fbs",
srcs = [
":target_selector_hint.fbs",
@@ -142,13 +151,17 @@
hdrs = ["target_selector.h"],
deps = [
":target_selector_hint_fbs",
+ ":target_selector_status_fbs",
"//aos/containers:sized_array",
"//aos/events:event_loop",
"//frc971/constants:constants_sender_lib",
"//frc971/control_loops:pose",
"//frc971/control_loops/drivetrain:localizer",
"//frc971/input:joystick_state_fbs",
+ "//frc971/shooter_interpolation:interpolation",
"//y2023/constants:constants_fbs",
+ "//y2023/control_loops/superstructure:superstructure_position_fbs",
+ "//y2023/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 20e5e25..aacfbbf 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -1,6 +1,8 @@
#include "y2023/control_loops/drivetrain/target_selector.h"
#include "aos/containers/sized_array.h"
+#include "frc971/shooter_interpolation/interpolation.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
namespace y2023::control_loops::drivetrain {
namespace {
@@ -13,10 +15,30 @@
: joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
hint_fetcher_(event_loop->MakeFetcher<TargetSelectorHint>("/drivetrain")),
+ superstructure_status_fetcher_(event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+ status_sender_(
+ event_loop->MakeSender<TargetSelectorStatus>("/drivetrain")),
constants_fetcher_(event_loop) {
CHECK(constants_fetcher_.constants().has_scoring_map());
CHECK(constants_fetcher_.constants().scoring_map()->has_red());
CHECK(constants_fetcher_.constants().scoring_map()->has_blue());
+ event_loop->MakeWatcher(
+ "/superstructure",
+ [this](const y2023::control_loops::superstructure::Position &msg) {
+ // Technically this means that even if we have a cube we are relying on
+ // getting a Position message before updating the game_piece_position_
+ // to zero. But if we aren't getting position messages, then things are
+ // very broken.
+ game_piece_position_ =
+ LateralOffsetForTimeOfFlight(msg.cone_position());
+ });
+
+ event_loop->AddPhasedLoop([this](int){
+ auto builder = status_sender_.MakeBuilder();
+ auto status_builder = builder.MakeBuilder<TargetSelectorStatus>();
+ status_builder.add_game_piece_position(game_piece_position_);
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+ }, std::chrono::milliseconds(100));
}
void TargetSelector::UpdateAlliance() {
@@ -151,4 +173,34 @@
return true;
}
+// TODO: Maybe this already handles field side correctly? Unsure if the line
+// follower ends up having positive as being robot frame relative or robot
+// direction relative...
+double TargetSelector::LateralOffsetForTimeOfFlight(double reading) {
+ superstructure_status_fetcher_.Fetch();
+ if (superstructure_status_fetcher_.get() != nullptr) {
+ switch (superstructure_status_fetcher_->game_piece()) {
+ case superstructure::GamePiece::NONE:
+ case superstructure::GamePiece::CUBE:
+ return 0.0;
+ case superstructure::GamePiece::CONE:
+ // execute logic below.
+ break;
+ }
+ } else {
+ return 0.0;
+ }
+ const TimeOfFlight *calibration =
+ CHECK_NOTNULL(constants_fetcher_.constants().robot()->tof());
+ // TODO(james): Use a generic interpolation table class.
+ auto table = CHECK_NOTNULL(calibration->interpolation_table());
+ CHECK_EQ(2u, table->size());
+ double x1 = table->Get(0)->tof_reading();
+ double x2 = table->Get(1)->tof_reading();
+ double y1 = table->Get(0)->lateral_position();
+ double y2 = table->Get(1)->lateral_position();
+ return frc971::shooter_interpolation::Blend((reading - x1) / (x2 - x1), y1,
+ y2);
+}
+
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index b1a9a39..bed56ce 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -6,6 +6,8 @@
#include "frc971/input/joystick_state_generated.h"
#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/drivetrain/target_selector_hint_generated.h"
+#include "y2023/control_loops/drivetrain/target_selector_status_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
namespace y2023::control_loops::drivetrain {
// This target selector provides the logic to choose which position to try to
@@ -37,7 +39,7 @@
}
double TargetRadius() const override { return 0.0; }
- double GamePieceRadius() const override { return 0.0; }
+ double GamePieceRadius() const override { return game_piece_position_; }
bool SignedRadii() const override { return true; }
Side DriveDirection() const override { return drive_direction_; }
// We will manage any desired hysteresis in the target selection.
@@ -45,12 +47,17 @@
private:
void UpdateAlliance();
+ // Returns the Y coordinate of a game piece given the time-of-flight reading.
+ double LateralOffsetForTimeOfFlight(double reading);
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ aos::Sender<TargetSelectorStatus> status_sender_;
std::optional<TargetSelectorHintT> last_hint_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
const localizer::HalfField *scoring_map_ = nullptr;
+ double game_piece_position_ = 0.0;
Side drive_direction_ = Side::DONT_CARE;
};
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector_hint.fbs b/y2023/control_loops/drivetrain/target_selector_hint.fbs
index 399d772..357bc21 100644
--- a/y2023/control_loops/drivetrain/target_selector_hint.fbs
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -3,7 +3,7 @@
namespace y2023.control_loops.drivetrain;
// Which of the grids we are going for.
-// From the perspective of the robot!
+// From the perspective of the driver station!
enum GridSelectionHint : ubyte {
LEFT,
MIDDLE,
@@ -18,7 +18,7 @@
}
// Within a row, which spot to score in.
-// From the perspective of the robot!
+// From the perspective of the driver station!
enum SpotSelectionHint : ubyte {
LEFT,
MIDDLE,
diff --git a/y2023/control_loops/drivetrain/target_selector_status.fbs b/y2023/control_loops/drivetrain/target_selector_status.fbs
new file mode 100644
index 0000000..2ca0a91
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector_status.fbs
@@ -0,0 +1,7 @@
+namespace y2023.control_loops.drivetrain;
+
+table TargetSelectorStatus {
+ game_piece_position:double (id: 0);
+}
+
+root_type TargetSelectorStatus;
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index e6f14aa..a4cb337 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -10,9 +10,9 @@
"superstructure_goal.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -30,9 +30,9 @@
"superstructure_status.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -53,11 +53,11 @@
"superstructure_position.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
- "//frc971/vision:calibration_fbs_includes",
- "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs_includes",
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
],
)
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index ace3663..3a6de93 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -284,6 +284,7 @@
// TODO(james): Tune this. Also, gain schedule for auto mode?
Eigen::Matrix<double, 3, 1> noises(1.0, 1.0, 0.5);
+ noises /= 4.0;
// Scale noise by the distortion factor for this detection
noises *= (1.0 + FLAGS_distortion_noise_scalar * target.distortion_factor());
diff --git a/y2023/localizer/scoring_map.fbs b/y2023/localizer/scoring_map.fbs
index 92b48ca..e53ea0b 100644
--- a/y2023/localizer/scoring_map.fbs
+++ b/y2023/localizer/scoring_map.fbs
@@ -2,8 +2,8 @@
namespace y2023.localizer;
-// "left" and "right" in this file are taken from the perspective of a robot
-// on the field.
+// "left" and "right" in this file are taken from the perspective of a driver
+// off the field.
// A row of three scoring positions, where the cube scoring locations will be
// in the middle.
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-02-22.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-02-22.json
deleted file mode 100644
index b21da40..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-02-22.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi1", "team_number": 971, "intrinsics": [ 890.980713, 0.0, 619.298645, 0.0, 890.668762, 364.009766, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.487722, 0.222354, 0.844207, 0.025116, 0.864934, -0.008067, 0.501821, -0.246003, 0.118392, 0.974933, -0.188387, 0.532497, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.449172, 0.252318, 0.000881, -0.000615, -0.082208 ], "calibration_timestamp": 1358501902915096335, "camera_id": "23-05" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-03-05.json
new file mode 100644
index 0000000..f585519
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi1", "team_number": 971, "intrinsics": [ 890.980713, 0.0, 619.298645, 0.0, 890.668762, 364.009766, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.487918, 0.221538, 0.844309, 0.190808, 0.866039, 0.00192, 0.499972, -0.218036, 0.109142, 0.97515, -0.192797, 0.544037, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.449172, 0.252318, 0.000881, -0.000615, -0.082208 ], "calibration_timestamp": 1358501902915096335, "camera_id": "23-05" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-05.json
deleted file mode 100644
index 1a90acf..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 9971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.838991, 0.208096, 0.502782, 0.218892, 0.515207, -0.006486, -0.857041, -0.023335, -0.175086, 0.978087, -0.112654, 0.663515, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json
new file mode 100644
index 0000000..547d8d7
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_2023-03-08.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 9971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.830467, 0.233602, 0.505721, 0.242226, 0.519225, 0.004294, -0.854627, -0.167586, -0.201814, 0.972323, -0.117727, 0.614872, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
\ No newline at end of file
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index e399470..9352bdd 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -20,8 +20,8 @@
DEFINE_string(json_path, "y2023/vision/maps/target_map.json",
"Specify path for json with initial pose guesses.");
-DEFINE_string(config, "y2023/aos_config.json",
- "Path to the config file to use.");
+DEFINE_string(config, "",
+ "If set, override the log's config file with this one.");
DEFINE_string(constants_path, "y2023/constants/constants.json",
"Path to the constant file");
DEFINE_string(output_dir, "y2023/vision/maps",
@@ -76,6 +76,7 @@
ceres::examples::Pose3d target_pose_camera =
PoseUtils::Affine3dToPose3d(H_camera_target);
double distance_from_camera = target_pose_camera.p.norm();
+ double distortion_factor = target_pose_fbs->distortion_factor();
CHECK(map.has_monotonic_timestamp_ns())
<< "Need detection timestamps for mapping";
@@ -85,6 +86,7 @@
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
.distance_from_camera = distance_from_camera,
+ .distortion_factor = distortion_factor,
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
}
}
@@ -125,12 +127,15 @@
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
+ std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config =
+ (FLAGS_config.empty()
+ ? std::nullopt
+ : std::make_optional(aos::configuration::ReadConfig(FLAGS_config)));
// open logfiles
- aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles),
- &config.message());
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(unsorted_logfiles),
+ config.has_value() ? &config->message() : nullptr);
// Send new april tag poses. This allows us to take a log of images, then play
// with the april detection code and see those changes take effect in mapping
constexpr size_t kNumPis = 4;
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 1c0beac..03985c0 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -412,7 +412,7 @@
},
{
"name": "/drivetrain",
- "type": "y2019.control_loops.drivetrain.TargetSelectorHint",
+ "type": "y2023.control_loops.drivetrain.TargetSelectorStatus",
"source_node": "roborio"
},
{