Swap to combined voltage/encoder message for localizer
Change-Id: I23b553b142bde46c321409105d49a716eb7059b6
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index c00ce90..789de2b 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -30,6 +30,11 @@
)
static_flatbuffer(
+ name = "rio_localizer_inputs_fbs",
+ srcs = ["rio_localizer_inputs.fbs"],
+)
+
+static_flatbuffer(
name = "drivetrain_output_fbs",
srcs = ["drivetrain_output.fbs"],
)
@@ -455,6 +460,7 @@
":localizer",
":localizer_fbs",
":polydrivetrain",
+ ":rio_localizer_inputs_fbs",
":spline_goal_fbs",
":splinedrivetrain",
":ssdrivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2b84a9d..95fab0a2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -350,7 +350,11 @@
dt_openloop_(dt_config_, filters_.kf()),
dt_closedloop_(dt_config_, filters_.kf(), localizer),
dt_spline_(dt_config_),
- dt_line_follow_(dt_config_, localizer->target_selector()) {
+ dt_line_follow_(dt_config_, localizer->target_selector()),
+ localizer_input_sender_(
+ event_loop->TryMakeSender<
+ frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(30);
for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
trajectory_fetchers_[ii].fetcher =
@@ -613,6 +617,17 @@
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
+
+ if (localizer_input_sender_.valid()) {
+ auto localizer_input_builder = localizer_input_sender_.MakeStaticBuilder();
+ localizer_input_builder->set_left_encoder(position->left_encoder());
+ localizer_input_builder->set_right_encoder(position->right_encoder());
+ if (output) {
+ localizer_input_builder->set_left_voltage(output_struct.left_voltage);
+ localizer_input_builder->set_right_voltage(output_struct.right_voltage);
+ }
+ localizer_input_builder.CheckOk(localizer_input_builder.Send());
+ }
}
flatbuffers::Offset<Output> DrivetrainLoop::Zero(
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 87266f1..332b117 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -18,6 +18,7 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_static.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/polytope.h"
@@ -191,6 +192,8 @@
bool has_been_enabled_ = false;
aos::SendFailureCounter status_failure_counter_;
+ aos::Sender<::frc971::control_loops::drivetrain::RioLocalizerInputsStatic>
+ localizer_input_sender_;
};
} // namespace frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
index 747b16c..1cf83d4 100644
--- a/frc971/control_loops/drivetrain/localization/BUILD
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -13,6 +13,7 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:rio_localizer_inputs_fbs",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:calibration_fbs",
"@org_tuxfamily_eigen//:eigen",
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 1f4aadc..0986037 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -4,39 +4,62 @@
LocalizationUtils::LocalizationUtils(aos::EventLoop *event_loop)
: event_loop_(event_loop),
- output_fetcher_(event_loop->MakeFetcher<Output>("/drivetrain")),
+ output_fetcher_(event_loop->TryMakeFetcher<Output>("/drivetrain")),
position_fetcher_(event_loop->TryMakeFetcher<Position>("/drivetrain")),
+ combined_fetcher_(
+ event_loop->TryMakeFetcher<RioLocalizerInputs>("/drivetrain")),
clock_offset_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/roborio/aos")) {}
+namespace {
+template <typename T>
+Eigen::Vector2d GetVoltage(T &fetcher, aos::monotonic_clock::time_point now) {
+ fetcher.Fetch();
+ // Determine if the robot is likely to be disabled currently.
+ const bool disabled =
+ (fetcher.get() == nullptr) ||
+ (fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
+ now);
+ return disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{fetcher->left_voltage(),
+ fetcher->right_voltage()};
+}
+} // namespace
Eigen::Vector2d LocalizationUtils::VoltageOrZero(
aos::monotonic_clock::time_point now) {
- output_fetcher_.Fetch();
- // Determine if the robot is likely to be disabled currently.
- const bool disabled = (output_fetcher_.get() == nullptr) ||
- (output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- now);
- return disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()};
+ if (output_fetcher_.valid()) {
+ return GetVoltage(output_fetcher_, now);
+ } else {
+ CHECK(combined_fetcher_.valid());
+ return GetVoltage(combined_fetcher_, now);
+ }
}
+namespace {
+template <typename T>
+std::optional<Eigen::Vector2d> GetPosition(
+ T &fetcher, aos::monotonic_clock::time_point now) {
+ fetcher.Fetch();
+ const bool stale =
+ (fetcher.get() == nullptr) ||
+ (fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
+ now);
+ return stale ? std::nullopt
+ : std::make_optional<Eigen::Vector2d>(fetcher->left_encoder(),
+ fetcher->right_encoder());
+}
+} // namespace
std::optional<Eigen::Vector2d> LocalizationUtils::Encoders(
aos::monotonic_clock::time_point now) {
- CHECK(position_fetcher_.valid());
- position_fetcher_.Fetch();
- const bool stale = (position_fetcher_.get() == nullptr) ||
- (position_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- now);
- return stale ? std::nullopt
- : std::make_optional<Eigen::Vector2d>(
- position_fetcher_->left_encoder(),
- position_fetcher_->right_encoder());
+ if (position_fetcher_.valid()) {
+ return GetPosition(position_fetcher_, now);
+ } else {
+ CHECK(combined_fetcher_.valid());
+ return GetPosition(combined_fetcher_, now);
+ }
}
bool LocalizationUtils::MaybeInAutonomous() {
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 07f9aff..4d21e3b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -7,6 +7,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_generated.h"
#include "frc971/control_loops/pose.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/vision/calibration_generated.h"
@@ -50,6 +51,8 @@
aos::EventLoop *const event_loop_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::RioLocalizerInputs>
+ combined_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
};
diff --git a/frc971/control_loops/drivetrain/rio_localizer_inputs.fbs b/frc971/control_loops/drivetrain/rio_localizer_inputs.fbs
new file mode 100644
index 0000000..b9dc359
--- /dev/null
+++ b/frc971/control_loops/drivetrain/rio_localizer_inputs.fbs
@@ -0,0 +1,12 @@
+namespace frc971.control_loops.drivetrain;
+
+// This combines the drivetrian Position and Output messages
+// to be forwarded to the imu orin.
+table RioLocalizerInputs {
+ left_voltage:double (id: 0);
+ right_voltage:double (id: 1);
+ left_encoder:double (id: 2);
+ right_encoder:double (id: 3);
+}
+
+root_type RioLocalizerInputs;
diff --git a/y2024/BUILD b/y2024/BUILD
index 9e27ab2..212f70e 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -154,6 +154,7 @@
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_output_fbs",
+ "//frc971/control_loops/drivetrain:rio_localizer_inputs_fbs",
"//y2024/control_loops/superstructure:superstructure_position_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2024/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index 441360c..a177188 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -406,11 +406,7 @@
// the camera measurement and the current estimate of the
// pose. This doesn't affect any of the math, it just makes the code a bit
// more convenient to write given the Correct() interface we already have.
- observations_.CorrectKnownH(
- Eigen::Vector3d::Zero(), &U,
- Corrector(state_at_capture.value(), H_field_target, H_robot_camera,
- H_camera_target),
- R, t_);
+ observations_.CorrectKnownH(Eigen::Vector3d::Zero(), &U, corrector, R, t_);
++total_accepted_targets_;
++cameras_.at(camera_index).total_accepted_targets;
VLOG(1) << "new state " << ekf_.X_hat().topRows<3>().transpose();
diff --git a/y2024/localizer/localizer_test.cc b/y2024/localizer/localizer_test.cc
index 25c0bab..a2fc200 100644
--- a/y2024/localizer/localizer_test.cc
+++ b/y2024/localizer/localizer_test.cc
@@ -6,6 +6,7 @@
#include "aos/events/simulated_event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_static.h"
#include "frc971/control_loops/pose.h"
#include "frc971/vision/target_map_generated.h"
#include "frc971/vision/target_map_utils.h"
@@ -60,6 +61,10 @@
constants_fetcher_(imu_test_event_loop_.get()),
output_sender_(
roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
+ combined_sender_(
+ roborio_test_event_loop_->MakeSender<
+ frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
+ "/drivetrain")),
target_sender_(
camera_test_event_loop_->MakeSender<frc971::vision::TargetMap>(
"/camera0")),
@@ -81,6 +86,14 @@
output_builder.add_right_voltage(output_voltages_(1));
builder.CheckOk(builder.Send(output_builder.Finish()));
}
+ {
+ auto builder = combined_sender_.MakeStaticBuilder();
+ builder->set_left_voltage(output_voltages_(0));
+ builder->set_right_voltage(output_voltages_(1));
+ builder->set_left_encoder(drivetrain_plant_.GetLeftPosition());
+ builder->set_right_encoder(drivetrain_plant_.GetRightPosition());
+ builder.CheckOk(builder.Send());
+ }
});
roborio_test_event_loop_->OnRun([timer, this]() {
timer->Schedule(roborio_test_event_loop_->monotonic_now(),
@@ -274,6 +287,8 @@
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
aos::Sender<Output> output_sender_;
+ aos::Sender<frc971::control_loops::drivetrain::RioLocalizerInputsStatic>
+ combined_sender_;
aos::Sender<frc971::vision::TargetMap> target_sender_;
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
control_sender_;
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 2f5f4b7..5ba882a 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -222,6 +222,14 @@
"source_node": "roborio",
"frequency": 400,
"max_size": 128,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.RioLocalizerInputs",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 80,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
@@ -241,18 +249,7 @@
"source_node": "roborio",
"frequency": 400,
"max_size": 80,
- "num_senders": 2,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "imu"
- ],
- "destination_nodes": [
- {
- "name": "imu",
- "priority": 5,
- "time_to_live": 5000000
- }
- ]
+ "num_senders": 2
},
{
"name": "/drivetrain",