Merge "Stop armv7 ci builds"
diff --git a/aos/configuration.cc b/aos/configuration.cc
index 1d5e60a..ad0a489 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -1355,8 +1355,10 @@
case LoggerConfig::LOCAL_LOGGER:
return channel->source_node()->string_view() == node_name;
case LoggerConfig::LOCAL_AND_REMOTE_LOGGER:
- CHECK(channel->has_logger_nodes());
- CHECK_GT(channel->logger_nodes()->size(), 0u);
+ CHECK(channel->has_logger_nodes())
+ << "Missing logger nodes on " << StrippedChannelToString(channel);
+ CHECK_GT(channel->logger_nodes()->size(), 0u)
+ << "Missing logger nodes on " << StrippedChannelToString(channel);
if (channel->source_node()->string_view() == node_name) {
return true;
@@ -1364,8 +1366,10 @@
[[fallthrough]];
case LoggerConfig::REMOTE_LOGGER:
- CHECK(channel->has_logger_nodes());
- CHECK_GT(channel->logger_nodes()->size(), 0u);
+ CHECK(channel->has_logger_nodes())
+ << "Missing logger nodes on " << StrippedChannelToString(channel);
+ CHECK_GT(channel->logger_nodes()->size(), 0u)
+ << "Missing logger nodes on " << StrippedChannelToString(channel);
for (const flatbuffers::String *logger_node : *channel->logger_nodes()) {
if (logger_node->string_view() == node_name) {
return true;
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 9b03907..8a30408 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -228,9 +228,6 @@
srcs = [
"irq_affinity.cc",
],
- data = [
- "//aos/starter:rockpi_config.json",
- ],
visibility = ["//visibility:public"],
deps = [
":irq_affinity_lib",
diff --git a/aos/util/error_counter.h b/aos/util/error_counter.h
index cf6cb7f..3e69a71 100644
--- a/aos/util/error_counter.h
+++ b/aos/util/error_counter.h
@@ -67,5 +67,40 @@
private:
flatbuffers::Vector<flatbuffers::Offset<Count>> *vector_ = nullptr;
};
+
+// The ArrayErrorCounter serves the same purpose as the ErrorCounter class,
+// except that:
+// (a) It owns its own memory, rather than modifying a flatbuffer in-place.
+// (b) Because of this, the user has greater flexibility in choosing when to
+// reset the error counters.
+template <typename Error, typename Count>
+class ArrayErrorCounter {
+ public:
+ static constexpr size_t kNumErrors = ErrorCounter<Error, Count>::kNumErrors;
+ ArrayErrorCounter() { ResetCounts(); }
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Count>>>
+ PopulateCounts(flatbuffers::FlatBufferBuilder *fbb) const {
+ const flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Count>>>
+ offset = ErrorCounter<Error, Count>::Initialize(fbb);
+ flatbuffers::Vector<flatbuffers::Offset<Count>> *vector =
+ flatbuffers::GetMutableTemporaryPointer(*fbb, offset);
+ for (size_t ii = 0; ii < kNumErrors; ++ii) {
+ vector->GetMutableObject(ii)->mutate_count(error_counts_.at(ii));
+ }
+ return offset;
+ }
+
+ void IncrementError(Error error) {
+ DCHECK_LT(static_cast<size_t>(error), error_counts_.size());
+ error_counts_.at(static_cast<size_t>(error))++;
+ }
+
+ // Sets all the error counts to zero.
+ void ResetCounts() { error_counts_.fill(0); }
+
+ private:
+ std::array<size_t, kNumErrors> error_counts_;
+};
} // namespace aos::util
#endif // AOS_UTIL_ERROR_COUNTER_H_
diff --git a/aos/util/error_counter_test.cc b/aos/util/error_counter_test.cc
index 2166cea..567d71d 100644
--- a/aos/util/error_counter_test.cc
+++ b/aos/util/error_counter_test.cc
@@ -34,4 +34,45 @@
EXPECT_EQ(0u, message.message().error_counts()->Get(0)->count());
EXPECT_EQ(0u, message.message().error_counts()->Get(1)->count());
}
+
+// Tests the ArrayErrorCounter
+TEST(ErrorCounterTest, ARrayErrorCounter) {
+ ArrayErrorCounter<aos::timing::SendError, aos::timing::SendErrorCount>
+ counter;
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ counter.IncrementError(aos::timing::SendError::MESSAGE_SENT_TOO_FAST);
+ counter.IncrementError(aos::timing::SendError::MESSAGE_SENT_TOO_FAST);
+ counter.IncrementError(aos::timing::SendError::INVALID_REDZONE);
+ {
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<aos::timing::SendErrorCount>>>
+ counts_offset = counter.PopulateCounts(&fbb);
+ aos::timing::Sender::Builder builder(fbb);
+ builder.add_error_counts(counts_offset);
+ fbb.Finish(builder.Finish());
+ aos::FlatbufferDetachedBuffer<aos::timing::Sender> message = fbb.Release();
+ ASSERT_EQ(2u, message.message().error_counts()->size());
+ EXPECT_EQ(aos::timing::SendError::MESSAGE_SENT_TOO_FAST,
+ message.message().error_counts()->Get(0)->error());
+ EXPECT_EQ(2u, message.message().error_counts()->Get(0)->count());
+ EXPECT_EQ(aos::timing::SendError::INVALID_REDZONE,
+ message.message().error_counts()->Get(1)->error());
+ EXPECT_EQ(1u, message.message().error_counts()->Get(1)->count());
+ }
+
+ counter.ResetCounts();
+ {
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<aos::timing::SendErrorCount>>>
+ counts_offset = counter.PopulateCounts(&fbb);
+ aos::timing::Sender::Builder builder(fbb);
+ builder.add_error_counts(counts_offset);
+ fbb.Finish(builder.Finish());
+ aos::FlatbufferDetachedBuffer<aos::timing::Sender> message = fbb.Release();
+ ASSERT_EQ(2u, message.message().error_counts()->size());
+ EXPECT_EQ(0u, message.message().error_counts()->Get(0)->count());
+ EXPECT_EQ(0u, message.message().error_counts()->Get(1)->count());
+ }
+}
} // namespace aos::util::testing
diff --git a/frc971/analysis/foxglove.md b/frc971/analysis/foxglove.md
new file mode 100644
index 0000000..8276584
--- /dev/null
+++ b/frc971/analysis/foxglove.md
@@ -0,0 +1,36 @@
+We have some support for using [Foxglove Studio](https://studio.foxglove.dev)
+for visualizing robot data.
+
+# Accessing Foxglove
+
+You have three main options for using foxglove studio:
+1. Go to https://studio.foxglove.dev and use the most up-to-date studio. This
+ is convenient; it won't work when you do not have Internet access, and
+ has some limitations when it comes to accessing unsecured websockets.
+2. Download the foxglove desktop application.
+3. Run our local copy by running `bazel run //frc971/analysis:local_foxglove`
+ This will work offline, and serves foxglove at http://localhost:8000 by
+ default.
+
+# Log Visualization
+
+If looking at data from a log, you will first need to convert one of our AOS
+logs to MCAP so that it can be viewed in foxglove. In order to do so,
+run `bazel run -c opt //aos/util:log_to_mcap -- /path/to/log --output_path /tmp/log.mcap`.
+This will create an MCAP file at the specified path, which you can then open
+in any of the various foxglove options.
+
+# Live Visualization
+
+On the pis, we run a `foxglove_websocket` application by default. This exposes
+a websocket on the 8765 port. How you connect to this varies depending on
+what method you are using to create a foxglove instance.
+
+If using https://studio.foxglove.dev, you cannot directly access
+ws://10.9.71.10X:8765 due to security constraints. Instead, you will have to
+port forward by doing something like `ssh -L 8765:localhost:8765 pi@10.9.71.101`
+to expose the port locally, and then use the `ws://localhost:8765` websocket.
+
+If using the local foxglove, you can just use the pi IP address directly.
+
+I have not tried using the desktop Foxglove application for this.
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7e0500e..86c1ec0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -831,17 +831,3 @@
"//aos/network/www:proxy",
],
)
-
-cc_library(
- name = "localization_utils",
- srcs = ["localization_utils.cc"],
- hdrs = ["localization_utils.h"],
- deps = [
- ":drivetrain_output_fbs",
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//frc971/input:joystick_state_fbs",
- "//frc971/vision:calibration_fbs",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 89b2182..ce4e1b9 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -322,8 +322,6 @@
// Set the initial guess of the state. Can only be called once, and before
// any measurement updates have occured.
- // TODO(james): We may want to actually re-initialize and reset things on
- // the field. Create some sort of Reset() function.
void ResetInitialState(::aos::monotonic_clock::time_point t,
const State &state, const StateSquare &P) {
observations_.clear();
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
new file mode 100644
index 0000000..06c8d57
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -0,0 +1,66 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+cc_library(
+ name = "utils",
+ srcs = ["utils.cc"],
+ hdrs = ["utils.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/input:joystick_state_fbs",
+ "//frc971/vision:calibration_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "puppet_localizer",
+ srcs = ["puppet_localizer.cc"],
+ hdrs = ["puppet_localizer.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ ],
+)
+
+cc_test(
+ name = "puppet_localizer_test",
+ srcs = ["puppet_localizer_test.cc"],
+ data = ["//y2022/control_loops/drivetrain:simulation_config"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":puppet_localizer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/network:team_number",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_output_fbs",
+ srcs = [
+ "localizer_output.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "localizer_output_ts_fbs",
+ srcs = ["localizer_output.fbs"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2022/localizer/localizer_output.fbs b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
similarity index 100%
rename from y2022/localizer/localizer_output.fbs
rename to frc971/control_loops/drivetrain/localization/localizer_output.fbs
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
similarity index 82%
rename from y2022/control_loops/drivetrain/localizer.cc
rename to frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 0aa4456..3125793 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -1,10 +1,10 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-Localizer::Localizer(
+PuppetLocalizer::PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config)
@@ -15,8 +15,6 @@
localizer_output_fetcher_(
event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
"/localizer")),
- joystick_state_fetcher_(
- event_loop_->MakeFetcher<aos::JoystickState>("/aos")),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")) {
@@ -30,7 +28,7 @@
target_selector_.set_has_target(false);
}
-void Localizer::Reset(
+void PuppetLocalizer::Reset(
aos::monotonic_clock::time_point t,
const frc971::control_loops::drivetrain::HybridEkf<double>::State &state) {
// Go through and clear out all of the fetchers so that we don't get behind.
@@ -38,19 +36,12 @@
ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
}
-void Localizer::Update(const Eigen::Matrix<double, 2, 1> &U,
+void PuppetLocalizer::Update(const Eigen::Matrix<double, 2, 1> &U,
aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
- joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() != nullptr &&
- joystick_state_fetcher_->autonomous()) {
- // TODO(james): This is an inelegant way to avoid having the localizer mess
- // up splines. Do better.
- // return;
- }
if (localizer_output_fetcher_.Fetch()) {
clock_offset_fetcher_.Fetch();
bool message_bridge_connected = true;
@@ -79,8 +70,6 @@
std::chrono::nanoseconds(
localizer_output_fetcher_->monotonic_timestamp_ns()) -
monotonic_offset);
- // TODO: Finish implementing simple x/y/theta updater with state_at_capture.
- // TODO: Implement turret/camera processing logic on pi side.
std::optional<State> state_at_capture =
ekf_.LastStateBeforeTime(capture_time);
if (!state_at_capture.has_value()) {
@@ -104,4 +93,4 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
diff --git a/y2022/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
similarity index 78%
rename from y2022/control_loops/drivetrain/localizer.h
rename to frc971/control_loops/drivetrain/localization/puppet_localizer.h
index 77b29eb..4f8f4f3 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
@@ -1,5 +1,5 @@
-#ifndef Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
-#define Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
#include <string_view>
@@ -7,20 +7,20 @@
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/input/joystick_state_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-// This class handles the localization for the 2022 robot. Rather than actually
-// doing any work on the roborio, we farm all the localization out to a
+// This class handles the localization for the 2022/2023 robots. Rather than
+// actually doing any work on the roborio, we farm all the localization out to a
// raspberry pi and it then sends out LocalizerOutput messages that we treat as
-// measurement updates. See //y2022/localizer.
-// TODO(james): Needs tests. Should refactor out some of the code from the 2020
-// localizer test.
-class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
+// measurement updates. See //y202*/localizer.
+// TODO(james): Needs more tests. Should refactor out some of the code from the
+// 2020 localizer test.
+class PuppetLocalizer
+ : public frc971::control_loops::drivetrain::LocalizerInterface {
public:
typedef frc971::control_loops::TypedPose<float> Pose;
typedef frc971::control_loops::drivetrain::HybridEkf<float> HybridEkf;
@@ -29,9 +29,10 @@
typedef typename HybridEkf::StateSquare StateSquare;
typedef typename HybridEkf::Input Input;
typedef typename HybridEkf::Output Output;
- Localizer(aos::EventLoop *event_loop,
- const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config);
+ PuppetLocalizer(
+ aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
const override {
return ekf_.X_hat().cast<double>();
@@ -93,7 +94,6 @@
HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
// Target selector to allow us to satisfy the LocalizerInterface requirements.
@@ -102,6 +102,6 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
-#endif // Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
similarity index 92%
rename from y2022/control_loops/drivetrain/localizer_test.cc
rename to frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
index 77f3988..d64c419 100644
--- a/y2022/control_loops/drivetrain/localizer_test.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
@@ -1,4 +1,4 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include <queue>
@@ -8,17 +8,17 @@
#include "aos/network/testing_time_converter.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-#include "y2022/localizer/localizer_output_generated.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
DECLARE_bool(die_on_malloc);
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
namespace testing {
@@ -29,7 +29,8 @@
namespace {
DrivetrainConfig<double> GetTest2022DrivetrainConfig() {
- DrivetrainConfig<double> config = GetDrivetrainConfig();
+ DrivetrainConfig<double> config =
+ y2022::control_loops::drivetrain::GetDrivetrainConfig();
return config;
}
} // namespace
@@ -39,11 +40,13 @@
using frc971::control_loops::drivetrain::DrivetrainLoop;
using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
+
// TODO(james): Make it so this actually tests the full system of the localizer.
class LocalizedDrivetrainTest : public frc971::testing::ControlLoopTest {
protected:
- // We must use the 2022 drivetrain config so that we don't have to deal
- // with shifting:
+ // We must use the 2022 drivetrain config so that we actually have a multi-nde
+ // config with a LocalizerOutput message.
+ // TODO(james): Refactor this test to be year-agnostic.
LocalizedDrivetrainTest()
: frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig(
@@ -108,7 +111,8 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ Eigen::Matrix<double, PuppetLocalizer::HybridEkf::kNStates, 1>
+ localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
localizer_.Reset(monotonic_now(), localizer_state);
@@ -169,7 +173,7 @@
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- Localizer localizer_;
+ PuppetLocalizer localizer_;
DrivetrainLoop drivetrain_;
std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
@@ -208,4 +212,4 @@
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/localization_utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
similarity index 97%
rename from frc971/control_loops/drivetrain/localization_utils.cc
rename to frc971/control_loops/drivetrain/localization/utils.cc
index c7968e1..ff027d0 100644
--- a/frc971/control_loops/drivetrain/localization_utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/drivetrain/localization_utils.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
namespace frc971::control_loops::drivetrain {
diff --git a/frc971/control_loops/drivetrain/localization_utils.h b/frc971/control_loops/drivetrain/localization/utils.h
similarity index 100%
rename from frc971/control_loops/drivetrain/localization_utils.h
rename to frc971/control_loops/drivetrain/localization/utils.h
diff --git a/frc971/imu_reader/imu_watcher.cc b/frc971/imu_reader/imu_watcher.cc
index 0e0d656..ed9c65f 100644
--- a/frc971/imu_reader/imu_watcher.cc
+++ b/frc971/imu_reader/imu_watcher.cc
@@ -28,8 +28,7 @@
right_encoder_(
-EncoderWrapDistance(drivetrain_distance_per_encoder_tick) / 2.0,
EncoderWrapDistance(drivetrain_distance_per_encoder_tick)) {
- event_loop->MakeWatcher("/localizer", [this, event_loop](
- const IMUValuesBatch &values) {
+ event_loop->MakeWatcher("/localizer", [this](const IMUValuesBatch &values) {
CHECK(values.has_readings());
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
@@ -79,11 +78,13 @@
: aos::monotonic_clock::epoch())
: aos::monotonic_clock::time_point(
std::chrono::microseconds(value->pico_timestamp_us()));
+ const aos::monotonic_clock::time_point pi_read_timestamp =
+ aos::monotonic_clock::time_point(
+ std::chrono::nanoseconds(value->monotonic_timestamp_ns()));
// TODO(james): If we get large enough drift off of the pico,
// actually do something about it.
if (!pico_offset_.has_value()) {
- pico_offset_ =
- event_loop->context().monotonic_event_time - pico_timestamp;
+ pico_offset_ = pi_read_timestamp - pico_timestamp;
last_pico_timestamp_ = pico_timestamp;
}
if (pico_timestamp < last_pico_timestamp_) {
@@ -91,17 +92,14 @@
}
const aos::monotonic_clock::time_point sample_timestamp =
pico_offset_.value() + pico_timestamp;
- pico_offset_error_ =
- event_loop->context().monotonic_event_time - sample_timestamp;
+ pico_offset_error_ = pi_read_timestamp - sample_timestamp;
const bool zeroed = zeroer_.Zeroed();
// When not zeroed, we aim to approximate zero acceleration by doing a
// zero-order hold on the gyro and setting the accelerometer readings to
// gravity.
- callback_(sample_timestamp,
- aos::monotonic_clock::time_point(std::chrono::nanoseconds(
- value->monotonic_timestamp_ns())),
- encoders, zeroed ? zeroer_.ZeroedGyro().value() : last_gyro_,
+ callback_(sample_timestamp, pi_read_timestamp, encoders,
+ zeroed ? zeroer_.ZeroedGyro().value() : last_gyro_,
zeroed ? zeroer_.ZeroedAccel().value()
: dt_config_.imu_transform.transpose() *
Eigen::Vector3d::UnitZ());
diff --git a/frc971/rockpi/BUILD b/frc971/rockpi/BUILD
new file mode 100644
index 0000000..f6d1840
--- /dev/null
+++ b/frc971/rockpi/BUILD
@@ -0,0 +1 @@
+exports_files(["rockpi_config.json"])
diff --git a/frc971/rockpi/contents/root/bin/change_hostname.sh b/frc971/rockpi/contents/root/bin/change_hostname.sh
index a784bb1..43e6528 100755
--- a/frc971/rockpi/contents/root/bin/change_hostname.sh
+++ b/frc971/rockpi/contents/root/bin/change_hostname.sh
@@ -40,23 +40,10 @@
done
fi
-# Put corret team number in roborio's address, or add it if missing
+# Put correct team number in roborio's address, or add it if missing
if grep '^10\.[0-9]*\.[0-9]*\.2\s*roborio$' /etc/hosts >/dev/null;
then
sed -i "s/^10\.[0-9]*\.[0-9]*\(\.2\s*roborio\)$/${IP_BASE}\1/" /etc/hosts
else
echo -e "${IP_BASE}.2\troborio" >> /etc/hosts
fi
-
-# Put corret team number in imu's address, or add it if missing
-if grep '^10\.[0-9]*\.[0-9]*\.105\s.*\s*imu$' /etc/hosts >/dev/null;
-then
- sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi5 imu\)$/${IP_BASE}\1${TEAM_NUMBER}\2/" /etc/hosts
-else
- if grep '^10\.[0-9]*\.[0-9]*\.105\s*pi-[0-9]*-[0-9]*\s*pi5$' /etc/hosts
- then
- sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi5\)$/${IP_BASE}\1${TEAM_NUMBER}\2 imu/" /etc/hosts
- else
- echo -e "${IP_BASE}.105\tpi-${TEAM_NUMBER}-5 pi5 imu" >> /etc/hosts
- fi
-fi
diff --git a/aos/starter/rockpi_config.json b/frc971/rockpi/rockpi_config.json
similarity index 100%
rename from aos/starter/rockpi_config.json
rename to frc971/rockpi/rockpi_config.json
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index 711ed7d..f01af3c 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -8,11 +8,11 @@
#include "aos/events/simulated_event_loop.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
+#include "external/com_github_foxglove_schemas/CompressedImage_schema.h"
+#include "external/com_github_foxglove_schemas/ImageAnnotations_schema.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/wpilib/imu_batch_generated.h"
-#include "external/com_github_foxglove_schemas/ImageAnnotations_schema.h"
-#include "external/com_github_foxglove_schemas/CompressedImage_schema.h"
DEFINE_bool(display_undistorted, false,
"If true, display the undistorted image.");
@@ -116,10 +116,10 @@
CalibrationFoxgloveVisualizer::CalibrationFoxgloveVisualizer(
aos::EventLoop *event_loop)
: event_loop_(event_loop),
- image_converter_(event_loop_, "/camera", "/visualization",
+ image_converter_(event_loop_, "/camera", "/camera",
ImageCompression::kJpeg),
annotations_sender_(
- event_loop_->MakeSender<foxglove::ImageAnnotations>("/visualization")) {}
+ event_loop_->MakeSender<foxglove::ImageAnnotations>("/camera")) {}
aos::FlatbufferDetachedBuffer<aos::Configuration>
CalibrationFoxgloveVisualizer::AddVisualizationChannels(
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 93d68a5..4b59406 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -88,8 +88,8 @@
void HandleCharuco(const aos::monotonic_clock::time_point eof,
std::vector<std::vector<cv::Point2f>> charuco_corners) {
auto builder = annotations_sender_.MakeBuilder();
- builder.CheckOk(
- builder.Send(BuildAnnotations(eof, charuco_corners, builder.fbb())));
+ builder.CheckOk(builder.Send(
+ BuildAnnotations(eof, charuco_corners, 2.0, builder.fbb())));
}
private:
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index dbff4db..2f9e81a 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -457,14 +457,15 @@
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
const aos::monotonic_clock::time_point monotonic_now,
- const std::vector<std::vector<cv::Point2f>> &corners,
+ const std::vector<std::vector<cv::Point2f>> &corners, double thickness,
flatbuffers::FlatBufferBuilder *fbb) {
std::vector<flatbuffers::Offset<foxglove::PointsAnnotation>> rectangles;
const struct timespec now_t = aos::time::to_timespec(monotonic_now);
foxglove::Time time{static_cast<uint32_t>(now_t.tv_sec),
static_cast<uint32_t>(now_t.tv_nsec)};
+ // Draw the points in pink
const flatbuffers::Offset<foxglove::Color> color_offset =
- foxglove::CreateColor(*fbb, 0.0, 1.0, 0.0, 1.0);
+ foxglove::CreateColor(*fbb, 1.0, 0.75, 0.8, 1.0);
for (const std::vector<cv::Point2f> &rectangle : corners) {
std::vector<flatbuffers::Offset<foxglove::Point2>> points_offsets;
for (const cv::Point2f &point : rectangle) {
@@ -482,7 +483,7 @@
points_builder.add_points(points_offset);
points_builder.add_outline_color(color_offset);
points_builder.add_outline_colors(colors_offset);
- points_builder.add_thickness(2.0);
+ points_builder.add_thickness(thickness);
rectangles.push_back(points_builder.Finish());
}
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 40b1601..f1846b5 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -175,7 +175,7 @@
// visualization purposes.
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
const aos::monotonic_clock::time_point monotonic_now,
- const std::vector<std::vector<cv::Point2f>> &corners,
+ const std::vector<std::vector<cv::Point2f>> &corners, double thickness,
flatbuffers::FlatBufferBuilder *fbb);
} // namespace vision
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index d563fb0..daa371e 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -488,38 +488,6 @@
vis_robot.SetCameraParameters(camera_mat);
vis_robot.SetDistortionCoefficients(dist_coeffs);
- /*
- // Draw an initial visualization
- Eigen::Vector3d T_world_imu_vec =
- calibration_parameters.initial_state.block<3, 1>(0, 0);
- Eigen::Translation3d T_world_imu(T_world_imu_vec);
- Eigen::Affine3d H_world_imu =
- T_world_imu * calibration_parameters.initial_orientation;
-
- vis_robot.DrawFrameAxes(H_world_imu, "imu");
-
- Eigen::Quaterniond R_imu_pivot(calibration_parameters.pivot_to_imu);
- Eigen::Translation3d T_imu_pivot(
- calibration_parameters.pivot_to_imu_translation);
- Eigen::Affine3d H_imu_pivot = T_imu_pivot * R_imu_pivot;
- Eigen::Affine3d H_world_pivot = H_world_imu * H_imu_pivot;
- vis_robot.DrawFrameAxes(H_world_pivot, "pivot");
-
- Eigen::Affine3d H_imupivot_camerapivot(
- Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitZ()));
- Eigen::Quaterniond R_camera_pivot(calibration_parameters.pivot_to_camera);
- Eigen::Translation3d T_camera_pivot(
- calibration_parameters.pivot_to_camera_translation);
- Eigen::Affine3d H_camera_pivot = T_camera_pivot * R_camera_pivot;
- Eigen::Affine3d H_world_camera = H_world_imu * H_imu_pivot *
- H_imupivot_camerapivot *
- H_camera_pivot.inverse();
- vis_robot.DrawFrameAxes(H_world_camera, "camera");
-
- cv::imshow("Original poses", image_mat);
- cv::waitKey();
- */
-
uint current_state_index = 0;
uint current_turret_index = 0;
for (uint i = 0; i < camera_times_.size() - 1; i++) {
@@ -623,11 +591,6 @@
cv::imshow("Live", image_mat);
cv::waitKey(50);
-
- if (i % 200 == 0) {
- LOG(INFO) << "Pausing at step " << i;
- cv::waitKey();
- }
}
LOG(INFO) << "Finished visualizing robot. Press any key to continue";
cv::waitKey();
@@ -849,11 +812,11 @@
trans_error_scale * filter.errorpz(i);
}
- LOG(INFO) << "Cost function calc took "
- << chrono::duration<double>(aos::monotonic_clock::now() -
- start_time)
- .count()
- << " seconds";
+ VLOG(2) << "Cost function calc took "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << " seconds";
return true;
}
@@ -898,7 +861,7 @@
calibration_parameters->accelerometer_bias.data());
}
- {
+ if (calibration_parameters->has_pivot) {
// The turret's Z rotation is redundant with the camera's mounting z
// rotation since it's along the rotation axis.
ceres::CostFunction *turret_z_cost_function =
@@ -922,6 +885,17 @@
calibration_parameters->pivot_to_imu_translation.data());
}
+ {
+ // The board rotation in z is a bit arbitrary, so hoping to limit it to
+ // increase repeatability
+ ceres::CostFunction *board_z_cost_function =
+ new ceres::AutoDiffCostFunction<PenalizeQuaternionZ, 1, 4>(
+ new PenalizeQuaternionZ());
+ problem.AddResidualBlock(
+ board_z_cost_function, nullptr,
+ calibration_parameters->board_to_world.coeffs().data());
+ }
+
problem.SetParameterization(
calibration_parameters->initial_orientation.coeffs().data(),
quaternion_local_parameterization);
@@ -952,9 +926,9 @@
// Run the solver!
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
- options.gradient_tolerance = 1e-12;
+ options.gradient_tolerance = 1e-6;
options.function_tolerance = 1e-6;
- options.parameter_tolerance = 1e-12;
+ options.parameter_tolerance = 1e-6;
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
LOG(INFO) << summary.FullReport();
diff --git a/frc971/vision/target_map.json b/frc971/vision/target_map.json
index 3f6eb54..8d971f3 100644
--- a/frc971/vision/target_map.json
+++ b/frc971/vision/target_map.json
@@ -7,12 +7,11 @@
"y": -2.938,
"z": 0.463
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -22,12 +21,11 @@
"y": -1.262,
"z": 0.463
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -37,12 +35,11 @@
"y": 0.414,
"z": 0.463
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -52,12 +49,11 @@
"y": 2.740,
"z": 0.695
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -68,11 +64,10 @@
"z": 0.695
},
"orientation": {
- /* yaw of 0 */
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -82,12 +77,11 @@
"y": 0.414,
"z": 0.463
},
- /* yaw of 0 */
"orientation": {
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -97,12 +91,11 @@
"y": -1.262,
"z": 0.463
},
- /* yaw of 0 */
"orientation": {
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -112,12 +105,11 @@
"y": -2.938,
"z": 0.463
},
- /* yaw of 0 */
"orientation": {
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
}
]
diff --git a/y2022/BUILD b/y2022/BUILD
index e978f7a..1a04c2e 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -102,7 +102,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//frc971/vision:vision_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//y2022/vision:target_estimate_fbs",
"//y2022/vision:ball_color_fbs",
@@ -132,7 +132,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/control_loops/drivetrain/BUILD b/y2022/control_loops/drivetrain/BUILD
index 18855a6..148a998 100644
--- a/y2022/control_loops/drivetrain/BUILD
+++ b/y2022/control_loops/drivetrain/BUILD
@@ -71,19 +71,6 @@
],
)
-cc_library(
- name = "localizer",
- srcs = ["localizer.cc"],
- hdrs = ["localizer.h"],
- deps = [
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//frc971/control_loops/drivetrain:hybrid_ekf",
- "//frc971/control_loops/drivetrain:localizer",
- "//y2022/localizer:localizer_output_fbs",
- ],
-)
-
cc_binary(
name = "drivetrain",
srcs = [
@@ -93,10 +80,10 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
- ":localizer",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
],
)
@@ -111,25 +98,6 @@
],
)
-cc_test(
- name = "localizer_test",
- srcs = ["localizer_test.cc"],
- data = [":simulation_config"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":drivetrain_base",
- ":localizer",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_writer",
- "//aos/network:team_number",
- "//frc971/control_loops:control_loop_test",
- "//frc971/control_loops:team_number_test_environment",
- "//frc971/control_loops/drivetrain:drivetrain_lib",
- "//frc971/control_loops/drivetrain:drivetrain_test_lib",
- "//y2022/localizer:localizer_output_fbs",
- ],
-)
-
cc_binary(
name = "trajectory_generator",
srcs = [
diff --git a/y2022/control_loops/drivetrain/drivetrain_main.cc b/y2022/control_loops/drivetrain/drivetrain_main.cc
index a422eaa..6e02cc7 100644
--- a/y2022/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_main.cc
@@ -3,8 +3,8 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-#include "y2022/control_loops/drivetrain/localizer.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -15,10 +15,11 @@
aos::configuration::ReadConfig("aos_config.json");
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<::y2022::control_loops::drivetrain::Localizer> localizer =
- std::make_unique<y2022::control_loops::drivetrain::Localizer>(
- &event_loop,
- y2022::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer =
+ std::make_unique<frc971::control_loops::drivetrain::PuppetLocalizer>(
+ &event_loop,
+ y2022::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2022::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index a085049..e0db64c 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -193,10 +193,10 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/queues:gyro_fbs",
"//third_party:phoenix",
"//third_party:wpilib",
- "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index c058254..71bc73b 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -8,11 +8,11 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/queues/gyro_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
namespace y2022::control_loops::superstructure {
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index a3b1774..dd0fb67 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -17,22 +17,6 @@
)
flatbuffer_cc_library(
- name = "localizer_output_fbs",
- srcs = [
- "localizer_output.fbs",
- ],
- gen_reflections = True,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_ts_library(
- name = "localizer_output_ts_fbs",
- srcs = ["localizer_output.fbs"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_cc_library(
name = "localizer_status_fbs",
srcs = [
"localizer_status.fbs",
@@ -94,7 +78,6 @@
hdrs = ["localizer.h"],
visibility = ["//visibility:public"],
deps = [
- ":localizer_output_fbs",
":localizer_status_fbs",
":localizer_visualization_fbs",
"//aos/containers:ring_buffer",
@@ -107,8 +90,9 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:improved_down_estimator",
- "//frc971/control_loops/drivetrain:localization_utils",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:utils",
"//frc971/imu_reader:imu_watcher",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:calibration_fbs",
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 46eff94..ec81573 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -479,21 +479,6 @@
}
namespace {
-// Converts a flatbuffer TransformationMatrix to an Eigen matrix. Technically,
-// this should be able to do a single memcpy, but the extra verbosity here seems
-// appropriate.
-Eigen::Matrix<double, 4, 4> FlatbufferToTransformationMatrix(
- const frc971::vision::calibration::TransformationMatrix &flatbuffer) {
- CHECK_EQ(16u, CHECK_NOTNULL(flatbuffer.data())->size());
- Eigen::Matrix<double, 4, 4> result;
- result.setIdentity();
- for (int row = 0; row < 4; ++row) {
- for (int col = 0; col < 4; ++col) {
- result(row, col) = (*flatbuffer.data())[row * 4 + col];
- }
- }
- return result;
-}
// Node names of the pis to listen for cameras from.
constexpr std::array<std::string_view, ModelBasedLocalizer::kNumPis> kPisToUse{
@@ -521,7 +506,8 @@
}
CHECK(calibration->has_fixed_extrinsics());
const Eigen::Matrix<double, 4, 4> fixed_extrinsics =
- FlatbufferToTransformationMatrix(*calibration->fixed_extrinsics());
+ control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->fixed_extrinsics());
// Calculate the pose of the camera relative to the robot origin.
Eigen::Matrix<double, 4, 4> H_robot_camera = fixed_extrinsics;
@@ -530,7 +516,8 @@
H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw<double>(
state.turret_position) *
- FlatbufferToTransformationMatrix(*calibration->turret_extrinsics());
+ control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->turret_extrinsics());
}
return H_robot_camera;
}
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index a403ca8..59ad75c 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -9,18 +9,18 @@
#include "aos/network/message_bridge_server_generated.h"
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
-#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
-#include "frc971/control_loops/drivetrain/localization_utils.h"
+#include "frc971/imu_reader/imu_watcher.h"
+#include "frc971/input/joystick_state_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/localizer/localizer_status_generated.h"
#include "y2022/localizer/localizer_visualization_generated.h"
#include "y2022/vision/target_estimate_generated.h"
-#include "frc971/imu_reader/imu_watcher.h"
namespace frc971::controls {
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 8d8a43c..99fb95d 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -98,11 +98,11 @@
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//aos/network:team_number",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:v4l2_reader",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index c2d4f5f..7f07704 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -11,10 +11,10 @@
#include "aos/events/shm_event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/v4l2_reader.h"
#include "frc971/vision/vision_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/vision/calibration_data.h"
#include "y2022/vision/gpio.h"
#include "y2022/vision/target_estimate_generated.h"
diff --git a/y2022/www/BUILD b/y2022/www/BUILD
index 386ca90..e806404 100644
--- a/y2022/www/BUILD
+++ b/y2022/www/BUILD
@@ -24,8 +24,8 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
"//y2022/control_loops/superstructure:superstructure_status_ts_fbs",
- "//y2022/localizer:localizer_output_ts_fbs",
"//y2022/localizer:localizer_status_ts_fbs",
"//y2022/localizer:localizer_visualization_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
index ec25607..d20637f 100644
--- a/y2022/www/field_handler.ts
+++ b/y2022/www/field_handler.ts
@@ -1,7 +1,7 @@
import {ByteBuffer} from 'flatbuffers';
import {Connection} from '../../aos/network/www/proxy';
import {IntakeState, Status as SuperstructureStatus, SuperstructureState} from '../control_loops/superstructure/superstructure_status_generated'
-import {LocalizerOutput} from '../localizer/localizer_output_generated';
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated';
import {RejectionReason} from '../localizer/localizer_status_generated';
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
import {LocalizerVisualization, TargetEstimateDebug} from '../localizer/localizer_visualization_generated';
diff --git a/y2023/BUILD b/y2023/BUILD
index 16273e5..dc5e708 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -6,6 +6,7 @@
binaries = [
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
+ "//aos/events:aos_timing_report_streamer",
],
data = [
":aos_config",
@@ -41,20 +42,24 @@
"//aos/util:foxglove_websocket",
"//y2023/vision:viewer",
"//y2023/vision:aprilrobotics",
- "//y2022/localizer:localizer_main",
+ "//y2023/localizer:localizer_main",
"//y2023/constants:constants_sender",
"//y2023/vision:foxglove_image_converter",
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
"//y2023/rockpi:imu_main",
+ "//frc971/image_streamer:image_streamer",
],
data = [
":aos_config",
+ "//frc971/rockpi:rockpi_config.json",
"//y2023/constants:constants.json",
+ "//y2023/vision:image_streamer_start",
"//y2023/www:www_files",
],
dirs = [
"//y2023/www:www_files",
+ "//frc971/image_streamer/www:www_files",
],
start_binaries = [
"//aos/network:message_bridge_client",
@@ -102,11 +107,11 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2023/constants:constants_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
- "//y2023/vision:april_debug_fbs",
+ "//y2023/localizer:visualization_fbs",
"@com_github_foxglove_schemas//:schemas",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -134,9 +139,9 @@
"//y2023/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
- "//y2022/localizer:localizer_status_fbs",
- "//y2022/localizer:localizer_output_fbs",
- "//y2022/localizer:localizer_visualization_fbs",
+ "//y2023/localizer:status_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//y2023/localizer:visualization_fbs",
"//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2023/constants.cc b/y2023/constants.cc
index e2e8522..dbe8025 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -47,20 +47,22 @@
roll_joint->zeroing.moving_buffer_size = 20;
roll_joint->zeroing.allowable_encoder_error = 0.9;
- wrist->zeroing_voltage = 3.0;
- wrist->operating_voltage = 12.0;
- wrist->zeroing_profile_params = {0.5, 3.0};
- wrist->default_profile_params = {6.0, 30.0};
- wrist->range = Values::kWristRange();
- wrist->make_integral_loop =
+ wrist->subsystem_params.zeroing_voltage = 3.0;
+ wrist->subsystem_params.operating_voltage = 12.0;
+ wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
+ wrist->subsystem_params.default_profile_params = {6.0, 30.0};
+ wrist->subsystem_params.range = Values::kWristRange();
+ wrist->subsystem_params.make_integral_loop =
control_loops::superstructure::wrist::MakeIntegralWristLoop;
- wrist->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
- wrist->zeroing_constants.one_revolution_distance =
+ wrist->subsystem_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ wrist->subsystem_params.zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kWristEncoderRatio();
- wrist->zeroing_constants.zeroing_threshold = 0.0005;
- wrist->zeroing_constants.moving_buffer_size = 20;
- wrist->zeroing_constants.allowable_encoder_error = 0.9;
- wrist->zeroing_constants.middle_position = Values::kWristRange().middle();
+ wrist->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
+ wrist->subsystem_params.zeroing_constants.moving_buffer_size = 20;
+ wrist->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
+ wrist->subsystem_params.zeroing_constants.middle_position =
+ Values::kWristRange().middle();
switch (team) {
// A set of constants for tests.
@@ -74,26 +76,32 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
case kCompTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.908708932890508;
- arm_proximal->potentiometer_offset = 0.931355973012855 + 8.6743197253382;
+ arm_proximal->zeroing.measured_absolute_position = 0.0910237406998185;
+ arm_proximal->potentiometer_offset = 0.931355973012855 + 8.6743197253382 -
+ 0.101200335326309 -
+ 0.0820901660993467;
- arm_distal->zeroing.measured_absolute_position = 0.560320674747198;
- arm_distal->potentiometer_offset = 0.436664933370656 + 0.49457213779426 +
- 6.78213223139724 - 0.0220711555235029 -
- 0.0162945074111813;
+ arm_distal->zeroing.measured_absolute_position = 0.556077643172765;
+ arm_distal->potentiometer_offset =
+ 0.436664933370656 + 0.49457213779426 + 6.78213223139724 -
+ 0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
+ 0.0164398318919943;
- roll_joint->zeroing.measured_absolute_position = 0.779367181558787;
+ roll_joint->zeroing.measured_absolute_position = 1.10682573591996;
roll_joint->potentiometer_offset =
3.87038557084874 - 0.0241774522172967 + 0.0711345168020632 -
0.866186131631967 - 0.0256788357596952 + 0.18101759154572017 -
- 0.0208958996127179;
+ 0.0208958996127179 - 0.186395903925026 + 0.45801689548395 -
+ 0.5935210745062;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
@@ -107,7 +115,8 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
@@ -121,7 +130,8 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->zeroing_constants.measured_absolute_position = 0.0;
+ wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index 598a7d3..816de02 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -40,7 +40,7 @@
}
static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
- static constexpr double kDrivetrainStatorCurrentLimit() { return 40.0; }
+ static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
static double DrivetrainEncoderToMeters(int32_t in) {
return ((static_cast<double>(in) /
@@ -142,6 +142,17 @@
// Rollers
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 60.0; }
+ static constexpr double kRollerVoltage() { return 12.0; }
+
+ // Game object is fed into end effector for at least this time
+ static constexpr std::chrono::milliseconds kExtraIntakingTime() {
+ return std::chrono::seconds(2);
+ }
+
+ // Game object is spit from end effector for at least this time
+ static constexpr std::chrono::milliseconds kExtraSpittingTime() {
+ return std::chrono::seconds(2);
+ }
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
@@ -157,6 +168,12 @@
double potentiometer_offset;
};
+ struct AbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ };
+
struct ArmJointConstants {
::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
double potentiometer_offset;
@@ -166,9 +183,7 @@
ArmJointConstants arm_distal;
ArmJointConstants roll_joint;
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
- wrist;
+ AbsEncoderConstants wrist;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index f00cc1c..9ac8c8b 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -1,16 +1,17 @@
{
"cameras": [
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_2013-01-18_09-38-22.915096335.json' %}
+ "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-2_cam-23-06_2013-01-18_09-32-06.409252911.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-02-22.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_2013-01-18_09-11-56.768663953.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json' %}
+ "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' %}
}
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
new file mode 100644
index 0000000..6d5cbf9
--- /dev/null
+++ b/y2023/constants/9971.json
@@ -0,0 +1,17 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json' %}
+ }
+ ],
+ "target_map": {% include 'y2023/vision/maps/target_map.json' %}
+}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 9f8d1a4..344fa75 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -4,8 +4,10 @@
cc_library(
name = "simulated_constants_sender",
testonly = True,
- srcs = ["simulated_constants_sender.h"],
- hdrs = ["simulated_constants_sender.cc"],
+ srcs = ["simulated_constants_sender.cc"],
+ hdrs = ["simulated_constants_sender.h"],
+ data = [":test_constants.json"],
+ visibility = ["//y2023:__subpackages__"],
deps = [
":constants_fbs",
":constants_list_fbs",
@@ -16,11 +18,21 @@
)
jinja2_template(
+ name = "test_constants.json",
+ testonly = True,
+ src = "test_constants.jinja2.json",
+ includes = glob(["test_data/*.json"]),
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+jinja2_template(
name = "constants.json",
src = "constants.jinja2.json",
includes = [
"7971.json",
"971.json",
+ "9971.json",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
],
diff --git a/y2023/constants/constants.jinja2.json b/y2023/constants/constants.jinja2.json
index 2c8fce9..4fb9df7 100644
--- a/y2023/constants/constants.jinja2.json
+++ b/y2023/constants/constants.jinja2.json
@@ -10,7 +10,7 @@
},
{
"team": 9971,
- "data": {}
+ "data": {% include 'y2023/constants/9971.json' %}
}
]
}
diff --git a/y2023/constants/simulated_constants_sender.cc b/y2023/constants/simulated_constants_sender.cc
index 3086e99..f18c3c1 100644
--- a/y2023/constants/simulated_constants_sender.cc
+++ b/y2023/constants/simulated_constants_sender.cc
@@ -2,6 +2,7 @@
#include "y2023/constants/constants_list_generated.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/testing/path.h"
+#include "frc971/constants/constants_sender_lib.h"
namespace y2023 {
void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
@@ -14,5 +15,3 @@
}
}
} // namespace y2023
-
-#endif // Y2023_CONFIGURATION_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/constants/simulated_constants_sender.h b/y2023/constants/simulated_constants_sender.h
index 10ea793..44a868c 100644
--- a/y2023/constants/simulated_constants_sender.h
+++ b/y2023/constants/simulated_constants_sender.h
@@ -5,9 +5,10 @@
#include "aos/testing/path.h"
namespace y2023 {
-void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
- std::string constants_path = testing::ArtifactPath(
- "y2023/constants/constants.json"));
+void SendSimulationConstants(
+ aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path =
+ aos::testing::ArtifactPath("y2023/constants/test_constants.json"));
} // namespace y2023
#endif // Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/constants/test_constants.jinja2.json b/y2023/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..86bd834
--- /dev/null
+++ b/y2023/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 7971,
+ "data": {% include 'y2023/constants/test_data/test_team.json' %}
+ }
+ ]
+}
diff --git a/y2023/constants/test_data/calibration_pi-1.json b/y2023/constants/test_data/calibration_pi-1.json
new file mode 100644
index 0000000..1902740
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-1.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi1",
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 0.0,
+ 1.0,
+ 1.0,
+
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2023/constants/test_data/calibration_pi-2.json b/y2023/constants/test_data/calibration_pi-2.json
new file mode 100644
index 0000000..289337d
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-2.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 1.0,
+ 0.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2023/constants/test_data/calibration_pi-3.json b/y2023/constants/test_data/calibration_pi-3.json
new file mode 100644
index 0000000..073a88a
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-3.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi3",
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 1.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2023/constants/test_data/calibration_pi-4.json b/y2023/constants/test_data/calibration_pi-4.json
new file mode 100644
index 0000000..0097bdb
--- /dev/null
+++ b/y2023/constants/test_data/calibration_pi-4.json
@@ -0,0 +1,45 @@
+{
+ "node_name": "pi4",
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -1.0,
+ 0.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2023/constants/test_data/target_map.json b/y2023/constants/test_data/target_map.json
new file mode 100644
index 0000000..6bd2947
--- /dev/null
+++ b/y2023/constants/test_data/target_map.json
@@ -0,0 +1,18 @@
+{
+ "target_poses": [
+ {
+ "id": 1,
+ "position": {
+ "x": 10.0,
+ "y": 10.0,
+ "z": 10.0
+ },
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ }
+ ]
+}
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
new file mode 100644
index 0000000..adc9eae
--- /dev/null
+++ b/y2023/constants/test_data/test_team.json
@@ -0,0 +1,17 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-1.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-2.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-3.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/constants/test_data/calibration_pi-4.json' %}
+ }
+ ],
+ "target_map": {% include 'y2023/constants/test_data/target_map.json' %}
+}
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index ac00e2b..09e5137 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -84,6 +84,7 @@
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
],
)
diff --git a/y2023/control_loops/drivetrain/drivetrain_main.cc b/y2023/control_loops/drivetrain/drivetrain_main.cc
index 0817b3d..32b2455 100644
--- a/y2023/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_main.cc
@@ -3,6 +3,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -14,11 +15,11 @@
aos::configuration::ReadConfig("aos_config.json");
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<::frc971::control_loops::drivetrain::DeadReckonEkf>
- localizer =
- std::make_unique<::frc971::control_loops::drivetrain::DeadReckonEkf>(
- &event_loop,
- ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer = std::make_unique<
+ ::frc971::control_loops::drivetrain::PuppetLocalizer>(
+ &event_loop,
+ ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2023::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());
diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index 9a5a156..561bb77 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -124,3 +124,19 @@
"@pip//python_gflags",
],
)
+
+py_binary(
+ name = "turret",
+ srcs = [
+ "turret.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
diff --git a/y2023/control_loops/python/drivetrain.py b/y2023/control_loops/python/drivetrain.py
index 863684c..05739fc 100644
--- a/y2023/control_loops/python/drivetrain.py
+++ b/y2023/control_loops/python/drivetrain.py
@@ -13,7 +13,7 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
kDrivetrain = drivetrain.DrivetrainParams(
- J=6.0,
+ J=6.5,
mass=58.0,
# TODO(austin): Measure radius a bit better.
robot_radius=0.39,
@@ -23,7 +23,7 @@
G=(14.0 / 54.0) * (22.0 / 56.0),
q_pos=0.24,
q_vel=2.5,
- efficiency=0.80,
+ efficiency=0.75,
has_imu=True,
force=True,
kf_q_voltage=1.0,
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 69e2497..054a32d 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -13,6 +13,8 @@
def add_edge(cc_file, name, segment, index, reverse):
+ segment.VerifyPoints()
+
cc_file.append(" // Adding edge %d" % index)
vmax = "vmax"
if segment.vmax:
@@ -71,7 +73,6 @@
def main(argv):
cc_file = []
- cc_file.append("")
cc_file.append("#include <memory>")
cc_file.append("")
cc_file.append(
@@ -84,6 +85,7 @@
cc_file.append(
"#include \"y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h\""
)
+ cc_file.append("")
cc_file.append("using frc971::control_loops::arm::SearchGraph;")
cc_file.append(
@@ -187,28 +189,24 @@
path_function_name(name))
cc_file.append("::std::unique_ptr<Path> %s() {" %
path_function_name(name))
- cc_file.append(
- " return ::std::unique_ptr<Path>(new Path(CosSpline{NSpline<4, 2>((Eigen::Matrix<double, 2, 4>() << "
- )
+ cc_file.append(" return ::std::unique_ptr<Path>(new Path(CosSpline{")
+ cc_file.append(" NSpline<4, 2>((Eigen::Matrix<double, 2, 4>() <<")
points = [
segment.start, segment.control1, segment.control2, segment.end
]
- for i in range(len(points)):
- cc_file.append("%.12f," % (points[i][0]))
- for i in range(len(points)):
- cc_file.append(
- "%.12f%s" %
- (points[i][1], ", " if i != len(points) - 1 else ""))
+ cc_file.append(" " +
+ " ".join(["%.12f," % (p[0]) for p in points]))
+ cc_file.append(" " +
+ ", ".join(["%.12f" % (p[1]) for p in points]))
- cc_file.append(").finished()), {")
+ cc_file.append(" ).finished()), {")
for alpha, roll in segment.alpha_rolls:
cc_file.append(
- "CosSpline::AlphaTheta{.alpha = %.12f, .theta = %.12f}" %
- (alpha, roll))
- if alpha != segment.alpha_rolls[-1][0]:
- cc_file.append(", ")
+ " CosSpline::AlphaTheta{.alpha = %.12f, .theta = %.12f},"
+ % (alpha, roll))
cc_file.append(" }}));")
cc_file.append("}")
+ cc_file.append("")
# Matrix of nodes
h_file.append("::std::vector<::Eigen::Matrix<double, 3, 1>> PointList();")
@@ -229,22 +227,25 @@
h_file.append("SearchGraph MakeSearchGraph("
"const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
- h_file.append(" "
- "const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,")
- h_file.append(" "
- "double vmax,")
+ h_file.append(
+ " const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,"
+ )
+ h_file.append(" double vmax,")
h_file.append(
"const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop);"
)
- cc_file.append("SearchGraph MakeSearchGraph("
- "const frc971::control_loops::arm::Dynamics *dynamics, "
- "::std::vector<TrajectoryAndParams> *trajectories,")
- cc_file.append(" "
- "const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,")
- cc_file.append(" "
- "double vmax,")
+ cc_file.append("")
+ cc_file.append("SearchGraph MakeSearchGraph(")
+ cc_file.append(" const frc971::control_loops::arm::Dynamics *dynamics,")
+ cc_file.append(" std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(
- "const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop) {"
+ " const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double vmax,"
+ )
+ cc_file.append(
+ " const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,"
+ )
+ cc_file.append(
+ " HybridKalman<3, 1, 1>> *hybrid_roll_joint_loop) {"
)
cc_file.append(" ::std::vector<SearchGraph::Edge> edges;")
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index b9e29d0..689401a 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -162,6 +162,8 @@
self.ax = self.fig.add_subplot(111)
plt.show(block=False)
+ self.index = 0
+
def do_key_press(self, event):
pass
@@ -330,8 +332,7 @@
set_color(cr, Color(0.0, 0.5, 1.0))
for segment in self.segments:
- color = [0, random.random(), 1]
- random.shuffle(color)
+ color = [0.2, random.random(), random.random()]
set_color(cr, Color(color[0], color[1], color[2]))
segment.DrawTo(cr, self.theta_version)
with px(cr):
@@ -411,13 +412,16 @@
elif keyval == Gdk.KEY_r:
self.prev_segment_pt = self.now_segment_pt
+ elif keyval == Gdk.KEY_o:
+ # Only prints current segment
+ print(repr(self.segments[self.index]))
elif keyval == Gdk.KEY_p:
# Print out the segments.
print(repr(self.segments))
elif keyval == Gdk.KEY_g:
# Generate theta points.
if self.segments:
- print(repr(self.segments[0].ToThetaPoints()))
+ print(repr(self.segments[self.index].ToThetaPoints()))
elif keyval == Gdk.KEY_e:
best_pt = self.now_segment_pt
best_dist = 1e10
@@ -432,6 +436,10 @@
best_dist = d
self.now_segment_pt = best_pt
+ elif keyval == Gdk.KEY_k:
+ self.index += 1
+ self.index = self.index % len(self.segments)
+
elif keyval == Gdk.KEY_t:
# Toggle between theta and xy renderings
if self.theta_version:
@@ -449,9 +457,9 @@
elif keyval == Gdk.KEY_z:
self.edit_control1 = not self.edit_control1
if self.edit_control1:
- self.now_segment_pt = self.segments[0].control1
+ self.now_segment_pt = self.segments[self.index].control1
else:
- self.now_segment_pt = self.segments[0].control2
+ self.now_segment_pt = self.segments[self.index].control2
if not self.theta_version:
data = to_xy(self.now_segment_pt[0], self.now_segment_pt[1])
self.last_pos = (data[0], data[1])
@@ -474,9 +482,9 @@
self.now_segment_pt = np.array(shift_angles(pt_theta))
if self.edit_control1:
- self.segments[0].control1 = self.now_segment_pt
+ self.segments[self.index].control1 = self.now_segment_pt
else:
- self.segments[0].control2 = self.now_segment_pt
+ self.segments[self.index].control2 = self.now_segment_pt
print('Clicked at theta: %s' % (repr(self.now_segment_pt, )))
if not self.theta_version:
@@ -485,9 +493,11 @@
self.circular_index_select))
print('c1: np.array([%f, %f])' %
- (self.segments[0].control1[0], self.segments[0].control1[1]))
+ (self.segments[self.index].control1[0],
+ self.segments[self.index].control1[1]))
print('c2: np.array([%f, %f])' %
- (self.segments[0].control2[0], self.segments[0].control2[1]))
+ (self.segments[self.index].control2[0],
+ self.segments[self.index].control2[1]))
self.redraw()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index b21f548..87ced83 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -6,22 +6,23 @@
joint_center[1] + l2 - l1,
0.0,
circular_index=1)
-
-neutral_to_pickup_1 = to_theta_with_circular_index(0.3, 0.6, circular_index=1)
-neutral_to_pickup_2 = to_theta_with_circular_index(0.3, 0.4, circular_index=1)
+neutral_to_pickup_1 = np.array([2.396694, 0.508020])
+neutral_to_pickup_2 = np.array([2.874513, 0.933160])
pickup_pos = to_theta_with_circular_index_and_roll(0.6,
- 0.1,
- 0.0,
- circular_index=1)
-neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
+ 0.4,
+ np.pi / 2.0,
+ circular_index=0)
-neutral_to_score_1 = to_theta_with_circular_index(-0.4, 1.2, circular_index=1)
-neutral_to_score_2 = to_theta_with_circular_index(-0.7, 1.2, circular_index=1)
+neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.95, np.pi / 2.0)]
+
+neutral_to_score_1 = np.array([0.994244, -1.417442])
+neutral_to_score_2 = np.array([1.711325, -0.679748])
+
score_pos = to_theta_with_circular_index_and_roll(-1.0,
1.2,
- 0.0,
- circular_index=1)
-neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
+ np.pi / 2.0,
+ circular_index=0)
+neutral_to_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
# TODO(Max): Add real paths for arm.
points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index cf4fd7b..fef1083 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -409,12 +409,11 @@
self.DoDrawTo(cr, theta_version)
- def ToThetaPoints(self):
+ def VerifyPoints(self):
points = self.DoToThetaPoints()
if self.roll_joint_collision(points, verbose=True) or \
self.arm_past_limit(points, verbose=True):
sys.exit(1)
- return points
class SplineSegmentBase(Path):
diff --git a/y2023/control_loops/python/turret.py b/y2023/control_loops/python/turret.py
new file mode 100644
index 0000000..4e8f117
--- /dev/null
+++ b/y2023/control_loops/python/turret.py
@@ -0,0 +1,54 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kTurret = angular_system.AngularSystemParams(name='Turret',
+ motor=control_loop.Falcon(),
+ G=0.01,
+ J=3.1,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=25 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kTurret, R)
+ angular_system.PlotMotion(kTurret, R)
+ return
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2023', 'control_loops', 'superstructure', 'turret']
+ angular_system.WriteAngularSystem(kTurret, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 52254e4..d00489a 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -60,6 +60,25 @@
)
cc_library(
+ name = "end_effector",
+ srcs = [
+ "end_effector.cc",
+ ],
+ hdrs = [
+ "end_effector.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:control_loop",
+ "//y2023:constants",
+ ],
+)
+
+cc_library(
name = "superstructure_lib",
srcs = [
"superstructure.cc",
@@ -68,6 +87,7 @@
"superstructure.h",
],
deps = [
+ ":end_effector",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index 5417dc8..07d54ec 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -55,8 +55,8 @@
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
bool trajectory_override, double *proximal_output, double *distal_output,
- double *roll_joint_output, bool /*intake*/, bool /*spit*/,
- flatbuffers::FlatBufferBuilder *fbb) {
+ double *roll_joint_output, flatbuffers::FlatBufferBuilder *fbb) {
+ ::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
(roll_joint_output == nullptr));
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 9cda7fc..6e4cc29 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -47,8 +47,7 @@
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
bool trajectory_override, double *proximal_output, double *distal_output,
- double *roll_joint_output, bool /*intake*/, bool /*spit*/,
- flatbuffers::FlatBufferBuilder *fbb);
+ double *roll_joint_output, flatbuffers::FlatBufferBuilder *fbb);
void Reset();
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
new file mode 100644
index 0000000..658eb10
--- /dev/null
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -0,0 +1,81 @@
+#include "y2023/control_loops/superstructure/end_effector.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+
+EndEffector::EndEffector()
+ : state_(EndEffectorState::IDLE), beambreak_(false) {}
+
+EndEffectorState EndEffector::RunIteration(
+ const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
+ bool cone_beambreak, bool cube_beambreak, double *roller_voltage) {
+ bool beambreak = cone_beambreak || cube_beambreak;
+
+ *roller_voltage = 0.0;
+
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ // If idle and intake requested, intake
+ if (roller_goal == RollerGoal::INTAKE) {
+ state_ = EndEffectorState::INTAKING;
+ timer_ = timestamp;
+ }
+ break;
+ case EndEffectorState::INTAKING:
+ // If intaking and beam break is not triggered, keep intaking
+ if (beambreak) {
+ // Beam has been broken, switch to loaded.
+ state_ = EndEffectorState::LOADED;
+ break;
+ } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
+ // Intaking failed, waited 2 seconds with no beambreak
+ state_ = EndEffectorState::IDLE;
+ break;
+ }
+
+ *roller_voltage = constants::Values::kRollerVoltage();
+
+ break;
+ case EndEffectorState::LOADED:
+ // If loaded and beam break not triggered, intake
+ if (!beambreak) {
+ state_ = EndEffectorState::INTAKING;
+ timer_ = timestamp;
+ }
+ // If loaded and spit requested, spit
+ else if (roller_goal == RollerGoal::SPIT) {
+ state_ = EndEffectorState::SPITTING;
+ }
+ break;
+ case EndEffectorState::SPITTING:
+ // If spit requested, spit
+ *roller_voltage = -constants::Values::kRollerVoltage();
+ if (beambreak_) {
+ if (!beambreak) {
+ timer_ = timestamp;
+ }
+ } else if (timestamp > timer_ + constants::Values::kExtraSpittingTime()) {
+ // Finished spitting
+ state_ = EndEffectorState::IDLE;
+ }
+
+ break;
+ }
+
+ beambreak_ = beambreak;
+
+ return state_;
+}
+
+void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
new file mode 100644
index 0000000..16007de
--- /dev/null
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -0,0 +1,37 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "y2023/constants.h"
+#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+
+class EndEffector {
+ public:
+ EndEffector();
+ EndEffectorState RunIteration(
+ const ::aos::monotonic_clock::time_point timestamp,
+ RollerGoal roller_goal, bool cone_beambreak, bool cube_beambreak,
+ double *intake_roller_voltage);
+ EndEffectorState state() const { return state_; }
+ void Reset();
+
+ private:
+ EndEffectorState state_ = EndEffectorState::IDLE;
+ aos::monotonic_clock::time_point timer_ = aos::monotonic_clock::min_time;
+
+ bool beambreak_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 1ad2625..3b4d2b0 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -29,7 +29,9 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- arm_(values_) {}
+ arm_(values_),
+ end_effector_(),
+ wrist_(values->wrist.subsystem_params) {}
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
@@ -41,6 +43,7 @@
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
arm_.Reset();
+ end_effector_.Reset();
}
OutputT output_struct;
@@ -61,11 +64,20 @@
output != nullptr ? &output_struct.proximal_voltage : nullptr,
output != nullptr ? &output_struct.distal_voltage : nullptr,
output != nullptr ? &output_struct.roll_joint_voltage : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->intake() : false,
- unsafe_goal != nullptr ? unsafe_goal->spit() : false,
-
status->fbb());
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> wrist_offset =
+ wrist_.Iterate(unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
+ position->wrist(),
+ output != nullptr ? &output_struct.wrist_voltage : nullptr,
+ status->fbb());
+
+ EndEffectorState end_effector_state = end_effector_.RunIteration(
+ timestamp,
+ unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
+ position->end_effector_cone_beam_break(),
+ position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
@@ -74,6 +86,8 @@
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
status_builder.add_arm(arm_status_offset);
+ status_builder.add_wrist(wrist_offset);
+ status_builder.add_end_effector_state(end_effector_state);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index 2d0fc43..827cc40 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
+#include "y2023/control_loops/superstructure/end_effector.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
@@ -28,12 +29,21 @@
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
double robot_velocity() const;
+ inline const arm::Arm &arm() const { return arm_; }
+ inline const EndEffector &end_effector() const { return end_effector_; }
+ inline const AbsoluteEncoderSubsystem &wrist() const { return wrist_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -47,6 +57,8 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
arm::Arm arm_;
+ EndEffector end_effector_;
+ AbsoluteEncoderSubsystem wrist_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 936cbee..8f2d5ab 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,11 @@
namespace y2023.control_loops.superstructure;
+enum RollerGoal: ubyte {
+ IDLE = 0,
+ INTAKE = 1,
+ SPIT = 2,
+}
table Goal {
// Used to identify a position in the planned set of positions on the arm.
@@ -13,11 +18,7 @@
wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
- // If this is true, the rollers should intake.
- intake:bool (id: 3);
-
- // If this is true, the rollers should spit.
- spit:bool (id: 4);
+ roller_goal:RollerGoal (id: 3);
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index f3f0471..adfa2a5 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -43,6 +43,20 @@
using RelativeEncoderSimulator = frc971::control_loops::SubsystemSimulator<
frc971::control_loops::RelativeEncoderProfiledJointStatus,
RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+using AbsoluteEncoderSimulator = ::frc971::control_loops::SubsystemSimulator<
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus,
+ Superstructure::AbsoluteEncoderSubsystem::State,
+ constants::Values::AbsEncoderConstants>;
+
+enum GamePiece {
+ kCone,
+ kCube,
+};
class ArmSimulation {
public:
@@ -169,6 +183,14 @@
dt_(dt),
arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing,
values->roll_joint.zeroing, dt_),
+ wrist_(new CappedTestPlant(wrist::MakeWristPlant()),
+ PositionSensorSimulator(
+ values->wrist.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->wrist, constants::Values::kWristRange(),
+ values->wrist.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
@@ -189,6 +211,9 @@
superstructure_output_fetcher_->distal_voltage(),
superstructure_output_fetcher_->roll_joint_voltage())
.finished());
+
+ wrist_.Simulate(superstructure_output_fetcher_->wrist_voltage(),
+ superstructure_status_fetcher_->wrist());
}
first_ = false;
SendPositionMessage();
@@ -200,7 +225,8 @@
arm_.InitializePosition(position);
}
- const ArmSimulation &arm() const { return arm_; }
+ ArmSimulation *arm() { return &arm_; }
+ AbsoluteEncoderSimulator *wrist() { return &wrist_; }
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
@@ -210,18 +236,40 @@
flatbuffers::Offset<ArmPosition> arm_offset =
arm_.GetSensorValues(builder.fbb());
+ frc971::AbsolutePosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> wrist_offset =
+ wrist_.encoder()->GetSensorValues(&wrist_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_arm(arm_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_end_effector_cone_beam_break(
+ end_effector_cone_beam_break_);
+ position_builder.add_end_effector_cube_beam_break(
+ end_effector_cube_beam_break_);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
+ void set_end_effector_cone_beam_break(bool triggered) {
+ end_effector_cone_beam_break_ = triggered;
+ }
+
+ void set_end_effector_cube_beam_break(bool triggered) {
+ end_effector_cube_beam_break_ = triggered;
+ }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
ArmSimulation arm_;
+ AbsoluteEncoderSimulator wrist_;
+
+ bool end_effector_cone_beam_break_;
+ bool end_effector_cube_beam_break_;
::aos::Sender<Position> superstructure_position_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
@@ -309,18 +357,23 @@
superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
// Check that our simulator matches the status
- EXPECT_NEAR(superstructure_plant_.arm().proximal_position(),
+ EXPECT_NEAR(superstructure_plant_.arm()->proximal_position(),
superstructure_status_fetcher_->arm()->theta0(), kEpsTheta);
- EXPECT_NEAR(superstructure_plant_.arm().distal_position(),
+ EXPECT_NEAR(superstructure_plant_.arm()->distal_position(),
superstructure_status_fetcher_->arm()->theta1(), kEpsTheta);
- EXPECT_NEAR(superstructure_plant_.arm().roll_joint_position(),
+ EXPECT_NEAR(superstructure_plant_.arm()->roll_joint_position(),
superstructure_status_fetcher_->arm()->theta2(), kEpsTheta);
- EXPECT_NEAR(superstructure_plant_.arm().proximal_velocity(),
+ EXPECT_NEAR(superstructure_plant_.arm()->proximal_velocity(),
superstructure_status_fetcher_->arm()->omega0(), kEpsOmega);
- EXPECT_NEAR(superstructure_plant_.arm().distal_velocity(),
+ EXPECT_NEAR(superstructure_plant_.arm()->distal_velocity(),
superstructure_status_fetcher_->arm()->omega1(), kEpsOmega);
- EXPECT_NEAR(superstructure_plant_.arm().roll_joint_velocity(),
+ EXPECT_NEAR(superstructure_plant_.arm()->roll_joint_velocity(),
superstructure_status_fetcher_->arm()->omega2(), kEpsOmega);
+
+ if (superstructure_goal_fetcher_->has_wrist()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->wrist()->unsafe_goal(),
+ superstructure_status_fetcher_->wrist()->position(), 0.001);
+ }
}
void CheckIfZeroed() {
@@ -392,13 +445,20 @@
// still.
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
-
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().middle());
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_roller_goal(RollerGoal::IDLE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -411,13 +471,20 @@
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
SetEnabled(true);
-
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_roller_goal(RollerGoal::IDLE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -439,8 +506,14 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_wrist(wrist_offset);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(20));
@@ -451,11 +524,21 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_wrist(wrist_offset);
+
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.wrist()->set_peak_velocity(23.0);
+ superstructure_plant_.wrist()->set_peak_acceleration(0.2);
+
// TODO(Milo): Make this a sane time
RunFor(chrono::seconds(20));
VerifyNearGoal();
@@ -466,6 +549,10 @@
SetEnabled(true);
WaitUntilZeroed();
RunFor(chrono::seconds(2));
+
+ EXPECT_EQ(ArmState::RUNNING, superstructure_.arm().state());
+ EXPECT_EQ(Superstructure::AbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.wrist().state());
}
// Tests that running disabled works
@@ -474,6 +561,178 @@
CheckIfZeroed();
}
+class SuperstructureBeambreakTest
+ : public SuperstructureTest,
+ public ::testing::WithParamInterface<GamePiece> {
+ public:
+ void SetBeambreak(GamePiece game_piece, bool status) {
+ if (game_piece == GamePiece::kCone) {
+ superstructure_plant_.set_end_effector_cone_beam_break(status);
+ } else {
+ superstructure_plant_.set_end_effector_cube_beam_break(status);
+ }
+ }
+};
+
+TEST_P(SuperstructureBeambreakTest, EndEffectorGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_roller_goal(RollerGoal::INTAKE);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ // This makes sure that we intake as normal when
+ // requesting intake.
+ RunFor(constants::Values::kExtraIntakingTime());
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
+ constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::INTAKING);
+
+ SetBeambreak(GetParam(), true);
+
+ // Checking that after the beambreak is set once intaking that the
+ // state changes to LOADED.
+ RunFor(dt());
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::LOADED);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_roller_goal(RollerGoal::IDLE);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ SetBeambreak(GetParam(), false);
+ // Checking that it's going back to intaking because we lost the
+ // beambreak sensor.
+ RunFor(dt() * 2);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
+ constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::INTAKING);
+
+ // Checking that we go back to idle after beambreak is lost and we
+ // set our goal to idle.
+ RunFor(dt() * 2 + constants::Values::kExtraIntakingTime());
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::IDLE);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_roller_goal(RollerGoal::INTAKE);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ // Going through intake -> loaded -> spitting
+ // Making sure that it's intaking normally.
+ RunFor(constants::Values::kExtraIntakingTime());
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
+ constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::INTAKING);
+
+ SetBeambreak(GetParam(), true);
+
+ // Checking that it's loaded once beambreak is sensing something.
+ RunFor(dt());
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::LOADED);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_roller_goal(RollerGoal::SPIT);
+
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+
+ // Checking that it stays spitting until 2 seconds after the
+ // beambreak is lost.
+ RunFor(dt() * 10);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
+ -constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::SPITTING);
+
+ SetBeambreak(GetParam(), false);
+
+ RunFor(constants::Values::kExtraSpittingTime());
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
+ -constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::SPITTING);
+
+ // Checking that it goes to idle after it's given time to stop spitting.
+ RunFor(dt() * 3);
+
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::IDLE);
+}
+
// Tests that we don't freak out without a goal.
TEST_F(SuperstructureTest, ArmSimpleGoal) {
SetEnabled(true);
@@ -543,6 +802,9 @@
VerifyNearGoal();
}
+INSTANTIATE_TEST_SUITE_P(EndEffectorGoal, SuperstructureBeambreakTest,
+ ::testing::Values(GamePiece::kCone, GamePiece::kCube));
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index bd3d607..927fe10 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -24,6 +24,12 @@
// Zero for wrist is facing staright outward.
// Positive position would be upwards
wrist:frc971.AbsolutePosition (id: 1);
+
+ // If this is true, the cone beam break is triggered.
+ end_effector_cone_beam_break:bool (id: 2);
+
+ // If this is true, the cube beam break is triggered.
+ end_effector_cube_beam_break:bool (id: 3);
}
root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index db86687..2555134 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -57,6 +57,19 @@
failed_solutions:uint32 (id: 17);
}
+// State of the superstructure state machine
+enum EndEffectorState : ubyte {
+ // Not doing anything
+ IDLE = 0,
+ // Intaking the game object into the end effector
+ INTAKING = 1,
+ // The game object is loaded into the end effector
+ LOADED = 2,
+ // Waiting for the arm to be at shooting goal and then telling the
+ // end effector to spit
+ SPITTING = 3,
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -67,6 +80,8 @@
arm:ArmStatus (id: 2);
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
+
+ end_effector_state:EndEffectorState (id: 4);
}
root_type Status;
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 3caaa41..adeda2b 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -30,6 +30,7 @@
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
+using y2023::control_loops::superstructure::RollerGoal;
namespace y2023 {
namespace input {
@@ -66,16 +67,15 @@
return;
}
- bool intake = false;
- bool spit = false;
+ RollerGoal roller_goal = RollerGoal::IDLE;
// TODO(milind): add more actions and paths
if (data.IsPressed(kIntake)) {
- intake = true;
- arm_goal_position_ = arm::PickupPosIndex();
+ roller_goal = RollerGoal::INTAKE;
+ arm_goal_position_ = arm::ScorePosIndex();
} else if (data.IsPressed(kSpit)) {
- spit = true;
- arm_goal_position_ = arm::PickupPosIndex();
+ roller_goal = RollerGoal::SPIT;
+ arm_goal_position_ = arm::ScorePosIndex();
} else if (data.IsPressed(kScore)) {
arm_goal_position_ = arm::ScorePosIndex();
}
@@ -86,8 +86,7 @@
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
superstructure_goal_builder.add_arm_goal_position(arm_goal_position_);
- superstructure_goal_builder.add_intake(intake);
- superstructure_goal_builder.add_spit(spit);
+ superstructure_goal_builder.add_roller_goal(roller_goal);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
new file mode 100644
index 0000000..8f1237c
--- /dev/null
+++ b/y2023/localizer/BUILD
@@ -0,0 +1,101 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+flatbuffer_cc_library(
+ name = "status_fbs",
+ srcs = [
+ "status.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/imu_reader:imu_failures_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "status_ts_fbs",
+ srcs = ["status.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/imu_reader:imu_failures_ts_fbs",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "visualization_fbs",
+ srcs = [
+ "visualization.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":status_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "visualization_ts_fbs",
+ srcs = ["visualization.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":status_ts_fbs",
+ ],
+)
+
+cc_library(
+ name = "localizer",
+ srcs = ["localizer.cc"],
+ hdrs = ["localizer.h"],
+ deps = [
+ ":status_fbs",
+ ":visualization_fbs",
+ "//aos/containers:sized_array",
+ "//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:utils",
+ "//frc971/imu_reader:imu_watcher",
+ "//frc971/vision:target_map_fbs",
+ "//y2023:constants",
+ "//y2023/constants:constants_fbs",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = ["//y2023:aos_config"],
+ deps = [
+ ":localizer",
+ ":status_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2023/constants:simulated_constants_sender",
+ "//y2023/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+cc_binary(
+ name = "localizer_main",
+ srcs = ["localizer_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":localizer",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//y2023/control_loops/drivetrain:drivetrain_base",
+ ],
+)
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
new file mode 100644
index 0000000..1d78528
--- /dev/null
+++ b/y2023/localizer/localizer.cc
@@ -0,0 +1,429 @@
+#include "y2023/localizer/localizer.h"
+
+#include "aos/containers/sized_array.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "y2023/constants.h"
+
+namespace y2023::localizer {
+namespace {
+constexpr std::array<std::string_view, Localizer::kNumCameras> kPisToUse{
+ "pi1", "pi2", "pi3", "pi4"};
+
+size_t CameraIndexForName(std::string_view name) {
+ for (size_t index = 0; index < kPisToUse.size(); ++index) {
+ if (name == kPisToUse.at(index)) {
+ return index;
+ }
+ }
+ LOG(FATAL) << "No camera named " << name;
+}
+
+std::map<uint64_t, Localizer::Transform> GetTargetLocations(
+ const Constants &constants) {
+ CHECK(constants.has_target_map());
+ CHECK(constants.target_map()->has_target_poses());
+ std::map<uint64_t, Localizer::Transform> transforms;
+ for (const frc971::vision::TargetPoseFbs *target :
+ *constants.target_map()->target_poses()) {
+ CHECK(target->has_id());
+ CHECK(target->has_position());
+ CHECK(target->has_orientation());
+ CHECK_EQ(0u, transforms.count(target->id()));
+ transforms[target->id()] = PoseToTransform(target);
+ }
+ return transforms;
+}
+} // namespace
+
+Localizer::Transform PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose) {
+ const frc971::vision::Position *position = pose->position();
+ const frc971::vision::Quaternion *quaternion = pose->orientation();
+ return (Eigen::Translation3d(
+ Eigen::Vector3d(position->x(), position->y(), position->z())) *
+ Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
+ quaternion->z()))
+ .matrix();
+}
+
+std::array<Localizer::CameraState, Localizer::kNumCameras>
+Localizer::MakeCameras(const Constants &constants, aos::EventLoop *event_loop) {
+ CHECK(constants.has_cameras());
+ std::array<Localizer::CameraState, Localizer::kNumCameras> cameras;
+ for (const CameraConfiguration *camera : *constants.cameras()) {
+ CHECK(camera->has_calibration());
+ const frc971::vision::calibration::CameraCalibration *calibration =
+ camera->calibration();
+ CHECK(!calibration->has_turret_extrinsics())
+ << "The 2023 robot does not have a turret.";
+ CHECK(calibration->has_node_name());
+ const size_t index =
+ CameraIndexForName(calibration->node_name()->string_view());
+ // We default-construct the extrinsics matrix to all-zeros; use that to
+ // sanity-check whether we have populated the matrix yet or not.
+ CHECK(cameras.at(index).extrinsics.norm() == 0)
+ << "Got multiple calibrations for "
+ << calibration->node_name()->string_view();
+ CHECK(calibration->has_fixed_extrinsics());
+ cameras.at(index).extrinsics =
+ frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->fixed_extrinsics());
+ cameras.at(index).debug_sender = event_loop->MakeSender<Visualization>(
+ absl::StrCat("/", calibration->node_name()->string_view(), "/camera"));
+ }
+ for (const CameraState &camera : cameras) {
+ CHECK(camera.extrinsics.norm() != 0) << "Missing a camera calibration.";
+ }
+ return cameras;
+}
+
+Localizer::Localizer(
+ aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config)
+ : event_loop_(event_loop),
+ dt_config_(dt_config),
+ constants_fetcher_(event_loop),
+ cameras_(MakeCameras(constants_fetcher_.constants(), event_loop)),
+ target_poses_(GetTargetLocations(constants_fetcher_.constants())),
+ down_estimator_(dt_config),
+ ekf_(dt_config),
+ observations_(&ekf_),
+ imu_watcher_(event_loop, dt_config,
+ y2023::constants::Values::DrivetrainEncoderToMeters(1),
+ std::bind(&Localizer::HandleImu, this, std::placeholders::_1,
+ std::placeholders::_2, std::placeholders::_3,
+ std::placeholders::_4, std::placeholders::_5)),
+ utils_(event_loop),
+ status_sender_(event_loop->MakeSender<Status>("/localizer")),
+ output_sender_(event_loop->MakeSender<frc971::controls::LocalizerOutput>(
+ "/localizer")) {
+ if (dt_config_.is_simulated) {
+ down_estimator_.assume_perfect_gravity();
+ }
+
+ for (size_t camera_index = 0; camera_index < kNumCameras; ++camera_index) {
+ const std::string_view pi_name = kPisToUse.at(camera_index);
+ event_loop->MakeWatcher(
+ absl::StrCat("/", pi_name, "/camera"),
+ [this, pi_name,
+ camera_index](const frc971::vision::TargetMap &targets) {
+ CHECK(targets.has_target_poses());
+ CHECK(targets.has_monotonic_timestamp_ns());
+ const std::optional<aos::monotonic_clock::duration> clock_offset =
+ utils_.ClockOffset(pi_name);
+ if (!clock_offset.has_value()) {
+ VLOG(1) << "Rejecting image due to disconnected message bridge at "
+ << event_loop_->monotonic_now();
+ cameras_.at(camera_index)
+ .rejection_counter.IncrementError(
+ RejectionReason::MESSAGE_BRIDGE_DISCONNECTED);
+ return;
+ }
+ const aos::monotonic_clock::time_point pi_capture_time(
+ std::chrono::nanoseconds(targets.monotonic_timestamp_ns()) -
+ clock_offset.value());
+ const aos::monotonic_clock::time_point capture_time =
+ pi_capture_time - imu_watcher_.pico_offset_error();
+ VLOG(2) << "Capture time of "
+ << targets.monotonic_timestamp_ns() * 1e-9
+ << " clock offset of "
+ << aos::time::DurationInSeconds(clock_offset.value())
+ << " pico error "
+ << aos::time::DurationInSeconds(
+ imu_watcher_.pico_offset_error());
+ if (pi_capture_time > event_loop_->context().monotonic_event_time) {
+ VLOG(1) << "Rejecting image due to being from future at "
+ << event_loop_->monotonic_now() << " with timestamp of "
+ << pi_capture_time << " and event time pf "
+ << event_loop_->context().monotonic_event_time;
+ cameras_.at(camera_index)
+ .rejection_counter.IncrementError(
+ RejectionReason::IMAGE_FROM_FUTURE);
+ return;
+ }
+ auto builder = cameras_.at(camera_index).debug_sender.MakeBuilder();
+ aos::SizedArray<flatbuffers::Offset<TargetEstimateDebug>, 20>
+ debug_offsets;
+ for (const frc971::vision::TargetPoseFbs *target :
+ *targets.target_poses()) {
+ VLOG(1) << "Handling target from " << camera_index;
+ auto offset = HandleTarget(camera_index, capture_time, *target,
+ builder.fbb());
+ if (debug_offsets.size() < debug_offsets.capacity()) {
+ debug_offsets.push_back(offset);
+ } else {
+ AOS_LOG(ERROR, "Dropped message from debug vector.");
+ }
+ }
+ auto vector_offset = builder.fbb()->CreateVector(
+ debug_offsets.data(), debug_offsets.size());
+ auto stats_offset =
+ StatisticsForCamera(cameras_.at(camera_index), builder.fbb());
+ Visualization::Builder visualize_builder(*builder.fbb());
+ visualize_builder.add_targets(vector_offset);
+ visualize_builder.add_statistics(stats_offset);
+ builder.CheckOk(builder.Send(visualize_builder.Finish()));
+ SendStatus();
+ });
+ }
+
+ event_loop_->AddPhasedLoop([this](int) { SendOutput(); },
+ std::chrono::milliseconds(5));
+
+ event_loop_->MakeWatcher(
+ "/drivetrain",
+ [this](
+ const frc971::control_loops::drivetrain::LocalizerControl &control) {
+ const double theta = control.keep_current_theta()
+ ? ekf_.X_hat(StateIdx::kTheta)
+ : control.theta();
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
+ ekf_.ResetInitialState(
+ t_,
+ (HybridEkf::State() << control.x(), control.y(), theta,
+ left_encoder, 0, right_encoder, 0, 0, 0, 0, 0, 0)
+ .finished(),
+ ekf_.P());
+ });
+
+ ekf_.set_ignore_accel(true);
+ // Priority should be lower than the imu reading process, but non-zero.
+ event_loop->SetRuntimeRealtimePriority(10);
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(),
+ HybridEkf::State::Zero(), ekf_.P());
+ });
+}
+
+void Localizer::HandleImu(aos::monotonic_clock::time_point sample_time_pico,
+ aos::monotonic_clock::time_point sample_time_pi,
+ std::optional<Eigen::Vector2d> encoders,
+ Eigen::Vector3d gyro, Eigen::Vector3d accel) {
+ last_encoder_readings_ = encoders;
+ // Ignore ivnalid readings; the HybridEkf will handle it reasonably.
+ if (!encoders.has_value()) {
+ return;
+ }
+ if (t_ == aos::monotonic_clock::min_time) {
+ t_ = sample_time_pico;
+ }
+ if (t_ + 10 * frc971::controls::ImuWatcher::kNominalDt < sample_time_pico) {
+ t_ = sample_time_pico;
+ ++clock_resets_;
+ }
+ const aos::monotonic_clock::duration dt = sample_time_pico - t_;
+ t_ = sample_time_pico;
+ // We don't actually use the down estimator currently, but it's really
+ // convenient for debugging.
+ down_estimator_.Predict(gyro, accel, dt);
+ const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
+ ekf_.UpdateEncodersAndGyro(encoders.value()(0), encoders.value()(1), yaw_rate,
+ utils_.VoltageOrZero(sample_time_pi), accel, t_);
+ SendStatus();
+}
+
+flatbuffers::Offset<TargetEstimateDebug> Localizer::RejectImage(
+ int camera_index, RejectionReason reason,
+ TargetEstimateDebug::Builder *builder) {
+ builder->add_accepted(false);
+ builder->add_rejection_reason(reason);
+ cameras_.at(camera_index).rejection_counter.IncrementError(reason);
+ return builder->Finish();
+}
+
+flatbuffers::Offset<TargetEstimateDebug> Localizer::HandleTarget(
+ int camera_index, const aos::monotonic_clock::time_point capture_time,
+ const frc971::vision::TargetPoseFbs &target,
+ flatbuffers::FlatBufferBuilder *debug_fbb) {
+ ++total_candidate_targets_;
+ ++cameras_.at(camera_index).total_candidate_targets;
+
+ TargetEstimateDebug::Builder builder(*debug_fbb);
+ builder.add_camera(camera_index);
+ builder.add_image_age_sec(aos::time::DurationInSeconds(
+ event_loop_->monotonic_now() - capture_time));
+
+ const uint64_t target_id = target.id();
+ VLOG(2) << aos::FlatbufferToJson(&target);
+ if (target_poses_.count(target_id) == 0) {
+ VLOG(1) << "Rejecting target due to invalid ID " << target_id;
+ return RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, &builder);
+ }
+
+ const Transform &H_field_target = target_poses_.at(target_id);
+ const Transform &H_robot_camera = cameras_.at(camera_index).extrinsics;
+
+ const Transform H_camera_target = PoseToTransform(&target);
+
+ const Transform H_field_camera = H_field_target * H_camera_target.inverse();
+ // Back out the robot position that is implied by the current camera
+ // 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 frc971::control_loops::Pose measured_camera_pose(H_field_camera);
+ builder.add_camera_x(measured_camera_pose.rel_pos().x());
+ builder.add_camera_y(measured_camera_pose.rel_pos().y());
+ // Because the camera uses Z as forwards rather than X, just calculate the
+ // debugging theta value using the transformation matrix directly.
+ builder.add_camera_theta(
+ std::atan2(H_field_camera(1, 2), H_field_camera(0, 2)));
+ // Calculate the camera-to-robot transformation matrix ignoring the
+ // pitch/roll of the camera.
+ const Transform H_camera_robot_stripped =
+ frc971::control_loops::Pose(H_robot_camera)
+ .AsTransformationMatrix()
+ .inverse();
+ const frc971::control_loops::Pose measured_pose(
+ measured_camera_pose.AsTransformationMatrix() * H_camera_robot_stripped);
+ // This "Z" is the robot pose directly implied by the camera results.
+ const Eigen::Matrix<double, 3, 1> Z(measured_pose.rel_pos().x(),
+ measured_pose.rel_pos().y(),
+ measured_pose.rel_theta());
+ builder.add_implied_robot_x(Z(Corrector::kX));
+ builder.add_implied_robot_y(Z(Corrector::kY));
+ builder.add_implied_robot_theta(Z(Corrector::kTheta));
+
+ // TODO(james): Tune this. Also, gain schedule for auto mode?
+ Eigen::Matrix<double, 3, 1> noises(1.0, 1.0, 0.5);
+
+ Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+ R.diagonal() = noises.cwiseAbs2();
+ // In order to do the EKF correction, we determine the expected state based
+ // on the state at the time the image was captured; however, we insert the
+ // correction update itself at the current time. This is technically not
+ // quite correct, but saves substantial CPU usage & code complexity by
+ // making
+ // it so that we don't have to constantly rewind the entire EKF history.
+ const std::optional<State> state_at_capture =
+ ekf_.LastStateBeforeTime(capture_time);
+
+ if (!state_at_capture.has_value()) {
+ VLOG(1) << "Rejecting image due to being too old.";
+ return RejectImage(camera_index, RejectionReason::IMAGE_TOO_OLD, &builder);
+ }
+
+ const Input U = ekf_.MostRecentInput();
+ VLOG(1) << "previous state " << ekf_.X_hat().topRows<3>().transpose();
+ // For the correction step, instead of passing in the measurement directly,
+ // we pass in (0, 0, 0) as the measurement and then for the expected
+ // measurement (Zhat) we calculate the error between the pose implied by
+ // 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(), Z), R, t_);
+ ++total_accepted_targets_;
+ ++cameras_.at(camera_index).total_accepted_targets;
+ VLOG(1) << "new state " << ekf_.X_hat().topRows<3>().transpose();
+ return builder.Finish();
+}
+
+Localizer::Output Localizer::Corrector::H(const State &, const Input &) {
+ CHECK(Z_.allFinite());
+ Eigen::Vector3d Zhat = H_ * state_at_capture_ - Z_;
+ // Rewrap angle difference to put it back in range.
+ Zhat(2) = aos::math::NormalizeAngle(Zhat(2));
+ VLOG(1) << "Zhat " << Zhat.transpose() << " Z_ " << Z_.transpose()
+ << " state " << (H_ * state_at_capture_).transpose();
+ return Zhat;
+}
+
+void Localizer::SendOutput() {
+ auto builder = output_sender_.MakeBuilder();
+ frc971::controls::LocalizerOutput::Builder output_builder =
+ builder.MakeBuilder<frc971::controls::LocalizerOutput>();
+ output_builder.add_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ event_loop_->context().monotonic_event_time.time_since_epoch())
+ .count());
+ output_builder.add_x(ekf_.X_hat(StateIdx::kX));
+ output_builder.add_y(ekf_.X_hat(StateIdx::kY));
+ output_builder.add_theta(ekf_.X_hat(StateIdx::kTheta));
+ output_builder.add_zeroed(imu_watcher_.zeroer().Zeroed());
+ output_builder.add_image_accepted_count(total_accepted_targets_);
+ const Eigen::Quaterniond &orientation =
+ Eigen::AngleAxis<double>(ekf_.X_hat(StateIdx::kTheta),
+ Eigen::Vector3d::UnitZ()) *
+ down_estimator_.X_hat();
+ frc971::controls::Quaternion quaternion;
+ quaternion.mutate_x(orientation.x());
+ quaternion.mutate_y(orientation.y());
+ quaternion.mutate_z(orientation.z());
+ quaternion.mutate_w(orientation.w());
+ output_builder.add_orientation(&quaternion);
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+}
+
+flatbuffers::Offset<frc971::control_loops::drivetrain::LocalizerState>
+Localizer::PopulateState(flatbuffers::FlatBufferBuilder *fbb) const {
+ frc971::control_loops::drivetrain::LocalizerState::Builder builder(*fbb);
+ builder.add_x(ekf_.X_hat(StateIdx::kX));
+ builder.add_y(ekf_.X_hat(StateIdx::kY));
+ builder.add_theta(ekf_.X_hat(StateIdx::kTheta));
+ builder.add_left_velocity(ekf_.X_hat(StateIdx::kLeftVelocity));
+ builder.add_right_velocity(ekf_.X_hat(StateIdx::kRightVelocity));
+ builder.add_left_encoder(ekf_.X_hat(StateIdx::kLeftEncoder));
+ builder.add_right_encoder(ekf_.X_hat(StateIdx::kRightEncoder));
+ builder.add_left_voltage_error(ekf_.X_hat(StateIdx::kLeftVoltageError));
+ builder.add_right_voltage_error(ekf_.X_hat(StateIdx::kRightVoltageError));
+ builder.add_angular_error(ekf_.X_hat(StateIdx::kAngularError));
+ builder.add_longitudinal_velocity_offset(
+ ekf_.X_hat(StateIdx::kLongitudinalVelocityOffset));
+ builder.add_lateral_velocity(ekf_.X_hat(StateIdx::kLateralVelocity));
+ return builder.Finish();
+}
+
+flatbuffers::Offset<ImuStatus> Localizer::PopulateImu(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ const auto zeroer_offset = imu_watcher_.zeroer().PopulateStatus(fbb);
+ const auto failures_offset = imu_watcher_.PopulateImuFailures(fbb);
+ ImuStatus::Builder builder(*fbb);
+ builder.add_zeroed(imu_watcher_.zeroer().Zeroed());
+ builder.add_faulted_zero(imu_watcher_.zeroer().Faulted());
+ builder.add_zeroing(zeroer_offset);
+ if (imu_watcher_.pico_offset().has_value()) {
+ builder.add_pico_offset_ns(imu_watcher_.pico_offset().value().count());
+ builder.add_pico_offset_error_ns(imu_watcher_.pico_offset_error().count());
+ }
+ if (last_encoder_readings_.has_value()) {
+ builder.add_left_encoder(last_encoder_readings_.value()(0));
+ builder.add_right_encoder(last_encoder_readings_.value()(1));
+ }
+ builder.add_imu_failures(failures_offset);
+ return builder.Finish();
+}
+
+flatbuffers::Offset<CumulativeStatistics> Localizer::StatisticsForCamera(
+ const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb) {
+ const auto counts_offset = camera.rejection_counter.PopulateCounts(fbb);
+ CumulativeStatistics::Builder stats_builder(*fbb);
+ stats_builder.add_total_accepted(camera.total_accepted_targets);
+ stats_builder.add_total_candidates(camera.total_candidate_targets);
+ stats_builder.add_rejection_reasons(counts_offset);
+ return stats_builder.Finish();
+}
+
+void Localizer::SendStatus() {
+ auto builder = status_sender_.MakeBuilder();
+ std::array<flatbuffers::Offset<CumulativeStatistics>, kNumCameras>
+ stats_offsets;
+ for (size_t ii = 0; ii < kNumCameras; ++ii) {
+ stats_offsets.at(ii) = StatisticsForCamera(cameras_.at(ii), builder.fbb());
+ }
+ auto stats_offset =
+ builder.fbb()->CreateVector(stats_offsets.data(), stats_offsets.size());
+ auto down_estimator_offset =
+ down_estimator_.PopulateStatus(builder.fbb(), t_);
+ auto imu_offset = PopulateImu(builder.fbb());
+ auto state_offset = PopulateState(builder.fbb());
+ Status::Builder status_builder = builder.MakeBuilder<Status>();
+ status_builder.add_state(state_offset);
+ status_builder.add_down_estimator(down_estimator_offset);
+ status_builder.add_imu(imu_offset);
+ status_builder.add_statistics(stats_offset);
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+}
+
+} // namespace y2023::localizer
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
new file mode 100644
index 0000000..8c53467
--- /dev/null
+++ b/y2023/localizer/localizer.h
@@ -0,0 +1,123 @@
+#ifndef Y2023_LOCALIZER_LOCALIZER_H_
+#define Y2023_LOCALIZER_LOCALIZER_H_
+
+#include <array>
+#include <map>
+
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
+#include "frc971/imu_reader/imu_watcher.h"
+#include "frc971/vision/target_map_generated.h"
+#include "y2023/constants/constants_generated.h"
+#include "y2023/localizer/status_generated.h"
+#include "y2023/localizer/visualization_generated.h"
+
+namespace y2023::localizer {
+
+class Localizer {
+ public:
+ static constexpr size_t kNumCameras = 4;
+ typedef Eigen::Matrix<double, 4, 4> Transform;
+ typedef frc971::control_loops::drivetrain::HybridEkf<double> HybridEkf;
+ typedef HybridEkf::State State;
+ typedef HybridEkf::Output Output;
+ typedef HybridEkf::Input Input;
+ typedef HybridEkf::StateIdx StateIdx;
+ Localizer(aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ dt_config);
+
+ private:
+ class Corrector : public HybridEkf::ExpectedObservationFunctor {
+ public:
+ // Indices used for each of the members of the output vector for this
+ // Corrector.
+ enum OutputIdx {
+ kX = 0,
+ kY = 1,
+ kTheta = 2,
+ };
+ Corrector(const State &state_at_capture, const Eigen::Vector3d &Z)
+ : state_at_capture_(state_at_capture), Z_(Z) {
+ H_.setZero();
+ H_(kX, StateIdx::kX) = 1;
+ H_(kY, StateIdx::kY) = 1;
+ H_(kTheta, StateIdx::kTheta) = 1;
+ }
+ Output H(const State &, const Input &) final;
+ Eigen::Matrix<double, HybridEkf::kNOutputs, HybridEkf::kNStates> DHDX(
+ const State &) final {
+ return H_;
+ }
+
+ private:
+ Eigen::Matrix<double, HybridEkf::kNOutputs, HybridEkf::kNStates> H_;
+ const State state_at_capture_;
+ const Eigen::Vector3d &Z_;
+ };
+
+ struct CameraState {
+ aos::Sender<Visualization> debug_sender;
+ Transform extrinsics = Transform::Zero();
+ aos::util::ArrayErrorCounter<RejectionReason, RejectionCount>
+ rejection_counter;
+ size_t total_candidate_targets = 0;
+ size_t total_accepted_targets = 0;
+ };
+
+ static std::array<CameraState, kNumCameras> MakeCameras(
+ const Constants &constants, aos::EventLoop *event_loop);
+ flatbuffers::Offset<TargetEstimateDebug> HandleTarget(
+ int camera_index, const aos::monotonic_clock::time_point capture_time,
+ const frc971::vision::TargetPoseFbs &target,
+ flatbuffers::FlatBufferBuilder *debug_fbb);
+ void HandleImu(aos::monotonic_clock::time_point sample_time_pico,
+ aos::monotonic_clock::time_point sample_time_pi,
+ std::optional<Eigen::Vector2d> encoders, Eigen::Vector3d gyro,
+ Eigen::Vector3d accel);
+ flatbuffers::Offset<TargetEstimateDebug> RejectImage(
+ int camera_index, RejectionReason reason,
+ TargetEstimateDebug::Builder *builder);
+
+ void SendOutput();
+ flatbuffers::Offset<frc971::control_loops::drivetrain::LocalizerState>
+ PopulateState(flatbuffers::FlatBufferBuilder *fbb) const;
+ flatbuffers::Offset<ImuStatus> PopulateImu(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+ void SendStatus();
+ static flatbuffers::Offset<CumulativeStatistics> StatisticsForCamera(
+ const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb);
+
+ aos::EventLoop *const event_loop_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ std::array<CameraState, kNumCameras> cameras_;
+ const std::array<Transform, kNumCameras> camera_extrinsics_;
+ const std::map<uint64_t, Transform> target_poses_;
+
+ frc971::control_loops::drivetrain::DrivetrainUkf down_estimator_;
+ HybridEkf ekf_;
+ HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
+
+ frc971::controls::ImuWatcher imu_watcher_;
+ frc971::control_loops::drivetrain::LocalizationUtils utils_;
+
+ aos::Sender<Status> status_sender_;
+ aos::Sender<frc971::controls::LocalizerOutput> output_sender_;
+ aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
+ size_t clock_resets_ = 0;
+
+ size_t total_candidate_targets_ = 0;
+ size_t total_accepted_targets_ = 0;
+
+ // For the status message.
+ std::optional<Eigen::Vector2d> last_encoder_readings_;
+};
+
+// Converts a TargetPoseFbs into a transformation matrix.
+Localizer::Transform PoseToTransform(const frc971::vision::TargetPoseFbs *pose);
+} // namespace y2023::localizer
+#endif // Y2023_LOCALIZER_LOCALIZER_H_
diff --git a/y2023/localizer/localizer_main.cc b/y2023/localizer/localizer_main.cc
new file mode 100644
index 0000000..98f0e81
--- /dev/null
+++ b/y2023/localizer/localizer_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+#include "y2023/localizer/localizer.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<y2023::Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+ y2023::localizer::Localizer localizer(
+ &event_loop, ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
new file mode 100644
index 0000000..77af69b
--- /dev/null
+++ b/y2023/localizer/localizer_test.cc
@@ -0,0 +1,459 @@
+#include "y2023/localizer/localizer.h"
+
+#include "aos/events/logging/log_writer.h"
+#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/pose.h"
+#include "frc971/vision/target_map_generated.h"
+#include "gtest/gtest.h"
+#include "y2023/constants/simulated_constants_sender.h"
+#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+#include "y2023/localizer/status_generated.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+DECLARE_bool(die_on_malloc);
+
+namespace y2023::localizer::testing {
+
+using frc971::control_loops::drivetrain::Output;
+
+class LocalizerTest : public ::testing::Test {
+ protected:
+ static constexpr uint64_t kTargetId = 1;
+ LocalizerTest()
+ : configuration_(aos::configuration::ReadConfig("y2023/aos_config.json")),
+ event_loop_factory_(&configuration_.message()),
+ roborio_node_([this]() {
+ // Get the constants sent before anything else happens.
+ // It has nothing to do with the roborio node.
+ SendSimulationConstants(&event_loop_factory_, 7971,
+ "y2023/constants/test_constants.json");
+ return aos::configuration::GetNode(&configuration_.message(),
+ "roborio");
+ }()),
+ imu_node_(
+ aos::configuration::GetNode(&configuration_.message(), "imu")),
+ camera_node_(
+ aos::configuration::GetNode(&configuration_.message(), "pi1")),
+ dt_config_(frc971::control_loops::drivetrain::testing::
+ GetTestDrivetrainConfig()),
+ localizer_event_loop_(
+ event_loop_factory_.MakeEventLoop("localizer", imu_node_)),
+ localizer_(localizer_event_loop_.get(), dt_config_),
+ drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop(
+ "drivetrain_plant", roborio_node_)),
+ drivetrain_plant_imu_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(),
+ drivetrain_plant_imu_event_loop_.get(), dt_config_,
+ std::chrono::microseconds(500)),
+ roborio_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", roborio_node_)),
+ imu_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", imu_node_)),
+ camera_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", camera_node_)),
+ logger_test_event_loop_(
+ event_loop_factory_.GetNodeEventLoopFactory("logger")
+ ->MakeEventLoop("test")),
+ constants_fetcher_(imu_test_event_loop_.get()),
+ output_sender_(
+ roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
+ target_sender_(
+ camera_test_event_loop_->MakeSender<frc971::vision::TargetMap>(
+ "/camera")),
+ control_sender_(roborio_test_event_loop_->MakeSender<
+ frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")),
+ output_fetcher_(
+ roborio_test_event_loop_
+ ->MakeFetcher<frc971::controls::LocalizerOutput>("/localizer")),
+ status_fetcher_(
+ imu_test_event_loop_->MakeFetcher<Status>("/localizer")) {
+ FLAGS_die_on_malloc = true;
+ {
+ aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
+ {
+ auto builder = output_sender_.MakeBuilder();
+ auto output_builder = builder.MakeBuilder<Output>();
+ output_builder.add_left_voltage(output_voltages_(0));
+ output_builder.add_right_voltage(output_voltages_(1));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ }
+ });
+ roborio_test_event_loop_->OnRun([timer, this]() {
+ timer->Setup(roborio_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(5));
+ });
+ }
+ {
+ // Sanity check that the test calibration files look like what we
+ // expect.
+ CHECK_EQ("pi1", constants_fetcher_.constants()
+ .cameras()
+ ->Get(0)
+ ->calibration()
+ ->node_name()
+ ->string_view());
+ const Eigen::Matrix<double, 4, 4> H_robot_camera =
+ frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *constants_fetcher_.constants()
+ .cameras()
+ ->Get(0)
+ ->calibration()
+ ->fixed_extrinsics());
+
+ CHECK_EQ(kTargetId, constants_fetcher_.constants()
+ .target_map()
+ ->target_poses()
+ ->Get(0)
+ ->id());
+ const Eigen::Matrix<double, 4, 4> H_field_target = PoseToTransform(
+ constants_fetcher_.constants().target_map()->target_poses()->Get(0));
+ // For reference, the camera should pointed straight forwards on the
+ // robot, offset by 1 meter.
+ aos::TimerHandler *timer = camera_test_event_loop_->AddTimer(
+ [this, H_robot_camera, H_field_target]() {
+ if (!send_targets_) {
+ return;
+ }
+ const frc971::control_loops::Pose robot_pose(
+ {drivetrain_plant_.GetPosition().x(),
+ drivetrain_plant_.GetPosition().y(), 0.0},
+ drivetrain_plant_.state()(2, 0));
+
+ const Eigen::Matrix<double, 4, 4> H_field_camera =
+ robot_pose.AsTransformationMatrix() * H_robot_camera;
+ const Eigen::Matrix<double, 4, 4> H_camera_target =
+ H_field_camera.inverse() * H_field_target;
+
+ const Eigen::Quaterniond quat(H_camera_target.block<3, 3>(0, 0));
+ const Eigen::Vector3d translation(
+ H_camera_target.block<3, 1>(0, 3));
+
+ auto builder = target_sender_.MakeBuilder();
+ frc971::vision::Quaternion::Builder quat_builder(*builder.fbb());
+ quat_builder.add_w(quat.w());
+ quat_builder.add_x(quat.x());
+ quat_builder.add_y(quat.y());
+ quat_builder.add_z(quat.z());
+ auto quat_offset = quat_builder.Finish();
+ frc971::vision::Position::Builder position_builder(*builder.fbb());
+ position_builder.add_x(translation.x());
+ position_builder.add_y(translation.y());
+ position_builder.add_z(translation.z());
+ auto position_offset = position_builder.Finish();
+
+ frc971::vision::TargetPoseFbs::Builder target_builder(
+ *builder.fbb());
+ target_builder.add_id(send_target_id_);
+ target_builder.add_position(position_offset);
+ target_builder.add_orientation(quat_offset);
+ auto target_offset = target_builder.Finish();
+
+ auto targets_offset = builder.fbb()->CreateVector({target_offset});
+ frc971::vision::TargetMap::Builder map_builder(*builder.fbb());
+ map_builder.add_target_poses(targets_offset);
+ map_builder.add_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ camera_test_event_loop_->monotonic_now().time_since_epoch())
+ .count());
+
+ builder.CheckOk(builder.Send(map_builder.Finish()));
+ });
+ camera_test_event_loop_->OnRun([timer, this]() {
+ timer->Setup(camera_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(50));
+ });
+ }
+
+ localizer_control_send_timer_ =
+ roborio_test_event_loop_->AddTimer([this]() {
+ auto builder = control_sender_.MakeBuilder();
+ auto control_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerControl>();
+ control_builder.add_x(localizer_control_x_);
+ control_builder.add_y(localizer_control_y_);
+ control_builder.add_theta(localizer_control_theta_);
+ control_builder.add_theta_uncertainty(0.01);
+ control_builder.add_keep_current_theta(false);
+ builder.CheckOk(builder.Send(control_builder.Finish()));
+ });
+
+ // Get things zeroed.
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(status_fetcher_.Fetch());
+ CHECK(status_fetcher_->imu()->zeroed());
+
+ if (!FLAGS_output_folder.empty()) {
+ logger_event_loop_ =
+ event_loop_factory_.MakeEventLoop("logger", imu_node_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingOnRun(FLAGS_output_folder);
+ }
+ }
+
+ void SendLocalizerControl(double x, double y, double theta) {
+ localizer_control_x_ = x;
+ localizer_control_y_ = y;
+ localizer_control_theta_ = theta;
+ localizer_control_send_timer_->Setup(
+ roborio_test_event_loop_->monotonic_now());
+ }
+ ::testing::AssertionResult IsNear(double expected, double actual,
+ double epsilon) {
+ if (std::abs(expected - actual) < epsilon) {
+ return ::testing::AssertionSuccess();
+ } else {
+ return ::testing::AssertionFailure()
+ << "Expected " << expected << " but got " << actual
+ << " with a max difference of " << epsilon
+ << " and an actual difference of " << std::abs(expected - actual);
+ }
+ }
+ ::testing::AssertionResult VerifyEstimatorAccurate(double eps) {
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
+ ::testing::AssertionResult result(true);
+ status_fetcher_.Fetch();
+ if (!(result = IsNear(status_fetcher_->state()->x(), true_state(0), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(status_fetcher_->state()->y(), true_state(1), eps))) {
+ return result;
+ }
+ if (!(result =
+ IsNear(status_fetcher_->state()->theta(), true_state(2), eps))) {
+ return result;
+ }
+ return result;
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_node_;
+ const aos::Node *const imu_node_;
+ const aos::Node *const camera_node_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ std::unique_ptr<aos::EventLoop> localizer_event_loop_;
+ Localizer localizer_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
+ frc971::control_loops::drivetrain::testing::DrivetrainSimulation
+ drivetrain_plant_;
+
+ std::unique_ptr<aos::EventLoop> roborio_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> camera_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> logger_test_event_loop_;
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+
+ aos::Sender<Output> output_sender_;
+ aos::Sender<frc971::vision::TargetMap> target_sender_;
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+ control_sender_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> output_fetcher_;
+ aos::Fetcher<Status> status_fetcher_;
+
+ Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero();
+
+ aos::TimerHandler *localizer_control_send_timer_;
+
+ bool send_targets_ = false;
+
+ double localizer_control_x_ = 0.0;
+ double localizer_control_y_ = 0.0;
+ double localizer_control_theta_ = 0.0;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+
+ uint64_t send_target_id_ = kTargetId;
+
+ gflags::FlagSaver flag_saver_;
+};
+
+// Test a simple scenario with no errors where the robot should just drive
+// straight forwards.
+TEST_F(LocalizerTest, Nominal) {
+ output_voltages_ << 1.0, 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-6);
+ // Confirm that we did indeed drive forwards (and straight), as expected.
+ EXPECT_LT(0.1, output_fetcher_->x());
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Confirm that when the robot drives backwards that we localize correctly.
+TEST_F(LocalizerTest, NominalReverse) {
+ output_voltages_ << -1.0, -1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-6);
+ // Confirm that we did indeed drive backwards (and straight), as expected.
+ EXPECT_GT(-0.1, output_fetcher_->x());
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Confirm that when the robot turns counter-clockwise that we localize
+// correctly.
+TEST_F(LocalizerTest, NominalSpinInPlace) {
+ output_voltages_ << -1.0, 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-2);
+ // Confirm that we did indeed turn counter-clockwise.
+ EXPECT_NEAR(0.0, output_fetcher_->x(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_LT(0.1, output_fetcher_->theta());
+ EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Confirm that when the robot drives in a curve that we localize
+// successfully.
+TEST_F(LocalizerTest, NominalCurve) {
+ output_voltages_ << 2.0, 3.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-2);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-2);
+ // Confirm that we did indeed drive in a rough, counter-clockwise, curve.
+ EXPECT_LT(0.1, output_fetcher_->x());
+ EXPECT_LT(0.1, output_fetcher_->y());
+ EXPECT_LT(0.1, output_fetcher_->theta());
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Tests that, in the presence of a non-zero voltage error, that we correct
+// for it.
+TEST_F(LocalizerTest, VoltageErrorDisabled) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.set_left_voltage_offset(2.0);
+ drivetrain_plant_.set_right_voltage_offset(2.0);
+
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // We should've just ended up driving straight forwards.
+ EXPECT_LT(0.1, output_fetcher_->x());
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ EXPECT_NEAR(2.0, status_fetcher_->state()->left_voltage_error(), 1.0);
+ EXPECT_NEAR(2.0, status_fetcher_->state()->right_voltage_error(), 1.0);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.05));
+}
+
+// Tests that image corrections in the nominal case (no errors) causes no
+// issues.
+TEST_F(LocalizerTest, NominalImageCorrections) {
+ output_voltages_ << 3.0, 2.0;
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(status_fetcher_->statistics()->Get(0)->total_candidates(),
+ status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that image corrections when there is an error at the start results
+// in us actually getting corrected over time.
+TEST_F(LocalizerTest, ImageCorrections) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.mutable_state()->x() = 2.0;
+ drivetrain_plant_.mutable_state()->y() = 2.0;
+ SendLocalizerControl(5.0, 3.0, 0.0);
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(output_fetcher_.Fetch());
+ ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5);
+ ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
+
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(status_fetcher_->statistics()->Get(0)->total_candidates(),
+ status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that we correctly reject an invalid target.
+TEST_F(LocalizerTest, InvalidTargetId) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ send_target_id_ = 100;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+ ASSERT_EQ(status_fetcher_->statistics()
+ ->Get(0)
+ ->rejection_reasons()
+ ->Get(static_cast<size_t>(RejectionReason::NO_SUCH_TARGET))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+} // namespace y2023::localizer::testing
diff --git a/y2023/localizer/status.fbs b/y2023/localizer/status.fbs
new file mode 100644
index 0000000..4d0a9c1
--- /dev/null
+++ b/y2023/localizer/status.fbs
@@ -0,0 +1,59 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+include "frc971/imu_reader/imu_failures.fbs";
+
+namespace y2023.localizer;
+
+enum RejectionReason : uint8 {
+ // For some reason, the image timestamp indicates that the image was taken
+ // in the future.
+ IMAGE_FROM_FUTURE = 0,
+ // The image was too old for the buffer of old state estimates that we
+ // maintain.
+ IMAGE_TOO_OLD = 1,
+ // Message bridge is not yet connected, and so we can't get accurate
+ // time offsets betwee nnodes.
+ MESSAGE_BRIDGE_DISCONNECTED = 2,
+ // The target ID does not exist.
+ NO_SUCH_TARGET = 3,
+}
+
+table RejectionCount {
+ error:RejectionReason (id: 0);
+ count:uint (id: 1);
+}
+
+table CumulativeStatistics {
+ total_accepted:int (id: 0);
+ total_candidates:int (id: 1);
+ rejection_reasons:[RejectionCount] (id: 2);
+}
+
+table ImuStatus {
+ // Whether the IMU is zeroed or not.
+ zeroed:bool (id: 0);
+ // Whether the IMU zeroing is faulted or not.
+ faulted_zero:bool (id: 1);
+ zeroing:frc971.control_loops.drivetrain.ImuZeroerState (id: 2);
+ // Offset between the pico clock and the pi clock, such that
+ // pico_timestamp + pico_offset_ns = pi_timestamp
+ pico_offset_ns:int64 (id: 3);
+ // Error in the offset, if we assume that the pi/pico clocks are identical and
+ // that there is a perfectly consistent latency between the two. Will be zero
+ // for the very first cycle, and then referenced off of the initial offset
+ // thereafter. If greater than zero, implies that the pico is "behind",
+ // whether due to unusually large latency or due to clock drift.
+ pico_offset_error_ns:int64 (id: 4);
+ left_encoder:double (id: 5);
+ right_encoder:double (id: 6);
+ imu_failures:frc971.controls.ImuFailures (id: 7);
+}
+
+table Status {
+ state: frc971.control_loops.drivetrain.LocalizerState (id: 0);
+ down_estimator:frc971.control_loops.drivetrain.DownEstimatorState (id: 1);
+ imu:ImuStatus (id: 2);
+ // Statistics are per-camera, by camera index.
+ statistics:[CumulativeStatistics] (id: 3);
+}
+
+root_type Status;
diff --git a/y2023/localizer/visualization.fbs b/y2023/localizer/visualization.fbs
new file mode 100644
index 0000000..2785872
--- /dev/null
+++ b/y2023/localizer/visualization.fbs
@@ -0,0 +1,25 @@
+include "y2023/localizer/status.fbs";
+
+namespace y2023.localizer;
+
+table TargetEstimateDebug {
+ camera:uint8 (id: 0);
+ camera_x:double (id: 1);
+ camera_y:double (id: 2);
+ camera_theta:double (id: 3);
+ implied_robot_x:double (id: 4);
+ implied_robot_y:double (id: 5);
+ implied_robot_theta:double (id: 6);
+ accepted:bool (id: 7);
+ rejection_reason:RejectionReason (id: 8);
+ // Image age (more human-readable than trying to interpret raw nanosecond
+ // values).
+ image_age_sec:double (id: 9);
+}
+
+table Visualization {
+ targets:[TargetEstimateDebug] (id: 0);
+ statistics:CumulativeStatistics (id: 1);
+}
+
+root_type Visualization;
diff --git a/y2023/rockpi/BUILD b/y2023/rockpi/BUILD
index 91e8729..701383b 100644
--- a/y2023/rockpi/BUILD
+++ b/y2023/rockpi/BUILD
@@ -7,5 +7,6 @@
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/imu_reader:imu",
+ "//y2023:constants",
],
)
diff --git a/y2023/rockpi/imu_main.cc b/y2023/rockpi/imu_main.cc
index ac0c141..d53b3fb 100644
--- a/y2023/rockpi/imu_main.cc
+++ b/y2023/rockpi/imu_main.cc
@@ -2,6 +2,7 @@
#include "aos/init.h"
#include "aos/realtime.h"
#include "frc971/imu_reader/imu.h"
+#include "y2023/constants.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -12,8 +13,8 @@
aos::configuration::ReadConfig(FLAGS_config);
aos::ShmEventLoop event_loop(&config.message());
- // TODO(austin): Set the ratio...
- frc971::imu::Imu imu(&event_loop, 1.0);
+ frc971::imu::Imu imu(&event_loop,
+ y2023::constants::Values::DrivetrainEncoderToMeters(1));
event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
event_loop.SetRuntimeRealtimePriority(55);
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 839f11e..a0b0ce5 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,5 +1,3 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-
cc_binary(
name = "camera_reader",
srcs = [
@@ -33,7 +31,6 @@
"//frc971/constants:constants_sender_lib",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2023/vision:april_debug_fbs",
"//y2023/vision:vision_util",
"@com_google_absl//absl/strings",
],
@@ -75,14 +72,6 @@
],
)
-flatbuffer_cc_library(
- name = "april_debug_fbs",
- srcs = ["april_debug.fbs"],
- gen_reflections = 1,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
cc_library(
name = "aprilrobotics_lib",
srcs = [
@@ -92,7 +81,6 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
- ":april_debug_fbs",
":vision_util",
"//aos:init",
"//aos/events:shm_event_loop",
@@ -122,6 +110,12 @@
],
)
+filegroup(
+ name = "image_streamer_start",
+ srcs = ["image_streamer_start.sh"],
+ visibility = ["//visibility:public"],
+)
+
cc_binary(
name = "foxglove_image_converter",
srcs = ["foxglove_image_converter.cc"],
diff --git a/y2023/vision/april_debug.fbs b/y2023/vision/april_debug.fbs
deleted file mode 100644
index 4442448..0000000
--- a/y2023/vision/april_debug.fbs
+++ /dev/null
@@ -1,21 +0,0 @@
-namespace y2023.vision;
-
-// Stores image xy pixel coordinates of apriltag corners
-struct Point {
- x:double (id: 0);
- y:double (id: 1);
-}
-
-// Corner locations for each apriltag
-table AprilCorners {
- id:uint64 (id: 0);
- // Will always have 4 values, one for each corner
- points: [Point] (id: 1);
-}
-
-// List of positions of all apriltags detected in current frame
-table AprilDebug {
- corners: [AprilCorners] (id: 0);
-}
-
-root_type AprilDebug;
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 07eee64..b132f85 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -27,8 +27,8 @@
chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
- april_debug_sender_(
- event_loop->MakeSender<y2023::vision::AprilDebug>("/camera")) {
+ image_annotations_sender_(
+ event_loop->MakeSender<foxglove::ImageAnnotations>("/camera")) {
tag_family_ = tag16h5_create();
tag_detector_ = apriltag_detector_create();
@@ -80,7 +80,7 @@
void AprilRoboticsDetector::HandleImage(cv::Mat image_grayscale,
aos::monotonic_clock::time_point eof) {
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
- DetectTags(image_grayscale);
+ DetectTags(image_grayscale, eof);
auto builder = target_map_sender_.MakeBuilder();
std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
@@ -136,7 +136,8 @@
}
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
-AprilRoboticsDetector::DetectTags(cv::Mat image) {
+AprilRoboticsDetector::DetectTags(cv::Mat image,
+ aos::monotonic_clock::time_point eof) {
const aos::monotonic_clock::time_point start_time =
aos::monotonic_clock::now();
@@ -153,9 +154,9 @@
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
- std::vector<flatbuffers::Offset<AprilCorners>> corners_vector;
+ std::vector<std::vector<cv::Point2f>> corners_vector;
- auto builder = april_debug_sender_.MakeBuilder();
+ auto builder = image_annotations_sender_.MakeBuilder();
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
@@ -195,33 +196,19 @@
.count()
<< " seconds for pose estimation";
- std::vector<Point> corner_points;
-
+ std::vector<cv::Point2f> corner_points;
corner_points.emplace_back(det->p[0][0], det->p[0][1]);
corner_points.emplace_back(det->p[1][0], det->p[1][1]);
corner_points.emplace_back(det->p[2][0], det->p[2][1]);
corner_points.emplace_back(det->p[3][0], det->p[3][1]);
- auto corner_points_fbs =
- builder.fbb()->CreateVectorOfStructs(corner_points);
-
- AprilCorners::Builder april_corners_builder =
- builder.MakeBuilder<AprilCorners>();
-
- april_corners_builder.add_id(det->id);
- april_corners_builder.add_points(corner_points_fbs);
-
- corners_vector.emplace_back(april_corners_builder.Finish());
+ corners_vector.emplace_back(corner_points);
}
}
- auto corners_vector_fbs = builder.fbb()->CreateVector(corners_vector);
-
- AprilDebug::Builder april_debug_builder = builder.MakeBuilder<AprilDebug>();
-
- april_debug_builder.add_corners(corners_vector_fbs);
-
- builder.CheckOk(builder.Send(april_debug_builder.Finish()));
+ const auto annotations_offset =
+ frc971::vision::BuildAnnotations(eof, corners_vector, 5.0, builder.fbb());
+ builder.CheckOk(builder.Send(annotations_offset));
apriltag_detections_destroy(detections);
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 4477856..bbc1661 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -17,7 +17,6 @@
#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
#include "y2023/constants/constants_generated.h"
-#include "y2023/vision/april_debug_generated.h"
DECLARE_int32(team_number);
@@ -36,9 +35,9 @@
void UndistortDetection(apriltag_detection_t *det) const;
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
- cv::Mat image);
+ cv::Mat image, aos::monotonic_clock::time_point eof);
- const cv::Mat extrinsics() const { return extrinsics_; }
+ const std::optional<cv::Mat> extrinsics() const { return extrinsics_; }
const cv::Mat intrinsics() const { return intrinsics_; }
const cv::Mat dist_coeffs() const { return dist_coeffs_; }
@@ -57,14 +56,14 @@
const frc971::vision::calibration::CameraCalibration *calibration_;
cv::Mat intrinsics_;
cv::Mat projection_matrix_;
- cv::Mat extrinsics_;
+ std::optional<cv::Mat> extrinsics_;
cv::Mat dist_coeffs_;
aos::Ftrace ftrace_;
frc971::vision::ImageCallback image_callback_;
aos::Sender<frc971::vision::TargetMap> target_map_sender_;
- aos::Sender<y2023::vision::AprilDebug> april_debug_sender_;
+ aos::Sender<foxglove::ImageAnnotations> image_annotations_sender_;
};
} // namespace vision
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
index 1f30579..c9d960a 100755
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
+++ b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
@@ -19,24 +19,6 @@
0.008949,
-0.079813
],
- "fixed_extrinsics": [
- 1.0,
- 0.0,
- 0.0,
- 0.0,
- 0.0,
- 1.0,
- 0.0,
- 0.0,
- 0.0,
- 0.0,
- 1.0,
- 0.0,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
"calibration_timestamp": 1358499377194305339,
"camera_id": "23-01"
}
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
new file mode 100644
index 0000000..b21da40
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-02-22.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.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-2_cam-23-06_ext_2023-02-22.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-02-22.json
new file mode 100644
index 0000000..4759fda
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-02-22.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 971, "intrinsics": [ 893.242981, 0.0, 639.796692, 0.0, 892.498718, 354.109344, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.852213, 0.227336, 0.471224, 0.220072, 0.485092, -0.005909, -0.874443, -0.175232, -0.196008, 0.973799, -0.115315, 0.61409, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451751, 0.252422, 0.000531, 0.000079, -0.079369 ], "calibration_timestamp": 1358501526409252911, "camera_id": "23-06" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
index 1aa8ce6..306bea1 100755
--- a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi2",
+ "node_name": "pi3",
"team_number": 971,
"intrinsics": [
891.026001,
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json
new file mode 100644
index 0000000..8c86d20
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 892.627869, 0.0, 629.289978, 0.0, 891.73761, 373.299896, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.492232, -0.163335, -0.855002, 0.020122, -0.866067, 0.006706, -0.499883, -0.174518, 0.087382, 0.986548, -0.138158, 0.645307, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.454901, 0.266778, -0.000316, -0.000469, -0.091357 ], "calibration_timestamp": 1358500316768663953, "camera_id": "23-07" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
index e7367cc..393beef 100755
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi3",
+ "node_name": "pi2",
"team_number": 971,
"intrinsics": [
894.002502,
@@ -21,4 +21,4 @@
],
"calibration_timestamp": 1358503360377380613,
"camera_id": "23-10"
-}
\ No newline at end of file
+}
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json
new file mode 100644
index 0000000..92ab69c
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 971, "intrinsics": [ 891.88385, 0.0, 642.268616, 0.0, 890.626465, 353.272919, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.865915, -0.186983, -0.463928, -0.014873, -0.473362, 0.006652, 0.880843, -0.215738, -0.161617, 0.982341, -0.094271, 0.676433, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448426, 0.246817, 0.000002, 0.000948, -0.076717 ], "calibration_timestamp": 1358501265150551614, "camera_id": "23-08" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
index 68c2b94..e119c43 100755
--- a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi4",
+ "node_name": "pi1",
"team_number": 971,
"intrinsics": [
893.617798,
@@ -21,4 +21,4 @@
],
"calibration_timestamp": 1358499779650270322,
"camera_id": "23-09"
-}
\ No newline at end of file
+}
diff --git a/y2023/vision/calibrate_extrinsics.cc b/y2023/vision/calibrate_extrinsics.cc
index eab8724..10e1639 100644
--- a/y2023/vision/calibrate_extrinsics.cc
+++ b/y2023/vision/calibrate_extrinsics.cc
@@ -7,8 +7,8 @@
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "aos/util/file.h"
-#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/extrinsics_calibration.h"
#include "frc971/vision/vision_generated.h"
#include "frc971/wpilib/imu_batch_generated.h"
@@ -17,7 +17,7 @@
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
-DEFINE_string(target_type, "charuco",
+DEFINE_string(target_type, "charuco_diamond",
"Type of target: aruco|charuco|charuco_diamond");
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
DEFINE_string(output_logs, "/tmp/calibration/",
@@ -51,16 +51,17 @@
TargetType target_type = TargetTypeFromString(FLAGS_target_type);
std::unique_ptr<aos::EventLoop> constants_event_loop =
factory->MakeEventLoop("constants_fetcher", pi_event_loop->node());
- frc971::constants::ConstantsFetcher<y2023::Constants> constants_fetcher(
- constants_event_loop.get());
- *intrinsics =
- FLAGS_base_intrinsics.empty()
- ? aos::RecursiveCopyFlatBuffer(
- y2023::vision::FindCameraCalibration(
- constants_fetcher.constants(),
- pi_event_loop->node()->name()->string_view()))
- : aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
- FLAGS_base_intrinsics);
+ if (FLAGS_base_intrinsics.empty()) {
+ frc971::constants::ConstantsFetcher<y2023::Constants> constants_fetcher(
+ constants_event_loop.get());
+ *intrinsics =
+ aos::RecursiveCopyFlatBuffer(y2023::vision::FindCameraCalibration(
+ constants_fetcher.constants(),
+ pi_event_loop->node()->name()->string_view()));
+ } else {
+ *intrinsics = aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ FLAGS_base_intrinsics);
+ }
extractor = std::make_unique<Calibration>(
factory, pi_event_loop.get(), imu_event_loop.get(), FLAGS_pi,
&intrinsics->message(), target_type, FLAGS_image_channel, data);
@@ -80,6 +81,14 @@
// Now, accumulate all the data into the data object.
aos::logger::LogReader reader(
aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ reader.RemapLoggedChannel<foxglove::CompressedImage>("/pi1/camera");
+ reader.RemapLoggedChannel<foxglove::CompressedImage>("/pi2/camera");
+ reader.RemapLoggedChannel<foxglove::CompressedImage>("/pi3/camera");
+ reader.RemapLoggedChannel<foxglove::CompressedImage>("/pi4/camera");
+ reader.RemapLoggedChannel<foxglove::ImageAnnotations>("/pi1/camera");
+ reader.RemapLoggedChannel<foxglove::ImageAnnotations>("/pi2/camera");
+ reader.RemapLoggedChannel<foxglove::ImageAnnotations>("/pi3/camera");
+ reader.RemapLoggedChannel<foxglove::ImageAnnotations>("/pi4/camera");
aos::SimulatedEventLoopFactory factory(reader.configuration());
reader.RegisterWithoutStarting(&factory);
@@ -122,29 +131,40 @@
CHECK(data.camera_samples_size() > 0) << "Didn't get any camera observations";
// And now we have it, we can start processing it.
- const Eigen::Quaternion<double> nominal_initial_orientation(
- frc971::controls::ToQuaternionFromRotationVector(
- Eigen::Vector3d(0.0, 0.0, M_PI)));
- const Eigen::Quaternion<double> nominal_pivot_to_camera(
+ // NOTE: For y2023, with no turret, pivot == imu
+
+ // Define the mapping that takes imu frame (with z up) to camera frame (with z
+ // pointing out)
+ const Eigen::Quaterniond R_precam_cam(
+ Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitZ()));
+ // Set up initial conditions for the pis that are reasonable
+ Eigen::Quaternion<double> nominal_initial_orientation(
+ Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()));
+ Eigen::Quaternion<double> nominal_pivot_to_camera(
+ Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd(-0.75 * M_PI, Eigen::Vector3d::UnitY()));
+ Eigen::Vector3d nominal_pivot_to_camera_translation(8.0, 8.0, 0.0);
+ Eigen::Quaternion<double> nominal_board_to_world(
Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
- const Eigen::Quaternion<double> nominal_board_to_world(
- Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
Eigen::Matrix<double, 6, 1> nominal_initial_state =
Eigen::Matrix<double, 6, 1>::Zero();
- // Set x value to 0.5 m (center view on the board)
- // nominal_initial_state(0, 0) = 0.5;
- // Set y value to -1 m (approx distance from imu to board/world)
- nominal_initial_state(1, 0) = -1.0;
+ // Set y value to -1 m (approx distance from imu to the board/world)
+ nominal_initial_state(1, 0) = 1.0;
CalibrationParameters calibration_parameters;
calibration_parameters.initial_orientation = nominal_initial_orientation;
calibration_parameters.pivot_to_camera = nominal_pivot_to_camera;
+ calibration_parameters.pivot_to_camera_translation =
+ nominal_pivot_to_camera_translation;
+ // Board to world rotation
calibration_parameters.board_to_world = nominal_board_to_world;
+ // Initial imu location (and velocity)
calibration_parameters.initial_state = nominal_initial_state;
calibration_parameters.has_pivot = false;
- // Show the inverse of pivot_to_camera, since camera_to_pivot tells where the
- // camera is with respect to the pivot frame
+ // Show the inverse of pivot_to_camera, since camera_to_pivot tells where
+ // the camera is with respect to the pivot frame
const Eigen::Affine3d nominal_affine_pivot_to_camera =
Eigen::Translation3d(calibration_parameters.pivot_to_camera_translation) *
nominal_pivot_to_camera;
@@ -156,22 +176,20 @@
LOG(INFO) << "Initial Conditions for solver. Assumes:\n"
<< "1) board origin is same as world, but rotated pi/2 about "
"x-axis, so z points out\n"
- << "2) pivot origin matches imu origin\n"
+ << "2) pivot origin matches imu origin (since there's no turret)\n"
<< "3) camera is offset from pivot (depends on which camera)";
- LOG(INFO)
- << "Nominal initial_orientation of imu w.r.t. world (angle-axis vector): "
- << frc971::controls::ToRotationVectorFromQuaternion(
- nominal_initial_orientation)
- .transpose();
+ LOG(INFO) << "Nominal initial_orientation of imu w.r.t. world "
+ "(angle-axis vector): "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ nominal_initial_orientation)
+ .transpose();
LOG(INFO) << "Nominal initial_state: \n"
<< "Position: "
<< nominal_initial_state.block<3, 1>(0, 0).transpose() << "\n"
<< "Velocity: "
<< nominal_initial_state.block<3, 1>(3, 0).transpose();
- // TODO<Jim>: Might be nice to take out the rotation component that maps into
- // camera image coordinates (with x right, y down, z forward)
- LOG(INFO) << "Nominal camera_to_pivot (angle-axis vector): "
+ LOG(INFO) << "Nominal camera_to_pivot rotation (angle-axis vector): "
<< frc971::controls::ToRotationVectorFromQuaternion(
nominal_camera_to_pivot_rotation)
.transpose();
@@ -188,17 +206,20 @@
if (!FLAGS_output_calibration.empty()) {
aos::WriteFlatbufferToJson(FLAGS_output_calibration, merged_calibration);
+ } else {
+ LOG(WARNING) << "Calibration filename not provided, so not saving it";
}
LOG(INFO) << "RESULTS OF CALIBRATION SOLVER:";
std::cout << aos::FlatbufferToJson(&merged_calibration.message())
<< std::endl;
+
LOG(INFO) << "initial_orientation of imu w.r.t. world (angle-axis vector): "
<< frc971::controls::ToRotationVectorFromQuaternion(
calibration_parameters.initial_orientation)
.transpose();
LOG(INFO)
- << "initial_state: \n"
+ << "initial_state (imu): \n"
<< "Position: "
<< calibration_parameters.initial_state.block<3, 1>(0, 0).transpose()
<< "\n"
@@ -208,15 +229,17 @@
const Eigen::Affine3d affine_pivot_to_camera =
Eigen::Translation3d(calibration_parameters.pivot_to_camera_translation) *
calibration_parameters.pivot_to_camera;
+ const Eigen::Affine3d affine_camera_to_pivot =
+ affine_pivot_to_camera.inverse();
const Eigen::Quaterniond camera_to_pivot_rotation(
- affine_pivot_to_camera.inverse().rotation());
+ affine_camera_to_pivot.rotation());
const Eigen::Vector3d camera_to_pivot_translation(
- affine_pivot_to_camera.inverse().translation());
- LOG(INFO) << "camera to pivot (angle-axis vec): "
+ affine_camera_to_pivot.translation());
+ LOG(INFO) << "camera to pivot(imu) rotation (angle-axis vec): "
<< frc971::controls::ToRotationVectorFromQuaternion(
camera_to_pivot_rotation)
.transpose();
- LOG(INFO) << "camera to pivot translation: "
+ LOG(INFO) << "camera to pivot(imu) translation: "
<< camera_to_pivot_translation.transpose();
LOG(INFO) << "board_to_world (rotation) "
<< frc971::controls::ToRotationVectorFromQuaternion(
@@ -227,12 +250,24 @@
LOG(INFO) << "gyro_bias " << calibration_parameters.gyro_bias.transpose();
LOG(INFO) << "gravity " << 9.81 * calibration_parameters.gravity_scalar;
- LOG(INFO) << "pivot_to_camera change "
+ LOG(INFO) << "Checking delta from nominal (initial condition) to solved "
+ "values:";
+ LOG(INFO) << "nominal_pivot_to_camera rotation: "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ R_precam_cam * nominal_pivot_to_camera)
+ .transpose();
+ LOG(INFO) << "solved pivot_to_camera rotation: "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ R_precam_cam * calibration_parameters.pivot_to_camera)
+ .transpose();
+
+ LOG(INFO) << "pivot_to_camera rotation delta (zero if the IC's match the "
+ "solved value) "
<< frc971::controls::ToRotationVectorFromQuaternion(
calibration_parameters.pivot_to_camera *
nominal_pivot_to_camera.inverse())
.transpose();
- LOG(INFO) << "board_to_world delta "
+ LOG(INFO) << "board_to_world rotation change "
<< frc971::controls::ToRotationVectorFromQuaternion(
calibration_parameters.board_to_world *
nominal_board_to_world.inverse())
diff --git a/y2023/vision/image_streamer_start.sh b/y2023/vision/image_streamer_start.sh
new file mode 100755
index 0000000..48d9da7
--- /dev/null
+++ b/y2023/vision/image_streamer_start.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# Some configurations to avoid dropping frames
+# 640x480@30fps, 400x300@60fps.
+# Bitrate 500000-1500000
+WIDTH=640
+HEIGHT=480
+BITRATE=1500000
+LISTEN_ON="/camera/downsized"
+# Don't interfere with field webpage
+STREAMING_PORT=1181
+
+# Handle weirdness with openssl and gstreamer
+export OPENSSL_CONF=""
+
+# Enable for verbose logging
+#export GST_DEBUG=4
+
+export LD_LIBRARY_PATH=/usr/lib/aarch64-linux-gnu/gstreamer-1.0
+
+exec ./image_streamer --width=$WIDTH --height=$HEIGHT --bitrate=$BITRATE --listen_on=$LISTEN_ON --config=aos_config.json --streaming_port=$STREAMING_PORT
+
diff --git a/y2023/vision/maps/target_map.json b/y2023/vision/maps/target_map.json
index 3f6eb54..2aa1995 100644
--- a/y2023/vision/maps/target_map.json
+++ b/y2023/vision/maps/target_map.json
@@ -1,3 +1,7 @@
+/* Targets have positive Z axis pointing into the board, positive X to the right
+ when looking at the board, and positive Y is down when looking at the board.
+ This means that you will get an identity rotation from the camera to target
+ frame when the target is upright, flat, and centered in the camera's view.*/
{
"target_poses": [
{
@@ -7,12 +11,11 @@
"y": -2.938,
"z": 0.463
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -22,12 +25,11 @@
"y": -1.262,
"z": 0.463
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -37,12 +39,11 @@
"y": 0.414,
"z": 0.463
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -52,12 +53,11 @@
"y": 2.740,
"z": 0.695
},
- /* yaw of pi */
"orientation": {
- "w": 0.0,
- "x": 0.0,
- "y": 0.0,
- "z": 1.0
+ "w": -0.5,
+ "x": 0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -68,11 +68,10 @@
"z": 0.695
},
"orientation": {
- /* yaw of 0 */
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -82,12 +81,11 @@
"y": 0.414,
"z": 0.463
},
- /* yaw of 0 */
"orientation": {
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -97,12 +95,11 @@
"y": -1.262,
"z": 0.463
},
- /* yaw of 0 */
"orientation": {
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
},
{
@@ -112,12 +109,11 @@
"y": -2.938,
"z": 0.463
},
- /* yaw of 0 */
"orientation": {
- "w": 1.0,
- "x": 0.0,
- "y": 0.0,
- "z": 0.0
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
}
}
]
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 921c172..b41d7d5 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -29,10 +29,10 @@
namespace calibration = frc971::vision::calibration;
// Change reference frame from camera to robot
-Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camrob_target,
+Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
Eigen::Affine3d extrinsics) {
- const Eigen::Affine3d H_robot_camrob = extrinsics;
- const Eigen::Affine3d H_robot_target = H_robot_camrob * H_camrob_target;
+ const Eigen::Affine3d H_robot_camera = extrinsics;
+ const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target;
return H_robot_target;
}
@@ -47,20 +47,13 @@
const TargetMapper::TargetPose target_pose =
PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
- Eigen::Affine3d H_camcv_target =
+ Eigen::Affine3d H_camera_target =
Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
- // With X, Y, Z being robot axes and x, y, z being camera axes,
- // x = -Y, y = -Z, z = X
- static const Eigen::Affine3d H_camcv_camrob =
- Eigen::Affine3d((Eigen::Matrix4d() << 0.0, -1.0, 0.0, 0.0, 0.0, 0.0,
- -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
- .finished());
- Eigen::Affine3d H_camrob_target = H_camcv_camrob.inverse() * H_camcv_target;
Eigen::Affine3d H_robot_target =
- CameraToRobotDetection(H_camrob_target, extrinsics);
+ CameraToRobotDetection(H_camera_target, extrinsics);
ceres::examples::Pose3d target_pose_camera =
- PoseUtils::Affine3dToPose3d(H_camrob_target);
+ PoseUtils::Affine3dToPose3d(H_camera_target);
double distance_from_camera = target_pose_camera.p.norm();
CHECK(map.has_monotonic_timestamp_ns())
@@ -86,7 +79,7 @@
auto detector_ptr = std::make_unique<AprilRoboticsDetector>(
detection_event_loop, kImageChannel);
// Get the camera extrinsics
- cv::Mat extrinsics_cv = detector_ptr->extrinsics();
+ cv::Mat extrinsics_cv = detector_ptr->extrinsics().value();
Eigen::Matrix4d extrinsics_matrix;
cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
const auto extrinsics = Eigen::Affine3d(extrinsics_matrix);
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 7877a57..4fb96bc 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -10,7 +10,6 @@
#include "frc971/vision/vision_generated.h"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
-#include "y2023/vision/april_debug_generated.h"
#include "y2023/vision/vision_util.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -28,10 +27,8 @@
using frc971::vision::CameraImage;
bool DisplayLoop(const cv::Mat intrinsics, const cv::Mat dist_coeffs,
- aos::Fetcher<CameraImage> *image_fetcher,
- aos::Fetcher<AprilDebug> *april_debug_fetcher) {
+ aos::Fetcher<CameraImage> *image_fetcher) {
const CameraImage *image;
- std::optional<const AprilDebug *> april_debug = std::nullopt;
// Read next image
if (!image_fetcher->Fetch()) {
@@ -41,12 +38,6 @@
image = image_fetcher->get();
CHECK(image != nullptr) << "Couldn't read image";
- if (april_debug_fetcher->Fetch()) {
- april_debug = april_debug_fetcher->get();
- } else {
- VLOG(2) << "Couldn't fetch next target map";
- }
-
// Create color image:
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
@@ -66,17 +57,6 @@
cv::Mat undistorted_image;
cv::undistort(bgr_image, undistorted_image, intrinsics, dist_coeffs);
-
- if (april_debug.has_value() && april_debug.value()->corners()->size() > 0) {
- for (const auto *corners : *april_debug.value()->corners()) {
- std::vector<cv::Point> points;
- for (const auto *point_fbs : *corners->points()) {
- points.emplace_back(point_fbs->x(), point_fbs->y());
- }
- cv::polylines(undistorted_image, points, true, cv::Scalar(255, 0, 0), 10);
- }
- }
-
cv::imshow("Display", undistorted_image);
int keystroke = cv::waitKey(1);
@@ -109,14 +89,11 @@
aos::Fetcher<CameraImage> image_fetcher =
event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
- aos::Fetcher<AprilDebug> april_debug_fetcher =
- event_loop.MakeFetcher<AprilDebug>("/camera");
// Run the display loop
event_loop.AddPhasedLoop(
[&](int) {
- if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher,
- &april_debug_fetcher)) {
+ if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher)) {
LOG(INFO) << "Calling event_loop Exit";
event_loop.Exit();
};
diff --git a/y2023/vision/vision_util.cc b/y2023/vision/vision_util.cc
index f4937e5..ca5ad89 100644
--- a/y2023/vision/vision_util.cc
+++ b/y2023/vision/vision_util.cc
@@ -18,11 +18,15 @@
LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
}
-cv::Mat CameraExtrinsics(
+std::optional<cv::Mat> CameraExtrinsics(
const frc971::vision::calibration::CameraCalibration *camera_calibration) {
CHECK(!camera_calibration->has_turret_extrinsics())
<< "No turret on 2023 robot";
+ if (!camera_calibration->has_fixed_extrinsics()) {
+ return std::nullopt;
+ }
+ CHECK(camera_calibration->fixed_extrinsics()->has_data());
cv::Mat result(4, 4, CV_32F,
const_cast<void *>(static_cast<const void *>(
camera_calibration->fixed_extrinsics()->data()->data())));
diff --git a/y2023/vision/vision_util.h b/y2023/vision/vision_util.h
index ce1a69d..79f5c92 100644
--- a/y2023/vision/vision_util.h
+++ b/y2023/vision/vision_util.h
@@ -10,7 +10,7 @@
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
const y2023::Constants &calibration_data, std::string_view node_name);
-cv::Mat CameraExtrinsics(
+std::optional<cv::Mat> CameraExtrinsics(
const frc971::vision::calibration::CameraCalibration *camera_calibration);
cv::Mat CameraIntrinsics(
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 3ccb036..9ffb783 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -197,6 +197,10 @@
position_builder.add_arm(arm_offset);
position_builder.add_wrist(wrist_offset);
+ position_builder.add_end_effector_cone_beam_break(
+ end_effector_cone_beam_break_->Get());
+ position_builder.add_end_effector_cube_beam_break(
+ end_effector_cube_beam_break_->Get());
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -338,6 +342,15 @@
wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
}
+ void set_end_effector_cone_beam_break(
+ ::std::unique_ptr<frc::DigitalInput> sensor) {
+ end_effector_cone_beam_break_ = ::std::move(sensor);
+ }
+ void set_end_effector_cube_beam_break(
+ ::std::unique_ptr<frc::DigitalInput> sensor) {
+ end_effector_cube_beam_break_ = ::std::move(sensor);
+ }
+
private:
std::shared_ptr<const Values> values_;
@@ -349,7 +362,8 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_;
+ std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_,
+ end_effector_cone_beam_break_, end_effector_cube_beam_break_;
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
@@ -817,6 +831,12 @@
sensor_reader.set_wrist_encoder(make_encoder(4));
sensor_reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(4));
+ // TODO(Max): Make the DigitalInput values the accurate robot values.
+ sensor_reader.set_end_effector_cone_beam_break(
+ make_unique<frc::DigitalInput>(6));
+ sensor_reader.set_end_effector_cube_beam_break(
+ make_unique<frc::DigitalInput>(7));
+
AddLoop(&sensor_reader_event_loop);
// Thread 4.
diff --git a/y2023/www/2022.png b/y2023/www/2022.png
deleted file mode 100644
index 68087bd..0000000
--- a/y2023/www/2022.png
+++ /dev/null
Binary files differ
diff --git a/y2023/www/2023.png b/y2023/www/2023.png
new file mode 100644
index 0000000..d3bffd1
--- /dev/null
+++ b/y2023/www/2023.png
Binary files differ
diff --git a/y2023/www/BUILD b/y2023/www/BUILD
index 539fa6d..f8b706c 100644
--- a/y2023/www/BUILD
+++ b/y2023/www/BUILD
@@ -24,7 +24,9 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
- "//y2023/control_loops/superstructure:superstructure_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+ "//y2023/localizer:status_ts_fbs",
+ "//y2023/localizer:visualization_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
)
diff --git a/y2023/www/field.html b/y2023/www/field.html
index f39c1a4..72d8f54 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -27,96 +27,12 @@
<table>
<tr>
- <th colspan="2">Aiming</th>
- </tr>
- <tr>
- <td>Shot distance</td>
- <td id="shot_distance"> NA </td>
- </tr>
- <tr>
- <td>Turret</td>
- <td id="turret"> NA </td>
- </tr>
- </table>
-
- <table>
- <tr>
- <th colspan="2">Catapult</th>
- </tr>
- <tr>
- <td>Fire</td>
- <td id="fire"> NA </td>
- </tr>
- <tr>
- <td>Solve Time</td>
- <td id="mpc_solve_time"> NA </td>
- </tr>
- <tr>
- <td>MPC Active</td>
- <td id="mpc_horizon"> NA </td>
- </tr>
- <tr>
- <td>Shot Count</td>
- <td id="shot_count"> NA </td>
- </tr>
- <tr>
- <td>Position</td>
- <td id="catapult"> NA </td>
- </tr>
- </table>
-
- <table>
- <tr>
- <th colspan="2">Superstructure</th>
- </tr>
- <tr>
- <td>State</td>
- <td id="superstructure_state"> NA </td>
- </tr>
- <tr>
- <td>Intake State</td>
- <td id="intake_state"> NA </td>
- </tr>
- <tr>
- <td>Reseating</td>
- <td id="reseating_in_catapult"> NA </td>
- </tr>
- <tr>
- <td>Flippers Open</td>
- <td id="flippers_open"> NA </td>
- </tr>
- <tr>
- <td>Climber</td>
- <td id="climber"> NA </td>
- </tr>
- </table>
-
- <table>
- <tr>
- <th colspan="2">Intakes</th>
- </tr>
- <tr>
- <td>Front Intake</td>
- <td id="front_intake"> NA </td>
- </tr>
- <tr>
- <td>Back Intake</td>
- <td id="back_intake"> NA </td>
- </tr>
- </table>
-
- <table>
- <tr>
<th colspan="2">Images</th>
</tr>
<tr>
<td> Images Accepted </td>
<td id="images_accepted"> NA </td>
</tr>
- <tr>
- <td> Images Rejected </td>
- <td id="images_rejected"> NA </td>
- </tr>
</table>
</div>
<div id="vision_readouts">
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 67566ea..0f5a975 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -1,7 +1,9 @@
import {ByteBuffer} from 'flatbuffers';
import {Connection} from '../../aos/network/www/proxy';
-import {Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated';
+import {RejectionReason} from '../localizer/status_generated';
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
+import {Visualization, TargetEstimateDebug} from '../localizer/visualization_generated';
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
@@ -9,34 +11,50 @@
const FIELD_SIDE_Y = FIELD_WIDTH / 2;
const FIELD_EDGE_X = FIELD_LENGTH / 2;
-const ROBOT_WIDTH = 34 * IN_TO_M;
-const ROBOT_LENGTH = 36 * IN_TO_M;
+const ROBOT_WIDTH = 25 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
const PI_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+const PIS = ['pi1', 'pi2', 'pi3', 'pi4'];
export class FieldHandler {
private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
private drivetrainStatus: DrivetrainStatus|null = null;
- private superstructureStatus: SuperstructureStatus|null = null;
// Image information indexed by timestamp (seconds since the epoch), so that
// we can stop displaying images after a certain amount of time.
- private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private localizerImageMatches = new Map<number, Visualization>();
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
private y: HTMLElement = (document.getElementById('y') as HTMLElement);
private theta: HTMLElement =
(document.getElementById('theta') as HTMLElement);
- private superstructureState: HTMLElement =
- (document.getElementById('superstructure_state') as HTMLElement);
private imagesAcceptedCounter: HTMLElement =
(document.getElementById('images_accepted') as HTMLElement);
- private imagesRejectedCounter: HTMLElement =
- (document.getElementById('images_rejected') as HTMLElement);
+ private rejectionReasonCells: HTMLElement[] = [];
private fieldImage: HTMLImageElement = new Image();
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
- this.fieldImage.src = "2022.png";
+ this.fieldImage.src = "2023.png";
+
+ for (const value in RejectionReason) {
+ // Typescript generates an iterator that produces both numbers and
+ // strings... don't do anything on the string iterations.
+ if (isNaN(Number(value))) {
+ continue;
+ }
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = RejectionReason[value];
+ row.appendChild(nameCell);
+ const valueCell = document.createElement('div');
+ valueCell.innerHTML = 'NA';
+ this.rejectionReasonCells.push(valueCell);
+ row.appendChild(valueCell);
+ document.getElementById('vision_readouts').appendChild(row);
+ }
for (let ii = 0; ii < PI_COLORS.length; ++ii) {
const legendEntry = document.createElement('div');
@@ -48,32 +66,61 @@
this.connection.addConfigHandler(() => {
// Visualization message is reliable so that we can see *all* the vision
// matches.
+ for (const pi in PIS) {
+ this.connection.addReliableHandler(
+ '/' + PIS[pi] + '/camera', "y2023.localizer.Visualization",
+ (data) => {
+ this.handleLocalizerDebug(pi, data);
+ });
+ }
this.connection.addHandler(
'/drivetrain', "frc971.control_loops.drivetrain.Status", (data) => {
this.handleDrivetrainStatus(data);
});
this.connection.addHandler(
- '/superstructure', "y2023.control_loops.superstructure.Status",
- (data) => {
- this.handleSuperstructureStatus(data);
+ '/localizer', "frc971.controls.LocalizerOutput", (data) => {
+ this.handleLocalizerOutput(data);
});
});
}
+ private handleLocalizerDebug(pi: string, data: Uint8Array): void {
+ const now = Date.now() / 1000.0;
+
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerImageMatches.set(
+ now, Visualization.getRootAsVisualization(fbBuffer));
+
+ const debug = this.localizerImageMatches.get(now);
+
+ if (debug.statistics()) {
+ if (debug.statistics().rejectionReasonsLength() ==
+ this.rejectionReasonCells.length) {
+ for (let ii = 0; ii < debug.statistics().rejectionReasonsLength();
+ ++ii) {
+ this.rejectionReasonCells[ii].innerHTML =
+ debug.statistics().rejectionReasons(ii).count().toString();
+ }
+ } else {
+ console.error('Unexpected number of rejection reasons in counter.');
+ }
+ }
+ }
+
+ private handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
+ }
+
private handleDrivetrainStatus(data: Uint8Array): void {
const fbBuffer = new ByteBuffer(data);
this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
}
- private handleSuperstructureStatus(data: Uint8Array): void {
- const fbBuffer = new ByteBuffer(data);
- this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
- }
-
drawField(): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
- ctx.scale(-1.0, 1.0);
+ ctx.scale(1.0, -1.0);
ctx.drawImage(
this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
-FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
@@ -81,8 +128,7 @@
}
drawCamera(
- x: number, y: number, theta: number, color: string = 'blue',
- extendLines: boolean = true): void {
+ x: number, y: number, theta: number, color: string = 'blue'): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
ctx.translate(x, y);
@@ -91,10 +137,6 @@
ctx.beginPath();
ctx.moveTo(0.5, 0.5);
ctx.lineTo(0, 0);
- if (extendLines) {
- ctx.lineTo(100.0, 0);
- ctx.lineTo(0, 0);
- }
ctx.lineTo(0.5, -0.5);
ctx.stroke();
ctx.beginPath();
@@ -104,9 +146,8 @@
}
drawRobot(
- x: number, y: number, theta: number, turret: number|null,
- color: string = 'blue', dashed: boolean = false,
- extendLines: boolean = true): void {
+ x: number, y: number, theta: number,
+ color: string = 'blue', dashed: boolean = false): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
ctx.translate(x, y);
@@ -125,11 +166,7 @@
// Draw line indicating which direction is forwards on the robot.
ctx.beginPath();
ctx.moveTo(0, 0);
- if (extendLines) {
- ctx.lineTo(1000.0, 0);
- } else {
- ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
- }
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
ctx.stroke();
ctx.restore();
@@ -142,25 +179,6 @@
div.classList.remove('near');
}
- setEstopped(div: HTMLElement): void {
- div.innerHTML = 'estopped';
- div.classList.add('faulted');
- div.classList.remove('zeroing');
- div.classList.remove('near');
- }
-
- setTargetValue(
- div: HTMLElement, target: number, val: number, tolerance: number): void {
- div.innerHTML = val.toFixed(4);
- div.classList.remove('faulted');
- div.classList.remove('zeroing');
- if (Math.abs(target - val) < tolerance) {
- div.classList.add('near');
- } else {
- div.classList.remove('near');
- }
- }
-
setValue(div: HTMLElement, val: number): void {
div.innerHTML = val.toFixed(4);
div.classList.remove('faulted');
@@ -174,15 +192,64 @@
// Draw the matches with debugging information from the localizer.
const now = Date.now() / 1000.0;
-
+
if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
this.drawRobot(
this.drivetrainStatus.trajectoryLogging().x(),
this.drivetrainStatus.trajectoryLogging().y(),
- this.drivetrainStatus.trajectoryLogging().theta(), null, "#000000FF",
+ this.drivetrainStatus.trajectoryLogging().theta(), "#000000FF",
false);
}
+ if (this.localizerOutput) {
+ if (!this.localizerOutput.zeroed()) {
+ this.setZeroing(this.x);
+ this.setZeroing(this.y);
+ this.setZeroing(this.theta);
+ } else {
+ this.setValue(this.x, this.localizerOutput.x());
+ this.setValue(this.y, this.localizerOutput.y());
+ this.setValue(this.theta, this.localizerOutput.theta());
+ }
+
+ this.drawRobot(
+ this.localizerOutput.x(), this.localizerOutput.y(),
+ this.localizerOutput.theta());
+
+ this.imagesAcceptedCounter.innerHTML =
+ this.localizerOutput.imageAcceptedCount().toString();
+ }
+
+ for (const [time, value] of this.localizerImageMatches) {
+ const age = now - time;
+ const kRemovalAge = 1.0;
+ if (age > kRemovalAge) {
+ this.localizerImageMatches.delete(time);
+ continue;
+ }
+ const kMaxImageAlpha = 0.5;
+ const ageAlpha = kMaxImageAlpha * (kRemovalAge - age) / kRemovalAge
+ for (let i = 0; i < value.targetsLength(); i++) {
+ const imageDebug = value.targets(i);
+ const x = imageDebug.impliedRobotX();
+ const y = imageDebug.impliedRobotY();
+ const theta = imageDebug.impliedRobotTheta();
+ const cameraX = imageDebug.cameraX();
+ const cameraY = imageDebug.cameraY();
+ const cameraTheta = imageDebug.cameraTheta();
+ const accepted = imageDebug.accepted();
+ // Make camera readings fade over time.
+ const alpha = Math.round(255 * ageAlpha).toString(16).padStart(2, '0');
+ const dashed = false;
+ const acceptedRgb = accepted ? '#00FF00' : '#FF0000';
+ const acceptedRgba = acceptedRgb + alpha;
+ const cameraRgb = PI_COLORS[imageDebug.camera()];
+ const cameraRgba = cameraRgb + alpha;
+ this.drawRobot(x, y, theta, acceptedRgba, dashed);
+ this.drawCamera(cameraX, cameraY, cameraTheta, cameraRgba);
+ }
+ }
+
window.requestAnimationFrame(() => this.draw());
}
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index b521e07..68dc14a 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -22,6 +22,7 @@
"logger": "LOCAL_AND_REMOTE_LOGGER",
"frequency": 50,
"num_senders": 20,
+ "max_size": 2048,
"logger_nodes": [
"roborio",
"logger"
@@ -300,6 +301,44 @@
"num_senders": 2
},
{
+ "name": "/localizer",
+ "type": "y2023.localizer.Status",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 1000,
+ "num_senders": 2
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerOutput",
+ "source_node": "imu",
+ "frequency": 210,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/localizer/frc971-controls-LocalizerOutput",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 210,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/imu/constants",
"type": "y2023.Constants",
"source_node": "imu",
@@ -320,8 +359,6 @@
{
"name": "localizer",
"executable_name": "localizer_main",
- /* TODO(james): Remove this once confident in the accelerometer code. */
- "args": ["--ignore_accelerometer"],
"user": "pi",
"nodes": [
"imu"
@@ -361,6 +398,13 @@
]
},
{
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "constants_sender",
"autorestart": false,
"user": "pi",
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index e5d62b0..bb78bc5 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -373,7 +373,7 @@
"max_size": 1843456,
"num_readers": 4,
"read_method": "PIN",
- "num_senders": 1
+ "num_senders": 18
},
{
"name": "/logger/camera/downsized",
@@ -442,24 +442,17 @@
],
"applications": [
{
- "name": "logger_message_bridge_client",
- "autostart": false,
- "executable_name": "message_bridge_client.sh",
- "args": [
- "--rmem=8388608",
- "--rt_priority=16"
- ],
+ "name": "message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "user": "pi",
"nodes": [
"logger"
]
},
{
- "name": "logger_message_bridge_server",
+ "name": "message_bridge_server",
"executable_name": "message_bridge_server",
- "autostart": false,
- "args": [
- "--rt_priority=16"
- ],
+ "user": "pi",
"nodes": [
"logger"
]
@@ -467,6 +460,7 @@
{
"name": "logger_camera_reader",
"executable_name": "camera_reader",
+ "user": "pi",
"args": ["--enable_ftrace", "--send_downsized_images"],
"nodes": [
"logger"
@@ -476,6 +470,7 @@
"name": "image_logger",
"executable_name": "logger_main",
"autostart": false,
+ "user": "pi",
"args": [
"--logging_folder",
"",
@@ -491,7 +486,8 @@
{
"name": "image_streamer",
"executable_name": "image_streamer_start.sh",
- "autostart": false,
+ "autostart": true,
+ "user": "pi",
"nodes": [
"logger"
]
@@ -499,6 +495,7 @@
{
"name": "constants_sender",
"autorestart": false,
+ "user": "pi",
"nodes": [
"logger"
]
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index b6eaf04..e125ebe 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -217,15 +217,6 @@
]
},
{
- "name": "/pi{{ NUM }}/camera",
- "type": "y2023.vision.AprilDebug",
- "source_node": "pi{{ NUM }}",
- "frequency": 40,
- "num_senders": 2,
- "max_size": 40000,
- "logger": "LOCAL_LOGGER"
- },
- {
"name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,
@@ -346,6 +337,14 @@
"frequency": 1,
"num_senders": 2,
"max_size": 4096
+ },
+ {
+ "name": "/pi{{ NUM }}/camera",
+ "type": "y2023.localizer.Visualization",
+ "source_node": "imu",
+ "frequency": 40,
+ "max_size": 1000,
+ "num_senders": 2
}
],
"applications": [
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 09873dc..e2fa87b 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -457,6 +457,9 @@
{
"name": "drivetrain",
"executable_name": "drivetrain",
+ "args": [
+ "--die_on_malloc"
+ ],
"nodes": [
"roborio"
]
@@ -464,6 +467,9 @@
{
"name": "trajectory_generator",
"executable_name": "trajectory_generator",
+ "args": [
+ "--die_on_malloc"
+ ],
"nodes": [
"roborio"
]
@@ -471,6 +477,9 @@
{
"name": "superstructure",
"executable_name": "superstructure",
+ "args": [
+ "--die_on_malloc"
+ ],
"nodes": [
"roborio"
]
@@ -488,6 +497,9 @@
{
"name": "joystick_reader",
"executable_name": "joystick_reader",
+ "args": [
+ "--die_on_malloc"
+ ],
"nodes": [
"roborio"
]
@@ -502,6 +514,9 @@
{
"name": "autonomous_action",
"executable_name": "autonomous_action",
+ "args": [
+ "--die_on_malloc"
+ ],
"autostart": false,
"nodes": [
"roborio"
