Merge changes I32fad1f0,I781d3396
* changes:
Be more accepting of blobs at larger distances
Tune new shot table with final battery compensation turned off
diff --git a/scouting/db/db.go b/scouting/db/db.go
index e21ca43..1f182b1 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -18,6 +18,11 @@
R1, R2, R3, B1, B2, B3 int32
}
+type Shift struct {
+ MatchNumber int32
+ R1scouter, R2scouter, R3scouter, B1scouter, B2scouter, B3scouter string
+}
+
type Stats struct {
TeamNumber, MatchNumber, Round int32
CompLevel string
@@ -90,6 +95,27 @@
return nil, errors.New(fmt.Sprint("Failed to create matches table: ", err))
}
+ statement, err = database.Prepare("CREATE TABLE IF NOT EXISTS shift_schedule (" +
+ "id SERIAL PRIMARY KEY, " +
+ "MatchNumber INTEGER, " +
+ "R1Scouter VARCHAR, " +
+ "R2Scouter VARCHAR, " +
+ "R3Scouter VARCHAR, " +
+ "B1Scouter VARCHAR, " +
+ "B2Scouter VARCHAR, " +
+ "B3scouter VARCHAR)")
+ if err != nil {
+ database.Close()
+ return nil, errors.New(fmt.Sprint("Failed to prepare shift schedule table creation: ", err))
+ }
+ defer statement.Close()
+
+ _, err = statement.Exec()
+ if err != nil {
+ database.Close()
+ return nil, errors.New(fmt.Sprint("Failed to create shift schedule table: ", err))
+ }
+
statement, err = database.Prepare("CREATE TABLE IF NOT EXISTS team_match_stats (" +
"TeamNumber INTEGER, " +
"MatchNumber INTEGER, " +
@@ -170,6 +196,15 @@
return errors.New(fmt.Sprint("Failed to drop matches table: ", err))
}
+ statement, err = database.Prepare("DROP TABLE IF EXISTS shift_schedule")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare dropping shifts table: ", err))
+ }
+ _, err = statement.Exec()
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to drop shifts table: ", err))
+ }
+
statement, err = database.Prepare("DROP TABLE IF EXISTS team_match_stats")
if err != nil {
return errors.New(fmt.Sprint("Failed to prepare dropping stats table: ", err))
@@ -224,6 +259,26 @@
return nil
}
+func (database *Database) AddToShift(sh Shift) error {
+ statement, err := database.Prepare("INSERT INTO shift_schedule(" +
+ "MatchNumber, " +
+ "R1scouter, R2scouter, R3scouter, B1scouter, B2scouter, B3scouter) " +
+ "VALUES (" +
+ "$1, " +
+ "$2, $3, $4, $5, $6, $7)")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare insertion into shift database: ", err))
+ }
+ defer statement.Close()
+
+ _, err = statement.Exec(sh.MatchNumber,
+ sh.R1scouter, sh.R2scouter, sh.R3scouter, sh.B1scouter, sh.B2scouter, sh.B3scouter)
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to insert into shift database: ", err))
+ }
+ return nil
+}
+
func (database *Database) AddToStats(s Stats) error {
matches, err := database.QueryMatches(s.TeamNumber)
if err != nil {
@@ -343,6 +398,27 @@
return matches, nil
}
+func (database *Database) ReturnAllShifts() ([]Shift, error) {
+ rows, err := database.Query("SELECT * FROM shift_schedule")
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to select from shift: ", err))
+ }
+ defer rows.Close()
+
+ shifts := make([]Shift, 0)
+ for rows.Next() {
+ var shift Shift
+ var id int
+ err := rows.Scan(&id, &shift.MatchNumber,
+ &shift.R1scouter, &shift.R2scouter, &shift.R3scouter, &shift.B1scouter, &shift.B2scouter, &shift.B3scouter)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to scan from shift: ", err))
+ }
+ shifts = append(shifts, shift)
+ }
+ return shifts, nil
+}
+
func (database *Database) ReturnStats() ([]Stats, error) {
rows, err := database.Query("SELECT * FROM team_match_stats")
if err != nil {
@@ -414,6 +490,27 @@
return matches, nil
}
+func (database *Database) QueryAllShifts(matchNumber_ int) ([]Shift, error) {
+ rows, err := database.Query("SELECT * FROM shift_schedule WHERE MatchNumber = $1", matchNumber_)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to select from shift for team: ", err))
+ }
+ defer rows.Close()
+
+ var shifts []Shift
+ for rows.Next() {
+ var shift Shift
+ var id int
+ err = rows.Scan(&id, &shift.MatchNumber,
+ &shift.R1scouter, &shift.R2scouter, &shift.R3scouter, &shift.B1scouter, &shift.B2scouter, &shift.B3scouter)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to scan from matches: ", err))
+ }
+ shifts = append(shifts, shift)
+ }
+ return shifts, nil
+}
+
func (database *Database) QueryStats(teamNumber_ int) ([]Stats, error) {
rows, err := database.Query("SELECT * FROM team_match_stats WHERE TeamNumber = $1", teamNumber_)
if err != nil {
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 391f336..f19b68c 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -261,6 +261,41 @@
}
}
+func TestQueryShiftDB(t *testing.T) {
+ fixture := createDatabase(t)
+ defer fixture.TearDown()
+
+ testDatabase := []Shift{
+ Shift{
+ MatchNumber: 1,
+ R1scouter: "Bob1", R2scouter: "Bob2", R3scouter: "Bob3", B1scouter: "Alice1", B2scouter: "Alice2", B3scouter: "Alice3",
+ },
+ Shift{
+ MatchNumber: 2,
+ R1scouter: "Bob1", R2scouter: "Bob2", R3scouter: "Bob3", B1scouter: "Alice1", B2scouter: "Alice2", B3scouter: "Alice3",
+ },
+ }
+
+ for i := 0; i < len(testDatabase); i++ {
+ err := fixture.db.AddToShift(testDatabase[i])
+ check(t, err, fmt.Sprint("Failed to add shift", i))
+ }
+
+ correct := []Shift{
+ Shift{
+ MatchNumber: 1,
+ R1scouter: "Bob1", R2scouter: "Bob2", R3scouter: "Bob3", B1scouter: "Alice1", B2scouter: "Alice2", B3scouter: "Alice3",
+ },
+ }
+
+ got, err := fixture.db.QueryAllShifts(1)
+ check(t, err, "Failed to query shift for match 1")
+
+ if !reflect.DeepEqual(correct, got) {
+ t.Fatalf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+}
+
func TestQueryStatsDB(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
@@ -475,6 +510,34 @@
}
}
+func TestAddReturnShiftDB(t *testing.T) {
+ fixture := createDatabase(t)
+ defer fixture.TearDown()
+
+ correct := []Shift{
+ Shift{
+ MatchNumber: 1,
+ R1scouter: "Bob1", R2scouter: "Bob2", R3scouter: "Bob3", B1scouter: "Alice1", B2scouter: "Alice2", B3scouter: "Alice3",
+ },
+ Shift{
+ MatchNumber: 2,
+ R1scouter: "Bob1", R2scouter: "Bob2", R3scouter: "Bob3", B1scouter: "Alice1", B2scouter: "Alice2", B3scouter: "Alice3",
+ },
+ }
+
+ for i := 0; i < len(correct); i++ {
+ err := fixture.db.AddToShift(correct[i])
+ check(t, err, fmt.Sprint("Failed to add shift", i))
+ }
+
+ got, err := fixture.db.ReturnAllShifts()
+ check(t, err, "Failed ReturnAllShifts()")
+
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+}
+
func TestReturnRankingsDB(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index f67fa12..fabc871 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -130,6 +130,19 @@
],
)
+cc_binary(
+ name = "superstructure_replay",
+ srcs = ["superstructure_replay.cc"],
+ deps = [
+ ":superstructure_lib",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/network:team_number",
+ ],
+)
+
cc_library(
name = "collision_avoidance_lib",
srcs = ["collision_avoidance.cc"],
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 1f0d1fc..c59737d 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -608,6 +608,7 @@
status_builder.add_fire(fire_);
status_builder.add_moving_too_fast(moving_too_fast);
status_builder.add_discarding_ball(discarding_ball_);
+ status_builder.add_collided(collided);
status_builder.add_ready_to_fire(state_ == SuperstructureState::LOADED &&
turret_near_goal && !collided);
status_builder.add_state(state_);
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index ec36dd4..5a1537e 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -41,9 +41,24 @@
.setColor(BLUE)
.setPointSize(1.0);
positionPlot.addMessageLine(status, ['fire'])
- .setColor(CYAN)
+ .setColor(BROWN)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['ready_to_fire'])
+ .setColor(GREEN)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['collided'])
+ .setColor(PINK)
.setPointSize(1.0);
+ const shotCountPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ shotCountPlot.plot.getAxisLabels().setTitle('Shot Count');
+ shotCountPlot.plot.getAxisLabels().setXLabel(TIME);
+ shotCountPlot.plot.getAxisLabels().setYLabel('balls');
+ shotCountPlot.plot.setDefaultYRange([-1.0, 2.0]);
+ shotCountPlot.addMessageLine(status, ['shot_count'])
+ .setColor(RED)
+ .setPointSize(1.0);
const intakePlot =
aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
diff --git a/y2022/control_loops/superstructure/superstructure_replay.cc b/y2022/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..b05cdb9
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,74 @@
+// This binary allows us to replay the superstructure code over existing logfile.
+// When you run this code, it generates a new logfile with the data all
+// replayed, so that it can then be run through the plotting tool or analyzed
+// in some other way. The original superstructure status data will be on the
+// /original/superstructure channel.
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "aos/network/team_number.h"
+#include "gflags/gflags.h"
+#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/superstructure.h"
+
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/superstructure_replay/",
+ "Logs all channels to the provided logfile.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ // TODO(james): Actually enforce not sending on the same buses as the logfile
+ // spews out.
+ reader.RemapLoggedChannel("/superstructure",
+ "y2022.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2022.control_loops.superstructure.Output");
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *roborio =
+ factory.GetNodeEventLoopFactory("roborio");
+
+ unlink(FLAGS_output_folder.c_str());
+ std::unique_ptr<aos::EventLoop> logger_event_loop =
+ roborio->MakeEventLoop("logger");
+ auto logger = std::make_unique<aos::logger::Logger>(logger_event_loop.get());
+ logger->StartLoggingOnRun(FLAGS_output_folder);
+
+ roborio->OnStartup([roborio]() {
+ roborio->AlwaysStart<y2022::control_loops::superstructure::Superstructure>(
+ "superstructure", std::make_shared<y2022::constants::Values>(
+ y2022::constants::MakeValues()));
+ });
+
+ std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
+ print_loop->SkipAosLog();
+ print_loop->MakeWatcher(
+ "/aos", [&print_loop](const aos::logging::LogMessageFbs &msg) {
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(&msg);
+ });
+ print_loop->MakeWatcher(
+ "/superstructure",
+ [&](const y2022::control_loops::superstructure::Status &status) {
+ if (status.estopped()) {
+ LOG(ERROR) << "Estopped";
+ }
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index ac60ce3..44dc19a 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -55,6 +55,8 @@
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying
reseating_in_catapult:bool (id: 13);
+ // Whether the turret/catapult is collided with the intake
+ collided:bool(id: 23);
// Whether the turret is ready for firing
ready_to_fire:bool (id: 20);
// Whether the robot is moving too fast to shoot
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 65ab20c..6325234 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -317,6 +317,7 @@
deps = [
":blob_detector_lib",
":calibration_data",
+ ":camera_reader_lib",
":target_estimator_lib",
"//aos:init",
"//aos/events:shm_event_loop",
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index 92d3727..0af4afc 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -21,12 +21,11 @@
using namespace frc971::vision;
-const calibration::CameraCalibration *CameraReader::FindCameraCalibration()
- const {
- const std::string_view node_name = event_loop_->node()->name()->string_view();
- const int team_number = aos::network::GetTeamNumber();
+const calibration::CameraCalibration *CameraReader::FindCameraCalibration(
+ const calibration::CalibrationData *calibration_data,
+ std::string_view node_name, int team_number) {
for (const calibration::CameraCalibration *candidate :
- *calibration_data_->camera_calibrations()) {
+ *calibration_data->camera_calibrations()) {
if (candidate->node_name()->string_view() != node_name) {
continue;
}
@@ -92,9 +91,7 @@
void CameraReader::ProcessImage(cv::Mat image_mat_distorted,
int64_t image_monotonic_timestamp_ns) {
- cv::Mat image_mat;
- cv::undistort(image_mat_distorted, image_mat, CameraIntrinsics(),
- CameraDistCoeffs());
+ cv::Mat image_mat = UndistortImage(image_mat_distorted, undistort_maps_);
BlobDetector::BlobResult blob_result;
BlobDetector::ExtractBlobs(image_mat, &blob_result);
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 7128890..8317c09 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -29,12 +29,73 @@
// TODO<jim>: Probably need to break out LED control to separate process
class CameraReader {
public:
+ static const calibration::CameraCalibration *FindCameraCalibration(
+ const calibration::CalibrationData *calibration_data,
+ std::string_view node_name, int team_number);
+
+ static cv::Mat CameraIntrinsics(
+ const calibration::CameraCalibration *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+ return result;
+ }
+
+ static cv::Mat CameraExtrinsics(
+ const calibration::CameraCalibration *camera_calibration) {
+ // TODO(james): What's the principled way to handle non-z-axis turrets?
+ const frc971::vision::calibration::TransformationMatrix *transform =
+ camera_calibration->has_turret_extrinsics()
+ ? camera_calibration->turret_extrinsics()
+ : camera_calibration->fixed_extrinsics();
+
+ cv::Mat result(4, 4, CV_32F,
+ const_cast<void *>(
+ static_cast<const void *>(transform->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), transform->data()->size());
+ return result;
+ }
+
+ static cv::Mat CameraDistCoeffs(
+ const calibration::CameraCalibration *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+ }
+
+ static std::pair<cv::Mat, cv::Mat> ComputeUndistortMaps(
+ const cv::Mat intrinsics, const cv::Mat dist_coeffs) {
+ std::pair<cv::Mat, cv::Mat> undistort_maps;
+ static const cv::Size kImageSize = {640, 480};
+ cv::initUndistortRectifyMap(intrinsics, dist_coeffs, cv::Mat(), intrinsics,
+ kImageSize, CV_16SC2, undistort_maps.first,
+ undistort_maps.second);
+ return undistort_maps;
+ }
+
+ static cv::Mat UndistortImage(cv::Mat image_distorted,
+ std::pair<cv::Mat, cv::Mat> undistort_maps) {
+ cv::Mat image;
+ cv::remap(image_distorted, image, undistort_maps.first,
+ undistort_maps.second, cv::INTER_LINEAR);
+ return image;
+ }
+
CameraReader(aos::ShmEventLoop *event_loop,
const calibration::CalibrationData *calibration_data,
V4L2Reader *reader)
: event_loop_(event_loop),
calibration_data_(calibration_data),
- camera_calibration_(FindCameraCalibration()),
+ camera_calibration_(FindCameraCalibration(
+ calibration_data_, event_loop_->node()->name()->string_view(),
+ aos::network::GetTeamNumber())),
+ undistort_maps_(
+ ComputeUndistortMaps(CameraIntrinsics(), CameraDistCoeffs())),
reader_(reader),
image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
target_estimator_(CameraIntrinsics(), CameraExtrinsics()),
@@ -60,8 +121,6 @@
double GetDutyCycle() { return duty_cycle_; }
private:
- const calibration::CameraCalibration *FindCameraCalibration() const;
-
// Processes an image (including sending the results).
void ProcessImage(cv::Mat image_mat_distorted,
int64_t image_monotonic_timestamp_ns);
@@ -70,40 +129,21 @@
void ReadImage();
cv::Mat CameraIntrinsics() const {
- cv::Mat result(3, 3, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->intrinsics()->data())));
- result.convertTo(result, CV_64F);
- CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
- return result;
+ return CameraIntrinsics(camera_calibration_);
}
cv::Mat CameraExtrinsics() const {
- // TODO(james): What's the principled way to handle non-z-axis turrets?
- const frc971::vision::calibration::TransformationMatrix *transform =
- camera_calibration_->has_turret_extrinsics()
- ? camera_calibration_->turret_extrinsics()
- : camera_calibration_->fixed_extrinsics();
-
- cv::Mat result(4, 4, CV_32F,
- const_cast<void *>(
- static_cast<const void *>(transform->data()->data())));
- result.convertTo(result, CV_64F);
- CHECK_EQ(result.total(), transform->data()->size());
- return result;
+ return CameraExtrinsics(camera_calibration_);
}
cv::Mat CameraDistCoeffs() const {
- const cv::Mat result(5, 1, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->dist_coeffs()->data())));
- CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
- return result;
+ return CameraDistCoeffs(camera_calibration_);
}
aos::ShmEventLoop *const event_loop_;
const calibration::CalibrationData *const calibration_data_;
const calibration::CameraCalibration *const camera_calibration_;
+ std::pair<cv::Mat, cv::Mat> undistort_maps_;
V4L2Reader *const reader_;
aos::Sender<CameraImage> image_sender_;
TargetEstimator target_estimator_;
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 9eef390..1447d81 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -378,16 +378,16 @@
[](const std::pair<size_t, size_t> &a,
const std::pair<size_t, size_t> &b) { return a.first < b.first; });
- size_t middle_tape_index = 1000;
+ std::optional<size_t> middle_tape_index = std::nullopt;
for (size_t i = 0; i < tape_indices.size(); ++i) {
if (tape_indices[i].second == middle_blob_index_) {
middle_tape_index = i;
}
}
- CHECK_NE(middle_tape_index, 1000) << "Failed to find middle tape";
+ CHECK(middle_tape_index.has_value()) << "Failed to find middle tape";
if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
<< middle_blob_index_;
for (size_t i = 0; i < tape_indices.size(); ++i) {
const auto distance = DistanceFromTapeIndex(
@@ -400,7 +400,7 @@
{
size_t offset = 0;
- for (size_t i = middle_tape_index + 1; i < tape_indices.size(); ++i) {
+ for (size_t i = *middle_tape_index + 1; i < tape_indices.size(); ++i) {
tape_indices[i].first -= offset;
if (tape_indices[i].first > tape_indices[i - 1].first + 1) {
@@ -412,7 +412,7 @@
}
if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
<< middle_blob_index_;
for (size_t i = 0; i < tape_indices.size(); ++i) {
const auto distance = DistanceFromTapeIndex(
@@ -425,7 +425,7 @@
{
size_t offset = 0;
- for (size_t i = middle_tape_index; i > 0; --i) {
+ for (size_t i = *middle_tape_index; i > 0; --i) {
tape_indices[i - 1].first -= offset;
if (tape_indices[i - 1].first + 1 < tape_indices[i].first) {
@@ -440,7 +440,7 @@
}
if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
<< middle_blob_index_;
for (size_t i = 0; i < tape_indices.size(); ++i) {
const auto distance = DistanceFromTapeIndex(
@@ -566,11 +566,11 @@
size_t blob_index, const std::vector<cv::Point_<S>> &tape_points) const {
auto distance = cv::Point_<S>(std::numeric_limits<S>::infinity(),
std::numeric_limits<S>::infinity());
- size_t final_match = 255;
+ std::optional<size_t> final_match = std::nullopt;
if (blob_index == middle_blob_index_) {
// Fix the middle blob so the solver can't go too far off
final_match = tape_points.size() / 2;
- distance = DistanceFromTapeIndex(blob_index, final_match, tape_points);
+ distance = DistanceFromTapeIndex(blob_index, *final_match, tape_points);
} else {
// Give the other blob_stats some freedom in case some are split into pieces
for (auto it = tape_points.begin(); it < tape_points.end(); it++) {
@@ -585,11 +585,11 @@
}
}
- VLOG(2) << "Matched index " << blob_index << " to " << final_match
+ CHECK(final_match.has_value());
+ VLOG(2) << "Matched index " << blob_index << " to " << *final_match
<< " distance " << distance.x << " " << distance.y;
- CHECK_NE(final_match, 255);
- return final_match;
+ return *final_match;
}
void TargetEstimator::DrawProjectedHub(
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 446f1f6..e455f66 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -13,6 +13,7 @@
#include "frc971/vision/vision_generated.h"
#include "y2022/vision/blob_detector.h"
#include "y2022/vision/calibration_data.h"
+#include "y2022/vision/camera_reader.h"
#include "y2022/vision/target_estimate_generated.h"
#include "y2022/vision/target_estimator.h"
@@ -213,51 +214,24 @@
const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
CalibrationData());
- const calibration::CameraCalibration *calibration = nullptr;
- for (const calibration::CameraCalibration *candidate :
- *calibration_data.message().camera_calibrations()) {
- if ((candidate->node_name()->string_view() == FLAGS_calibration_node) &&
- (candidate->team_number() == FLAGS_calibration_team_number)) {
- calibration = candidate;
- break;
- }
- }
+ const calibration::CameraCalibration *calibration =
+ CameraReader::FindCameraCalibration(&calibration_data.message(),
+ FLAGS_calibration_node,
+ FLAGS_calibration_team_number);
+ const auto intrinsics = CameraReader::CameraIntrinsics(calibration);
+ const auto extrinsics = CameraReader::CameraExtrinsics(calibration);
+ const auto dist_coeffs = CameraReader::CameraDistCoeffs(calibration);
- CHECK(calibration) << "No calibration data found for node \""
- << FLAGS_calibration_node << "\" with team number "
- << FLAGS_calibration_team_number;
-
- const auto intrinsics_float = cv::Mat(
- 3, 3, CV_32F,
- const_cast<void *>(
- static_cast<const void *>(calibration->intrinsics()->data())));
- cv::Mat intrinsics;
- intrinsics_float.convertTo(intrinsics, CV_64F);
-
- const frc971::vision::calibration::TransformationMatrix *transform =
- calibration->has_turret_extrinsics() ? calibration->turret_extrinsics()
- : calibration->fixed_extrinsics();
-
- const auto extrinsics_float = cv::Mat(
- 4, 4, CV_32F,
- const_cast<void *>(static_cast<const void *>(transform->data()->data())));
- cv::Mat extrinsics;
- extrinsics_float.convertTo(extrinsics, CV_64F);
-
- const auto dist_coeffs_float = cv::Mat(
- 5, 1, CV_32F,
- const_cast<void *>(
- static_cast<const void *>(calibration->dist_coeffs()->data())));
- cv::Mat dist_coeffs;
- dist_coeffs_float.convertTo(dist_coeffs, CV_64F);
+ // Compute undistortion map once for efficiency
+ const auto undistort_maps =
+ CameraReader::ComputeUndistortMaps(intrinsics, dist_coeffs);
TargetEstimator estimator(intrinsics, extrinsics);
for (auto it = file_list.begin() + FLAGS_skip; it < file_list.end(); it++) {
LOG(INFO) << "Reading file " << (it - file_list.begin()) << ": " << *it;
- cv::Mat image_mat_distorted = cv::imread(it->c_str());
- cv::Mat image_mat;
- cv::undistort(image_mat_distorted, image_mat, intrinsics, dist_coeffs);
+ cv::Mat image_mat =
+ CameraReader::UndistortImage(cv::imread(it->c_str()), undistort_maps);
BlobDetector::BlobResult blob_result;
blob_result.binarized_image =
diff --git a/y2022/vision/viewer_replay.cc b/y2022/vision/viewer_replay.cc
index b2d3464..5d09d55 100644
--- a/y2022/vision/viewer_replay.cc
+++ b/y2022/vision/viewer_replay.cc
@@ -194,6 +194,8 @@
bool use_image = true;
if (FLAGS_detected_only || FLAGS_filtered_only) {
+ // TODO(milind): if adding target estimation here in the future,
+ // undistortion is needed
BlobDetector::BlobResult blob_result;
BlobDetector::ExtractBlobs(image_mat, &blob_result);
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 8651923..1a5a1aa 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -7,7 +7,8 @@
"frequency": 100,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes" : [
- "imu"
+ "imu",
+ "logger"
],
"destination_nodes": [
{
@@ -18,6 +19,15 @@
"timestamp_logger_nodes": [
"roborio"
]
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
}
]
},
@@ -31,6 +41,15 @@
"max_size": 200
},
{
+ "name": "/roborio/aos/remote_timestamps/logger/roborio/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/roborio/aos",
"type": "aos.RobotState",
"source_node": "roborio",