Merge "Quiet down profiled subsystem"
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index de669d6..03d2dc2 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -92,6 +92,7 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
+ ":localizer",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
@@ -152,6 +153,7 @@
data = ["//y2020:config.json"],
deps = [
":drivetrain_base",
+ ":localizer",
"//aos:configuration",
"//aos:init",
"//aos/events:shm_event_loop",
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
index 04bd107..7159118 100644
--- a/y2020/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_main.cc
@@ -2,6 +2,7 @@
#include "aos/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/drivetrain/localizer.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -12,7 +13,7 @@
aos::configuration::ReadConfig("config.json");
::aos::ShmEventLoop event_loop(&config.message());
- ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2020::control_loops::drivetrain::Localizer localizer(
&event_loop, ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2020::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index c7d81c2..af6a841 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "gflags/gflags.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/drivetrain/localizer.h"
DEFINE_string(logfile, "/tmp/logfile.bfbs",
"Name of the logfile to read from.");
@@ -51,7 +52,7 @@
reader.event_loop_factory()->MakeEventLoop("drivetrain");
drivetrain_event_loop->SkipTimingReport();
- frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ y2020::control_loops::drivetrain::Localizer localizer(
drivetrain_event_loop.get(),
y2020::control_loops::drivetrain::GetDrivetrainConfig());
frc971::control_loops::drivetrain::DrivetrainLoop drivetrain(
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index a9e454a..5551179 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -111,9 +111,9 @@
FlatbufferToTransformationMatrix(
*result.camera_calibration()->fixed_extrinsics());
// Calculate the pose of the camera relative to the robot origin.
- Eigen::Matrix<double, 4, 4> H_camera_robot = fixed_extrinsics;
+ Eigen::Matrix<double, 4, 4> H_robot_camera = fixed_extrinsics;
if (is_turret) {
- H_camera_robot = H_camera_robot *
+ H_robot_camera = H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw(
turret_data.position) *
FlatbufferToTransformationMatrix(
@@ -130,14 +130,14 @@
!vision_result->has_field_to_target()) {
continue;
}
- const Eigen::Matrix<double, 4, 4> H_target_camera =
+ const Eigen::Matrix<double, 4, 4> H_camera_target =
FlatbufferToTransformationMatrix(*vision_result->camera_to_target());
- const Eigen::Matrix<double, 4, 4> H_target_field =
+ const Eigen::Matrix<double, 4, 4> H_field_target =
FlatbufferToTransformationMatrix(*vision_result->field_to_target());
// Back out the robot position that is implied by the current camera
// reading.
- const Pose measured_pose(H_target_field *
- (H_camera_robot * H_target_camera).inverse());
+ const Pose measured_pose(H_field_target *
+ (H_robot_camera * H_camera_target).inverse());
const Eigen::Matrix<double, 3, 1> Z(measured_pose.rel_pos().x(),
measured_pose.rel_pos().y(),
measured_pose.rel_theta());
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 3be86da..acab856 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -228,26 +228,27 @@
drivetrain_plant_.state()(2, 0));
std::unique_ptr<ImageMatchResultT> frame(new ImageMatchResultT());
- for (const auto &H_target_field : TargetLocations()) {
+ for (const auto &H_field_target : TargetLocations()) {
std::unique_ptr<CameraPoseT> camera_target(new CameraPoseT());
camera_target->field_to_target.reset(new TransformationMatrixT());
- camera_target->field_to_target->data = MatrixToVector(H_target_field);
+ camera_target->field_to_target->data = MatrixToVector(H_field_target);
- Eigen::Matrix<double, 4, 4> H_camera_turret =
+ Eigen::Matrix<double, 4, 4> H_turret_camera =
Eigen::Matrix<double, 4, 4>::Identity();
if (is_turreted_) {
- H_camera_turret = frc971::control_loops::TransformationMatrixForYaw(
+ H_turret_camera = frc971::control_loops::TransformationMatrixForYaw(
turret_position_) *
CameraTurretTransformation();
}
// 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_camera_turret).inverse() *
- H_target_field);
+ camera_target->camera_to_target->data =
+ MatrixToVector((robot_pose.AsTransformationMatrix() *
+ TurretRobotTransformation() * H_turret_camera)
+ .inverse() *
+ H_field_target);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -260,17 +261,17 @@
{
frame->camera_calibration->fixed_extrinsics.reset(
new TransformationMatrixT());
- TransformationMatrixT *H_turret_robot =
+ TransformationMatrixT *H_robot_turret =
frame->camera_calibration->fixed_extrinsics.get();
- H_turret_robot->data = MatrixToVector(TurretRobotTransformation());
+ H_robot_turret->data = MatrixToVector(TurretRobotTransformation());
}
if (is_turreted_) {
frame->camera_calibration->turret_extrinsics.reset(
new TransformationMatrixT());
- TransformationMatrixT *H_camera_turret =
+ TransformationMatrixT *H_turret_camera =
frame->camera_calibration->turret_extrinsics.get();
- H_camera_turret->data = MatrixToVector(CameraTurretTransformation());
+ H_turret_camera->data = MatrixToVector(CameraTurretTransformation());
}
camera_delay_queue_.emplace(monotonic_now(), std::move(frame));