Merge "Move aos/input and aos/robot_state to frc971/input"
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index aa1e279..5bef204 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -176,40 +176,6 @@
absl::Span<const uint8_t> data_;
};
-// String backed flatbuffer.
-template <typename T>
-class FlatbufferString : public NonSizePrefixedFlatbuffer<T> {
- public:
- // Builds a flatbuffer using the contents of the string.
- FlatbufferString(const std::string_view data) : data_(data) {}
- // Builds a Flatbuffer by copying the data from the other flatbuffer.
- FlatbufferString(const NonSizePrefixedFlatbuffer<T> &other) {
- absl::Span<const uint8_t> d = other.span();
- data_ = std::string(reinterpret_cast<const char *>(d.data()), d.size());
- }
-
- // Copies the data from the other flatbuffer.
- FlatbufferString &operator=(const NonSizePrefixedFlatbuffer<T> &other) {
- absl::Span<const uint8_t> d = other.span();
- data_ = std::string(reinterpret_cast<const char *>(d.data()), d.size());
- return *this;
- }
-
- virtual ~FlatbufferString() override {}
-
- absl::Span<uint8_t> span() override {
- return absl::Span<uint8_t>(reinterpret_cast<uint8_t *>(data_.data()),
- data_.size());
- }
- absl::Span<const uint8_t> span() const override {
- return absl::Span<const uint8_t>(
- reinterpret_cast<const uint8_t *>(data_.data()), data_.size());
- }
-
- private:
- std::string data_;
-};
-
// ResizeableBuffer backed flatbuffer.
template <typename T>
class FlatbufferVector : public NonSizePrefixedFlatbuffer<T> {
diff --git a/aos/testing/flatbuffer_eq.h b/aos/testing/flatbuffer_eq.h
index b6b93d9..c4b708b 100644
--- a/aos/testing/flatbuffer_eq.h
+++ b/aos/testing/flatbuffer_eq.h
@@ -51,7 +51,7 @@
template <typename T>
class FlatbufferEqMatcher : public ::testing::MatcherInterface<const T *> {
public:
- FlatbufferEqMatcher(aos::FlatbufferString<T> expected)
+ FlatbufferEqMatcher(aos::FlatbufferSpan<T> expected)
: expected_(std::move(expected)) {}
~FlatbufferEqMatcher() override = default;
@@ -70,7 +70,7 @@
}
private:
- const aos::FlatbufferString<T> expected_;
+ const aos::FlatbufferSpan<T> expected_;
};
// Returns a googlemock matcher which will compare a `const T *` or a `const
@@ -81,7 +81,7 @@
template <typename T>
inline auto FlatbufferEq(const aos::NonSizePrefixedFlatbuffer<T> &expected) {
return FlatbufferUnwrapped(::testing::MakeMatcher(
- new FlatbufferEqMatcher(aos::FlatbufferString<T>(expected))));
+ new FlatbufferEqMatcher(aos::FlatbufferSpan<T>(expected))));
}
} // namespace testing
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 01dd1a3..38e00da 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -190,6 +190,7 @@
"//aos/events/logging:log_writer",
"//frc971/control_loops/drivetrain:drivetrain_lib",
"//frc971/control_loops/drivetrain:trajectory_generator",
+ "//y2020/control_loops/superstructure:superstructure_lib",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
],
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index fc50556..8f6ff63 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -5,8 +5,8 @@
// in some other way. The original drivetrain status data will be on the
// /original/drivetrain channel.
#include "aos/configuration.h"
-#include "aos/events/logging/log_writer.h"
#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"
@@ -16,6 +16,7 @@
#include "gflags/gflags.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/drivetrain/localizer.h"
+#include "y2020/control_loops/superstructure/superstructure.h"
DEFINE_string(config, "y2020/config.json",
"Name of the config file to replay using.");
@@ -74,6 +75,10 @@
"frc971.control_loops.drivetrain.Status");
reader.RemapLoggedChannel("/drivetrain",
"frc971.control_loops.drivetrain.Output");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Output");
reader.Register();
std::vector<std::unique_ptr<LoggerState>> loggers;
@@ -117,6 +122,13 @@
y2020::control_loops::drivetrain::GetDrivetrainConfig(),
drivetrain_event_loop.get(), &localizer);
+ std::unique_ptr<aos::EventLoop> superstructure_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("superstructure", node);
+ superstructure_event_loop->SkipTimingReport();
+
+ y2020::control_loops::superstructure::Superstructure superstructure(
+ superstructure_event_loop.get());
+
reader.event_loop_factory()->Run();
return 0;
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 1cab511..5169a64 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -227,8 +227,15 @@
// reading. Note that the Pose object ignores any roll/pitch components, so
// if the camera's extrinsics for pitch/roll are off, this should just
// ignore it.
- const Pose measured_pose(H_field_target *
- (H_robot_camera * H_camera_target).inverse());
+ const Pose measured_camera_pose(H_field_target * H_camera_target.inverse());
+ // Calculate the camera-to-robot transformation matrix ignoring the
+ // pitch/roll of the camera.
+ // TODO(james): This could probably be made a bit more efficient, but I
+ // don't think this is anywhere near our bottleneck currently.
+ const Eigen::Matrix<float, 4, 4> H_camera_robot_stripped =
+ Pose(H_robot_camera).AsTransformationMatrix().inverse();
+ const Pose measured_pose(measured_camera_pose.AsTransformationMatrix() *
+ H_camera_robot_stripped);
// This "Z" is the robot pose directly implied by the camera results.
// Currently, we do not actually use this result directly. However, it is
// kept around in case we want to quickly re-enable it.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index a3e26b5..ab3b075 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -286,11 +286,11 @@
// TODO(james): Use non-zero turret angles.
camera_target->camera_to_target.reset(new TransformationMatrixT());
- camera_target->camera_to_target->data =
- MatrixToVector((robot_pose.AsTransformationMatrix() *
- TurretRobotTransformation() * H_turret_camera)
- .inverse() *
- H_field_target);
+ camera_target->camera_to_target->data = MatrixToVector(
+ (robot_pose.AsTransformationMatrix() * TurretRobotTransformation() *
+ H_turret_camera * camera_calibration_offset_)
+ .inverse() *
+ H_field_target);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -366,6 +366,11 @@
std::unique_ptr<ImageMatchResultT>>>
camera_delay_queue_;
+ // Offset to add to the camera for actually taking the images, to simulate an
+ // inaccurate calibration.
+ Eigen::Matrix<double, 4, 4> camera_calibration_offset_ =
+ Eigen::Matrix<double, 4, 4>::Identity();
+
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
void set_camera_is_turreted(bool turreted) { is_turreted_ = turreted; }
@@ -453,6 +458,24 @@
EXPECT_TRUE(VerifyEstimatorAccurate(2e-3));
}
+// Tests that camera updates with a perfect model but incorrect camera pitch
+// results in no errors.
+TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdateWithBadPitch) {
+ // Introduce some camera pitch.
+ camera_calibration_offset_.template block<3, 3>(0, 0) =
+ Eigen::AngleAxis<double>(0.1, Eigen::Vector3d::UnitY())
+ .toRotationMatrix();
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(3));
+ VerifyNearGoal();
+ EXPECT_TRUE(VerifyEstimatorAccurate(2e-3));
+}
+
// Tests that camera updates with a constant initial error in the position
// results in convergence.
TEST_F(LocalizedDrivetrainTest, InitialPositionError) {
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 6cf9e8e..4365b60 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -1,5 +1,10 @@
+// These need to come before opencv, or they don't compile. Presumably opencv
+// #defines something annoying.
+// clang-format off
#include "Eigen/Dense"
#include "Eigen/Geometry"
+// clang-format on
+
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
@@ -37,17 +42,11 @@
class CameraCalibration {
public:
- CameraCalibration(const std::string_view training_data_bfbs) {
- const sift::TrainingData *const training_data =
- flatbuffers::GetRoot<sift::TrainingData>(training_data_bfbs.data());
- {
- flatbuffers::Verifier verifier(
- reinterpret_cast<const uint8_t *>(training_data_bfbs.data()),
- training_data_bfbs.size());
- CHECK(training_data->Verify(verifier));
- }
-
- camera_calibration_ = FindCameraCalibration(training_data);
+ CameraCalibration(const absl::Span<const uint8_t> training_data_bfbs) {
+ const aos::FlatbufferSpan<sift::TrainingData> training_data(
+ training_data_bfbs);
+ CHECK(training_data.Verify());
+ camera_calibration_ = FindCameraCalibration(&training_data.message());
}
cv::Mat CameraIntrinsics() const {
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 24e9ae1..ed2b4b9 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -6,7 +6,6 @@
#include "aos/flatbuffer_merge.h"
#include "aos/init.h"
#include "aos/network/team_number.h"
-
#include "y2020/vision/sift/sift971.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
@@ -599,15 +598,9 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
- const auto training_data_bfbs = SiftTrainingData();
- const sift::TrainingData *const training_data =
- flatbuffers::GetRoot<sift::TrainingData>(training_data_bfbs.data());
- {
- flatbuffers::Verifier verifier(
- reinterpret_cast<const uint8_t *>(training_data_bfbs.data()),
- training_data_bfbs.size());
- CHECK(training_data->Verify(verifier));
- }
+ const aos::FlatbufferSpan<sift::TrainingData> training_data(
+ SiftTrainingData());
+ CHECK(training_data.Verify());
const auto index_params = cv::makePtr<cv::flann::IndexParams>();
index_params->setAlgorithm(cvflann::FLANN_INDEX_KDTREE);
@@ -622,13 +615,12 @@
{
aos::Sender<sift::TrainingData> training_data_sender =
event_loop.MakeSender<sift::TrainingData>("/camera");
- training_data_sender.Send(
- aos::FlatbufferString<sift::TrainingData>(training_data_bfbs));
+ training_data_sender.Send(training_data);
}
V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
- CameraReader camera_reader(&event_loop, training_data, &v4l2_reader,
- &matcher);
+ CameraReader camera_reader(&event_loop, &training_data.message(),
+ &v4l2_reader, &matcher);
event_loop.Run();
}
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index 4ff3397..6223d76 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -49,6 +49,9 @@
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
+ deps = [
+ "@com_google_absl//absl/types:span",
+ ],
)
py_binary(
@@ -102,6 +105,9 @@
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
+ deps = [
+ "@com_google_absl//absl/types:span",
+ ],
)
cc_test(
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 402e468..2dc5cf2 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -219,13 +219,14 @@
output_prefix = [
b'#ifndef Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
b'#define Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
- b'#include <string_view>',
+ b'#include <stdint.h>',
+ b'#include "absl/types/span.h"',
b'namespace frc971 {',
b'namespace vision {',
- b'inline std::string_view SiftTrainingData() {',
+ b'inline absl::Span<const uint8_t> SiftTrainingData() {',
]
output_suffix = [
- b' return std::string_view(kData, sizeof(kData));',
+ b' return absl::Span<const uint8_t>(reinterpret_cast<const uint8_t *>(kData), sizeof(kData));',
b'}',
b'} // namespace vision',
b'} // namespace frc971',