Merge changes I06a504a9,I54acfc52,I512ddcd0,Ic0288231
* changes:
scouting: Allow background tasks to cancel themselves
scouting: Change background_task implementation
scouting: Make background_task interval configurable
Make the scouting background scraper more generic
diff --git a/WORKSPACE b/WORKSPACE
index 698c066..f411682 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -769,9 +769,9 @@
deps = ["@//third_party/allwpilib/wpimath"],
)
""",
- sha256 = "decff0a28fa4a167696cc2e1122b6a5acd2fef01d3bfd356d8cad25bb487a191",
+ sha256 = "340a9c8e726e2eb365b7a40a722df05fe7c7072c5c4a617fa0218eb6d074ad9f",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.10/api-cpp-23.0.10-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.11/api-cpp-23.0.11-headers.zip",
],
)
@@ -793,9 +793,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "00aea02c583d109056e2716e73b7d70e84d5c56a6daebd1dc9f4612c430894f8",
+ sha256 = "11f392bebfe54f512be9ef59809e1a10c4497e0ce92970645f054e7e04fe7ef6",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.10/api-cpp-23.0.10-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.11/api-cpp-23.0.11-linuxathena.zip",
],
)
@@ -808,9 +808,9 @@
hdrs = glob(['ctre/**/*.h', 'ctre/**/*.hpp']),
)
""",
- sha256 = "3d503df97b711c150c0b50238f644c528e55d5b82418c8e3970c79faa14b749c",
+ sha256 = "7585e1bd9e581dd745e7f040ab521b966b40a04d05bc7fa82d6dafe2fb65764e",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.10/tools-23.0.10-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.11/tools-23.0.11-headers.zip",
],
)
@@ -832,9 +832,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "4ada1ed9e11c208da9e8a8e8a648a0fe426e6717121ebc2f1392ae3ddc7f2b8c",
+ sha256 = "b1daadfe782c43ed32c2e1a3956998f9604a3fc9282ef866fd8dc1482f3b8cc9",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.10/tools-23.0.10-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.11/tools-23.0.11-linuxathena.zip",
],
)
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 07a751c..1edc142 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -642,6 +642,19 @@
)
cc_test(
+ name = "log_reader_utils_test",
+ srcs = ["log_reader_utils_test.cc"],
+ data = [
+ ":multinode_pingpong_combined_config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":log_reader_utils",
+ ":multinode_logger_test_lib",
+ ],
+)
+
+cc_test(
name = "multinode_logger_test",
srcs = ["multinode_logger_test.cc"],
copts = select({
@@ -729,3 +742,21 @@
"@com_google_absl//absl/strings",
],
)
+
+cc_binary(
+ name = "log_config_extractor",
+ srcs = [
+ "log_config_extractor.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:aos_cli_utils",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events/logging:log_reader",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/aos/events/logging/log_config_extractor.cc b/aos/events/logging/log_config_extractor.cc
new file mode 100644
index 0000000..aa2eed0
--- /dev/null
+++ b/aos/events/logging/log_config_extractor.cc
@@ -0,0 +1,133 @@
+#include <iostream>
+#include <filesystem>
+#include <vector>
+
+#include "aos/configuration_generated.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/logfile_sorting.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "flatbuffers/flatbuffers.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(output_path, "/tmp/",
+ "Destination folder for output files. If this flag is not used, "
+ "it stores the files in /tmp/.");
+DEFINE_bool(convert_to_json, false,
+ "If true, can be used to convert bfbs to json.");
+DEFINE_bool(bfbs, false,
+ "If true, write as a binary flatbuffer inside the output_path.");
+DEFINE_bool(json, false, "If true, write as a json inside the output_path.");
+DEFINE_bool(stripped, false,
+ "If true, write as a stripped json inside the output_path.");
+DEFINE_bool(quiet, false,
+ "If true, do not print configuration to stdout. If false, print "
+ "stripped json");
+
+namespace aos {
+
+void WriteConfig(const aos::Configuration *config, std::string output_path) {
+ // In order to deduplicate the reflection schemas in the resulting config
+ // flatbuffer (and thus reduce the size of the final flatbuffer), use
+ // MergeConfiguration() rather than the more generic
+ // RecursiveCopyFlatBuffer(). RecursiveCopyFlatBuffer() is sometimes used to
+ // reduce flatbuffer memory usage, but it only does so by rearranging memory
+ // locations, not by actually deduplicating identical flatbuffers.
+ std::vector<aos::FlatbufferVector<reflection::Schema>> schemas;
+ for (const Channel *c : *config->channels()) {
+ schemas.emplace_back(RecursiveCopyFlatBuffer(c->schema()));
+ }
+ auto config_flatbuffer = configuration::MergeConfiguration(
+ RecursiveCopyFlatBuffer(config), schemas);
+
+ if (FLAGS_bfbs) {
+ WriteFlatbufferToFile(output_path + ".bfbs", config_flatbuffer);
+ LOG(INFO) << "Done writing bfbs to " << output_path << ".bfbs";
+ }
+
+ if (FLAGS_json) {
+ WriteFlatbufferToJson(output_path + ".json", config_flatbuffer);
+ LOG(INFO) << "Done writing json to " << output_path << ".json";
+ }
+
+ if (FLAGS_stripped || !FLAGS_quiet) {
+ auto *channels = config_flatbuffer.mutable_message()->mutable_channels();
+ for (size_t i = 0; i < channels->size(); i++) {
+ channels->GetMutableObject(i)->clear_schema();
+ }
+ if (FLAGS_stripped) {
+ WriteFlatbufferToJson(output_path + ".stripped.json", config_flatbuffer);
+ LOG(INFO) << "Done writing stripped json to " << output_path
+ << ".stripped.json";
+ }
+ if (!FLAGS_quiet) {
+ std::cout << FlatbufferToJson(config_flatbuffer) << std::endl;
+ }
+ }
+}
+
+int Main(int argc, char *argv[]) {
+ CHECK(argc > 1) << "Must provide an argument";
+
+ std::string output_path = FLAGS_output_path;
+ if (output_path.back() != '/') {
+ output_path += "/";
+ }
+ if (!std::filesystem::exists(output_path)) {
+ LOG(ERROR)
+ << "Output path is invalid. Make sure the path exists before running.";
+ return EXIT_FAILURE;
+ }
+ output_path += "aos_config";
+
+ std::shared_ptr<const aos::Configuration> config;
+ // Check if the user wants to use stdin (denoted by '-') which will help
+ // convert configs in json to bfbs (see example in SetUsageMessage)
+ std::string_view arg{argv[1]};
+ if (arg == "-") {
+ // Read in everything from stdin, blocks when there's no data on stdin
+ std::string stdin_data(std::istreambuf_iterator(std::cin), {});
+ aos::FlatbufferDetachedBuffer<aos::Configuration> buffer(
+ aos::JsonToFlatbuffer(stdin_data, aos::ConfigurationTypeTable()));
+ WriteConfig(&buffer.message(), output_path);
+ } else if (FLAGS_convert_to_json) {
+ aos::FlatbufferDetachedBuffer config = aos::configuration::ReadConfig(arg);
+ WriteFlatbufferToJson(output_path + ".json", config);
+ LOG(INFO) << "Done writing json to " << output_path << ".json";
+ } else {
+ const std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
+
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(unsorted_logfiles);
+
+ WriteConfig(logfiles[0].config.get(), output_path);
+ }
+ return EXIT_SUCCESS;
+}
+
+} // namespace aos
+
+int main(int argc, char *argv[]) {
+ gflags::SetUsageMessage(
+ "Binary to output the configuration of a log.\n"
+ "# print config as stripped json to stdout\n"
+ "# path to log should always be absolute path.\n"
+ "log_config_extractor /path/to/log\n"
+ "# write config to ~/work/results/aos_config.bfbs and "
+ "~/work/results/aos_config.json with no stdout "
+ "output\n"
+ "# make sure the output paths are valid and absolute paths.\n"
+ "log_config_extractor /path/to/log --output_path=~/work/results/ --bfbs "
+ "--json --quiet\n"
+ "# pass json config by stdin and output as bfbs\n"
+ "cat aos_config.json | log_config_extractor - --output_path "
+ "/absolute/path/to/dir --bfbs\n"
+ "# This can also be used to convert a bfbs file of config to json\n"
+ "log_config_extractor /path/to/config.bfbs --convert_to_json");
+
+ aos::InitGoogle(&argc, &argv);
+ return aos::Main(argc, argv);
+}
diff --git a/aos/events/logging/log_reader.cc b/aos/events/logging/log_reader.cc
index fb63e79..fa5e5d5 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -618,7 +618,7 @@
? nullptr
: std::make_unique<TimestampMapper>(std::move(filtered_parts)),
filters_.get(), std::bind(&LogReader::NoticeRealtimeEnd, this), node,
- State::ThreadedBuffering::kNo, MaybeMakeReplayChannelIndicies(node));
+ State::ThreadedBuffering::kNo, MaybeMakeReplayChannelIndices(node));
State *state = states_[node_index].get();
state->SetNodeEventLoopFactory(
event_loop_factory_->GetNodeEventLoopFactory(node),
@@ -809,7 +809,7 @@
? nullptr
: std::make_unique<TimestampMapper>(std::move(filtered_parts)),
filters_.get(), std::bind(&LogReader::NoticeRealtimeEnd, this), node,
- State::ThreadedBuffering::kYes, MaybeMakeReplayChannelIndicies(node));
+ State::ThreadedBuffering::kYes, MaybeMakeReplayChannelIndices(node));
State *state = states_[node_index].get();
state->SetChannelCount(logged_configuration()->channels()->size());
@@ -1333,7 +1333,8 @@
RemapConflict conflict_handling) {
if (replay_channels_ != nullptr) {
CHECK(std::find(replay_channels_->begin(), replay_channels_->end(),
- std::make_pair(name, type)) != replay_channels_->end())
+ std::make_pair(std::string{name}, std::string{type})) !=
+ replay_channels_->end())
<< "Attempted to remap channel " << name << " " << type
<< " which is not included in the replay channels passed to LogReader.";
}
@@ -1668,13 +1669,13 @@
// TODO(austin): Lazily re-build to save CPU?
}
-std::unique_ptr<const ReplayChannelIndicies>
-LogReader::MaybeMakeReplayChannelIndicies(const Node *node) {
+std::unique_ptr<const ReplayChannelIndices>
+LogReader::MaybeMakeReplayChannelIndices(const Node *node) {
if (replay_channels_ == nullptr) {
return nullptr;
} else {
- std::unique_ptr<ReplayChannelIndicies> replay_channel_indicies =
- std::make_unique<ReplayChannelIndicies>();
+ std::unique_ptr<ReplayChannelIndices> replay_channel_indices =
+ std::make_unique<ReplayChannelIndices>();
for (auto const &channel : *replay_channels_) {
const Channel *ch = configuration::GetChannel(
logged_configuration(), channel.first, channel.second, "", node);
@@ -1686,10 +1687,10 @@
}
const size_t channel_index =
configuration::ChannelIndex(logged_configuration(), ch);
- replay_channel_indicies->emplace_back(channel_index);
+ replay_channel_indices->emplace_back(channel_index);
}
- std::sort(replay_channel_indicies->begin(), replay_channel_indicies->end());
- return replay_channel_indicies;
+ std::sort(replay_channel_indices->begin(), replay_channel_indices->end());
+ return replay_channel_indices;
}
}
@@ -1748,16 +1749,19 @@
message_bridge::MultiNodeNoncausalOffsetEstimator *multinode_filters,
std::function<void()> notice_realtime_end, const Node *node,
LogReader::State::ThreadedBuffering threading,
- std::unique_ptr<const ReplayChannelIndicies> replay_channel_indicies)
+ std::unique_ptr<const ReplayChannelIndices> replay_channel_indices)
: timestamp_mapper_(std::move(timestamp_mapper)),
notice_realtime_end_(notice_realtime_end),
node_(node),
multinode_filters_(multinode_filters),
threading_(threading),
- replay_channel_indicies_(std::move(replay_channel_indicies)) {
- if (replay_channel_indicies_ != nullptr) {
+ replay_channel_indices_(std::move(replay_channel_indices)) {
+ // If timestamp_mapper_ is nullptr, then there are no log parts associated
+ // with this node. If there are no log parts for the node, there will be no
+ // log data, and so we do not need to worry about the replay channel filters.
+ if (replay_channel_indices_ != nullptr && timestamp_mapper_ != nullptr) {
timestamp_mapper_->set_replay_channels_callback(
- [filter = replay_channel_indicies_.get()](
+ [filter = replay_channel_indices_.get()](
const TimestampedMessage &message) -> bool {
auto const begin = filter->cbegin();
auto const end = filter->cend();
diff --git a/aos/events/logging/log_reader.h b/aos/events/logging/log_reader.h
index fd5a935..d4936f1 100644
--- a/aos/events/logging/log_reader.h
+++ b/aos/events/logging/log_reader.h
@@ -33,9 +33,9 @@
// Vector of pair of name and type of the channel
using ReplayChannels =
- std::vector<std::pair<std::string_view, std::string_view>>;
+ std::vector<std::pair<std::string, std::string>>;
// Vector of channel indices
-using ReplayChannelIndicies = std::vector<size_t>;
+using ReplayChannelIndices = std::vector<size_t>;
// We end up with one of the following 3 log file types.
//
@@ -350,7 +350,7 @@
message_bridge::MultiNodeNoncausalOffsetEstimator *multinode_filters,
std::function<void()> notice_realtime_end, const Node *node,
ThreadedBuffering threading,
- std::unique_ptr<const ReplayChannelIndicies> replay_channel_indicies);
+ std::unique_ptr<const ReplayChannelIndices> replay_channel_indices);
// Connects up the timestamp mappers.
void AddPeer(State *peer);
@@ -753,12 +753,12 @@
// If a ReplayChannels was passed to LogReader, this will hold the
// indices of the channels to replay for the Node represented by
// the instance of LogReader::State.
- std::unique_ptr<const ReplayChannelIndicies> replay_channel_indicies_;
+ std::unique_ptr<const ReplayChannelIndices> replay_channel_indices_;
};
// If a ReplayChannels was passed to LogReader then creates a
- // ReplayChannelIndicies for the given node. Otherwise, returns a nullptr.
- std::unique_ptr<const ReplayChannelIndicies> MaybeMakeReplayChannelIndicies(
+ // ReplayChannelIndices for the given node. Otherwise, returns a nullptr.
+ std::unique_ptr<const ReplayChannelIndices> MaybeMakeReplayChannelIndices(
const Node *node);
// Node index -> State.
diff --git a/aos/events/logging/log_reader_utils_test.cc b/aos/events/logging/log_reader_utils_test.cc
new file mode 100644
index 0000000..9977be9
--- /dev/null
+++ b/aos/events/logging/log_reader_utils_test.cc
@@ -0,0 +1,129 @@
+#include "aos/events/logging/log_reader_utils.h"
+
+#include "aos/events/logging/multinode_logger_test_lib.h"
+#include "aos/events/ping_lib.h"
+#include "aos/events/pong_lib.h"
+#include "aos/testing/tmpdir.h"
+
+namespace aos::logger::testing {
+
+namespace chrono = std::chrono;
+// Created this test fixture because the test case checks for channel names
+// which are different in different configs
+using MultinodeLoggerOneConfigTest = MultinodeLoggerTest;
+
+INSTANTIATE_TEST_SUITE_P(
+ All, MultinodeLoggerOneConfigTest,
+ ::testing::Combine(::testing::Values(ConfigParams{
+ "multinode_pingpong_combined_config.json", true,
+ kCombinedConfigSha1(), kCombinedConfigSha1()}),
+ ::testing::ValuesIn(SupportedCompressionAlgorithms())));
+
+// This test is to check if we are able to get the right channels from a log
+// given nodes and applications using the function ChannelsInLog
+TEST_P(MultinodeLoggerOneConfigTest, ChannelsInLogTest) {
+ // Run the logger
+ time_converter_.StartEqual();
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+ LoggerState pi2_logger = MakeLogger(pi2_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+ StartLogger(&pi2_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+ }
+
+ auto sorted_parts = SortParts(logfiles_);
+ // Read all the sorted log files
+ LogReader reader(sorted_parts);
+
+ std::vector<const Node *> active_nodes;
+ std::vector<std::string> applications;
+ // Get the active node
+ active_nodes.emplace_back(
+ configuration::GetNode(reader.configuration(), "pi1"));
+
+ // Get the application for which you want to check channels
+ applications.push_back("ping");
+ aos::logger::ChannelsInLogResult channels =
+ aos::logger::ChannelsInLog(sorted_parts, active_nodes, applications);
+
+ // Check for the right sender channels
+ std::vector<std::string> expected_senders;
+ expected_senders.push_back("/pi1/aos aos.logging.LogMessageFbs");
+ expected_senders.push_back("/pi1/aos aos.timing.Report");
+ expected_senders.push_back("/test aos.examples.Ping");
+
+ std::vector<std::string> check_senders;
+ for (const auto &sender : channels.senders.value()) {
+ check_senders.push_back(sender.name + " " + sender.type);
+ }
+ ASSERT_THAT(check_senders,
+ ::testing::UnorderedElementsAreArray(expected_senders));
+ ASSERT_EQ(channels.senders.value().size(), 3);
+
+ // Check for the right watcher channels
+ std::vector<std::string> expected_watchers;
+ expected_watchers.push_back("/test aos.examples.Pong");
+ std::vector<std::string> check_watchers;
+ for (const auto &watcher : channels.watchers.value()) {
+ check_watchers.push_back(watcher.name + " " + watcher.type);
+ }
+ ASSERT_THAT(check_watchers,
+ ::testing::UnorderedElementsAreArray(expected_watchers));
+ ASSERT_EQ(channels.watchers.value().size(), 1);
+
+ // There no fetcher channels, check for none
+ ASSERT_EQ(channels.fetchers.value().size(), 0);
+}
+
+// Test to run log reader with replay channels via simulated event loop
+TEST_P(MultinodeLoggerOneConfigTest, SingleNodeLogReplay) {
+ time_converter_.StartEqual();
+ std::vector<std::string> actual_filenames;
+ const std::string kLogfile1_1 =
+ aos::testing::TestTmpDir() + "/multi_logfile1/";
+ util::UnlinkRecursive(kLogfile1_1);
+
+ {
+ LoggerState pi1_logger = MakeLoggerState(
+ pi1_, &event_loop_factory_, SupportedCompressionAlgorithms()[0]);
+ pi2_->DisableStatistics();
+ pi2_->Disconnect(pi1_->node());
+ pi1_->Disconnect(pi2_->node());
+ pi1_logger.StartLogger(kLogfile1_1);
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+ pi1_logger.AppendAllFilenames(&actual_filenames);
+ }
+
+ ReplayChannels replay_channels{{"/test", "aos.examples.Ping"}};
+ LogReader reader(logger::SortParts(actual_filenames), &config_.message(),
+ &replay_channels);
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
+ int ping_count = 0;
+ int pong_count = 0;
+
+ // This sends out the fetched messages and advances time to the start of the
+ // log file.
+ reader.Register(&log_reader_factory);
+
+ const Node *pi1 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi1");
+
+ std::unique_ptr<EventLoop> pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ pi1_event_loop->MakeWatcher(
+ "/test", [&ping_count](const examples::Ping &) { ++ping_count; });
+ pi1_event_loop->MakeWatcher(
+ "/test", [&pong_count](const examples::Pong &) { ++pong_count; });
+
+ int sent_messages = 1999;
+ reader.event_loop_factory()->Run();
+ EXPECT_EQ(ping_count, sent_messages);
+ EXPECT_EQ(pong_count, 0);
+ reader.Deregister();
+}
+} // namespace aos::logger::testing
diff --git a/aos/events/logging/log_replayer.cc b/aos/events/logging/log_replayer.cc
index 0f7444a..c91464c 100644
--- a/aos/events/logging/log_replayer.cc
+++ b/aos/events/logging/log_replayer.cc
@@ -57,19 +57,16 @@
const std::vector<aos::logger::LogFile> logfiles =
aos::logger::SortParts(unsorted_logfiles);
+ aos::logger::LogReader config_reader(logfiles);
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
FLAGS_config.empty()
- ? aos::FlatbufferDetachedBuffer<aos::Configuration>::Empty()
+ ? CopyFlatBuffer<aos::Configuration>(config_reader.configuration())
: aos::configuration::ReadConfig(FLAGS_config);
if (FLAGS_plot_timing) {
- aos::logger::LogReader config_reader(logfiles);
-
// Go through the effort to add a ReplayTiming channel to ensure that we
// can capture timing information from the replay.
- const aos::Configuration *raw_config = FLAGS_config.empty()
- ? config_reader.configuration()
- : &config.message();
+ const aos::Configuration *raw_config = &config.message();
aos::ChannelT channel_overrides;
channel_overrides.max_size = 10000;
channel_overrides.frequency = 10000;
@@ -90,8 +87,7 @@
? std::nullopt
: std::make_optional(aos::JsonToFlatbuffer<ReplayConfig>(
aos::util::ReadFileToStringOrDie(FLAGS_replay_config.data())));
-
- std::vector<std::pair<std::string_view, std::string_view>> message_filter;
+ std::vector<std::pair<std::string, std::string>> message_filter;
if (FLAGS_skip_sender_channels && replay_config.has_value()) {
CHECK(replay_config.value().message().has_active_nodes());
std::vector<const Node *> active_nodes;
@@ -112,7 +108,7 @@
ChannelsInLog(logfiles, active_nodes, applications);
for (auto const &channel :
channels.watchers_and_fetchers_without_senders.value()) {
- message_filter.emplace_back(std::make_pair(channel.name, channel.type));
+ message_filter.emplace_back(channel.name, channel.type);
}
}
@@ -120,7 +116,8 @@
logfiles, &config.message(),
message_filter.empty() ? nullptr : &message_filter);
- if (replay_config.has_value()) {
+ if (replay_config.has_value() &&
+ replay_config.value().message().has_remap_channels()) {
for (auto const &remap_channel :
*replay_config.value().message().remap_channels()) {
auto const &channel = remap_channel->channel();
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 58c0fc3..02a9fdd 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -237,8 +237,7 @@
}
iovec_.clear();
- const size_t original_iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
- size_t iovec_size = original_iovec_size;
+ size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
iovec_.resize(iovec_size);
size_t counted_size = 0;
@@ -248,7 +247,7 @@
// The file is aligned if it is a multiple of kSector in length. The data is
// aligned if it's memory is kSector aligned, and the length is a multiple of
// kSector in length.
- bool aligned = (file_written_bytes_ % kSector) == 0;
+ bool aligned = (total_write_bytes_ % kSector) == 0;
size_t write_index = 0;
for (size_t i = 0; i < iovec_size; ++i) {
iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
@@ -296,13 +295,11 @@
++write_index;
}
- if (counted_size > 0) {
- // Either write the aligned data if it is all aligned, or write the rest
- // unaligned if we wrote aligned up above.
- WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
+ // Either write the aligned data if it is all aligned, or write the rest
+ // unaligned if we wrote aligned up above.
+ WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
- encoder_->Clear(original_iovec_size);
- }
+ encoder_->Clear(iovec_size);
}
size_t DetachedBufferWriter::WriteV(struct iovec *iovec_data, size_t iovec_size,
@@ -320,21 +317,21 @@
if (written > 0) {
// Flush asynchronously and force the data out of the cache.
- sync_file_range(fd_, file_written_bytes_, written, SYNC_FILE_RANGE_WRITE);
- if (file_written_bytes_ != 0) {
+ sync_file_range(fd_, total_write_bytes_, written, SYNC_FILE_RANGE_WRITE);
+ if (last_synced_bytes_ != 0) {
// Per Linus' recommendation online on how to do fast file IO, do a
// blocking flush of the previous write chunk, and then tell the kernel to
// drop the pages from the cache. This makes sure we can't get too far
// ahead.
sync_file_range(fd_, last_synced_bytes_,
- file_written_bytes_ - last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
SYNC_FILE_RANGE_WAIT_BEFORE | SYNC_FILE_RANGE_WRITE |
SYNC_FILE_RANGE_WAIT_AFTER);
posix_fadvise(fd_, last_synced_bytes_,
- file_written_bytes_ - last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
POSIX_FADV_DONTNEED);
- last_synced_bytes_ = file_written_bytes_;
+ last_synced_bytes_ = total_write_bytes_;
}
}
@@ -374,7 +371,6 @@
++total_write_count_;
total_write_messages_ += iovec_size;
total_write_bytes_ += written;
- file_written_bytes_ += written;
}
void DetachedBufferWriter::FlushAtThreshold(
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 0c0ef7f..50f6b40 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -212,8 +212,6 @@
bool supports_odirect_ = true;
int flags_ = 0;
- size_t file_written_bytes_ = 0;
-
aos::monotonic_clock::time_point last_flush_time_ =
aos::monotonic_clock::min_time;
};
diff --git a/aos/events/logging/multinode_logger_test.cc b/aos/events/logging/multinode_logger_test.cc
index bc1f5b8..f1784bf 100644
--- a/aos/events/logging/multinode_logger_test.cc
+++ b/aos/events/logging/multinode_logger_test.cc
@@ -18,21 +18,14 @@
using aos::testing::ArtifactPath;
using aos::testing::MessageCounter;
-constexpr std::string_view kCombinedConfigSha1(
- "5d73fe35bacaa59d24f8f0c1a806fe10b783b0fcc80809ee30a9db824e82538b");
-constexpr std::string_view kSplitConfigSha1(
- "f25e8f6f90d61f41c41517e652300566228b077e44cd86f1af2af4a9bed31ad4");
-constexpr std::string_view kReloggedSplitConfigSha1(
- "f1fabd629bdf8735c3d81bc791d7a454e8e636951c26cba6426545cbc97f911f");
-
INSTANTIATE_TEST_SUITE_P(
All, MultinodeLoggerTest,
::testing::Combine(
::testing::Values(
ConfigParams{"multinode_pingpong_combined_config.json", true,
- kCombinedConfigSha1, kCombinedConfigSha1},
+ kCombinedConfigSha1(), kCombinedConfigSha1()},
ConfigParams{"multinode_pingpong_split_config.json", false,
- kSplitConfigSha1, kReloggedSplitConfigSha1}),
+ kSplitConfigSha1(), kReloggedSplitConfigSha1()}),
::testing::ValuesIn(SupportedCompressionAlgorithms())));
INSTANTIATE_TEST_SUITE_P(
@@ -40,9 +33,9 @@
::testing::Combine(
::testing::Values(
ConfigParams{"multinode_pingpong_combined_config.json", true,
- kCombinedConfigSha1, kCombinedConfigSha1},
+ kCombinedConfigSha1(), kCombinedConfigSha1()},
ConfigParams{"multinode_pingpong_split_config.json", false,
- kSplitConfigSha1, kReloggedSplitConfigSha1}),
+ kSplitConfigSha1(), kReloggedSplitConfigSha1()}),
::testing::ValuesIn(SupportedCompressionAlgorithms())));
// Tests that we can write and read simple multi-node log files.
diff --git a/aos/events/logging/multinode_logger_test_lib.h b/aos/events/logging/multinode_logger_test_lib.h
index d3754e4..f3f322e 100644
--- a/aos/events/logging/multinode_logger_test_lib.h
+++ b/aos/events/logging/multinode_logger_test_lib.h
@@ -55,6 +55,16 @@
~LoggerState();
};
+constexpr std::string_view kCombinedConfigSha1() {
+ return "5d73fe35bacaa59d24f8f0c1a806fe10b783b0fcc80809ee30a9db824e82538b";
+}
+constexpr std::string_view kSplitConfigSha1() {
+ return "f25e8f6f90d61f41c41517e652300566228b077e44cd86f1af2af4a9bed31ad4";
+}
+constexpr std::string_view kReloggedSplitConfigSha1() {
+ return "f1fabd629bdf8735c3d81bc791d7a454e8e636951c26cba6426545cbc97f911f";
+}
+
LoggerState MakeLoggerState(NodeEventLoopFactory *node,
SimulatedEventLoopFactory *factory,
CompressionParams params,
diff --git a/aos/events/logging/realtime_replay_test.cc b/aos/events/logging/realtime_replay_test.cc
index 888b043..2a5703a 100644
--- a/aos/events/logging/realtime_replay_test.cc
+++ b/aos/events/logging/realtime_replay_test.cc
@@ -150,20 +150,24 @@
shm_event_loop.AddTimer([]() { LOG(INFO) << "Hello, World!"; })
->Setup(shm_event_loop.monotonic_now(), std::chrono::seconds(1));
- auto *const end_timer = shm_event_loop.AddTimer([&shm_event_loop]() {
- LOG(INFO) << "All done, quitting now";
- shm_event_loop.Exit();
- });
-
- // TODO(EricS) reader.OnEnd() is not working as expected when
- // using a channel filter.
- // keep looking for 3 seconds if some message comes, just in case
+ // End timer should not be called in this case, it should automatically quit
+ // the event loop and check for number of fetches messages
+ // This is added to make sure OnEnd is called consistently
+ // When OnEnd is not called after finishing of the log, this will eventually
+ // quit due to the end timer but will report a failure
size_t run_seconds = 3;
+ auto *const end_timer =
+ shm_event_loop.AddTimer([&shm_event_loop, run_seconds]() {
+ shm_event_loop.Exit();
+ FAIL() << "OnEnd wasn't called on log end so quitting after "
+ << run_seconds << " seconds.";
+ });
shm_event_loop.OnRun([&shm_event_loop, end_timer, run_seconds]() {
LOG(INFO) << "Quitting in: " << run_seconds;
end_timer->Setup(shm_event_loop.monotonic_now() +
std::chrono::seconds(run_seconds));
});
+
shm_event_loop.Run();
reader.Deregister();
diff --git a/aos/starter/roborio_irq_config.json b/aos/starter/roborio_irq_config.json
index 7b1d536..874de5a 100644
--- a/aos/starter/roborio_irq_config.json
+++ b/aos/starter/roborio_irq_config.json
@@ -35,7 +35,7 @@
{
"name": "FRC_NetCommDaem",
"scheduler": "SCHEDULER_FIFO",
- "priority": 15
+ "priority": 25
}
]
}
diff --git a/aos/time/BUILD b/aos/time/BUILD
index c0bd99d..04ae38f 100644
--- a/aos/time/BUILD
+++ b/aos/time/BUILD
@@ -8,7 +8,10 @@
],
visibility = ["//visibility:public"],
deps = select({
- "@platforms//os:linux": ["@com_github_google_glog//:glog"],
+ "@platforms//os:linux": [
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings",
+ ],
"//conditions:default": ["//motors/core"],
}),
)
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 05510e9..4f39c36 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -9,6 +9,7 @@
#ifdef __linux__
+#include "absl/strings/numbers.h"
#include "glog/logging.h"
#else // __linux__
@@ -80,6 +81,7 @@
return stream;
}
+#ifdef __linux__
std::optional<monotonic_clock::time_point> monotonic_clock::FromString(
const std::string_view now) {
// This should undo the operator << above.
@@ -97,18 +99,28 @@
bool negative = now[0] == '-';
- std::string sec(
+ std::string_view sec(
now.substr(negative ? 1 : 0, now.size() - (negative ? 14 : 13)));
- std::string nsec(now.substr(now.size() - 12, 9));
+ std::string_view nsec(now.substr(now.size() - 12, 9));
if (!std::all_of(sec.begin(), sec.end(), ::isdigit) ||
!std::all_of(nsec.begin(), nsec.end(), ::isdigit)) {
return std::nullopt;
}
+ std::chrono::seconds::rep seconds_data;
+ if (!absl::SimpleAtoi(sec, &seconds_data)) {
+ return std::nullopt;
+ }
+
+ std::chrono::nanoseconds::rep nanoseconds_data;
+ if (!absl::SimpleAtoi(nsec, &nanoseconds_data)) {
+ return std::nullopt;
+ }
+
return monotonic_clock::time_point(
- std::chrono::seconds((negative ? -1 : 1) * atoll(sec.c_str())) +
- std::chrono::nanoseconds((negative ? -1 : 1) * atoll(nsec.c_str())));
+ std::chrono::seconds((negative ? -1 : 1) * seconds_data) +
+ std::chrono::nanoseconds((negative ? -1 : 1) * nanoseconds_data));
}
std::optional<realtime_clock::time_point> realtime_clock::FromString(
@@ -123,7 +135,7 @@
return std::nullopt;
}
- std::string nsec(now.substr(now.size() - 9, 9));
+ std::string_view nsec(now.substr(now.size() - 9, 9));
if (!std::all_of(nsec.begin(), nsec.end(), ::isdigit)) {
return std::nullopt;
@@ -139,10 +151,16 @@
time_t seconds = mktime(&tm);
+ std::chrono::nanoseconds::rep nanoseconds_data;
+ if (!absl::SimpleAtoi(nsec, &nanoseconds_data)) {
+ return std::nullopt;
+ }
+
return realtime_clock::time_point(
std::chrono::seconds(seconds) +
- std::chrono::nanoseconds(atoll(nsec.c_str())));
+ std::chrono::nanoseconds(nanoseconds_data));
}
+#endif
std::ostream &operator<<(std::ostream &stream,
const aos::realtime_clock::time_point &now) {
diff --git a/aos/time/time.h b/aos/time/time.h
index 52d763f..cd4ecda 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -26,8 +26,10 @@
// Converts the time string to a time_point if it is well formatted. This is
// designed to reverse operator <<.
+#ifdef __linux__
static std::optional<monotonic_clock::time_point> FromString(
const std::string_view now);
+#endif
// Returns the epoch (0).
static constexpr monotonic_clock::time_point epoch() {
@@ -56,8 +58,10 @@
// Converts the time string to a time_point if it is well formatted. This is
// designed to reverse operator <<.
+#ifdef __linux__
static std::optional<realtime_clock::time_point> FromString(
const std::string_view now);
+#endif
// Returns the epoch (0).
static constexpr realtime_clock::time_point epoch() {
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 1ebf1ba..60528a9 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -59,6 +59,7 @@
"//y2022/localizer:localizer_plotter",
"//y2022/vision:vision_plotter",
"//y2023/localizer:corrections_plotter",
+ "//y2023/localizer:localizer_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 1b62e42..57d5041 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -52,6 +52,8 @@
'../../y2022/control_loops/superstructure/climber_plotter'
import {plotLocalizer as plot2022Localizer} from
'../../y2022/localizer/localizer_plotter'
+import {plotLocalizer as plot2023Localizer} from
+ '../../y2023/localizer/localizer_plotter'
import {plotVision as plot2022Vision} from
'../../y2022/vision/vision_plotter'
import {plotVision as plot2023Corrections} from
@@ -115,6 +117,7 @@
['Down Estimator', new PlotState(plotDiv, plotDownEstimator)],
['Robot State', new PlotState(plotDiv, plotRobotState)],
['2023 Vision', new PlotState(plotDiv, plot2023Corrections)],
+ ['2023 Localizer', new PlotState(plotDiv, plot2023Localizer)],
['2020 Finisher', new PlotState(plotDiv, plot2020Finisher)],
['2020 Accelerator', new PlotState(plotDiv, plot2020Accelerator)],
['2020 Hood', new PlotState(plotDiv, plot2020Hood)],
diff --git a/frc971/can_logger/BUILD b/frc971/can_logger/BUILD
new file mode 100644
index 0000000..a3b002a
--- /dev/null
+++ b/frc971/can_logger/BUILD
@@ -0,0 +1,39 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_binary(
+ name = "can_logger",
+ srcs = [
+ "can_logger_main.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":can_logger_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/time",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "can_logger_lib",
+ srcs = [
+ "can_logger.cc",
+ "can_logger.h",
+ ],
+ deps = [
+ ":can_logging_fbs",
+ "//aos/events:event_loop",
+ "//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "can_logging_fbs",
+ srcs = [
+ "can_logging.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
diff --git a/frc971/can_logger/can_logger.cc b/frc971/can_logger/can_logger.cc
new file mode 100644
index 0000000..6b4258a
--- /dev/null
+++ b/frc971/can_logger/can_logger.cc
@@ -0,0 +1,88 @@
+#include "frc971/can_logger/can_logger.h"
+
+namespace frc971 {
+namespace can_logger {
+
+CanLogger::CanLogger(aos::EventLoop *event_loop,
+ std::string_view interface_name)
+ : fd_(socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW)),
+ frames_sender_(event_loop->MakeSender<CanFrame>("/can")) {
+ struct ifreq ifr;
+ strcpy(ifr.ifr_name, interface_name.data());
+ PCHECK(ioctl(fd_.get(), SIOCGIFINDEX, &ifr) == 0)
+ << "Failed to get index for interface " << interface_name;
+
+ int enable_canfd = true;
+ PCHECK(setsockopt(fd_.get(), SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd,
+ sizeof(enable_canfd)) == 0)
+ << "Failed to enable CAN FD";
+
+ struct sockaddr_can addr;
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+
+ PCHECK(bind(fd_.get(), reinterpret_cast<struct sockaddr *>(&addr),
+ sizeof(addr)) == 0)
+ << "Failed to bind socket to interface " << interface_name;
+
+ int recieve_buffer_size;
+ socklen_t opt_size = sizeof(recieve_buffer_size);
+ PCHECK(getsockopt(fd_.get(), SOL_SOCKET, SO_RCVBUF, &recieve_buffer_size,
+ &opt_size) == 0);
+ CHECK_EQ(opt_size, sizeof(recieve_buffer_size));
+ VLOG(0) << "CAN recieve bufffer is " << recieve_buffer_size << " bytes large";
+
+ aos::TimerHandler *timer_handler = event_loop->AddTimer([this]() { Poll(); });
+ timer_handler->set_name("CAN logging Loop");
+ timer_handler->Setup(event_loop->monotonic_now(), kPollPeriod);
+}
+
+void CanLogger::Poll() {
+ VLOG(2) << "Polling";
+ int frames_read = 0;
+ while (ReadFrame()) {
+ frames_read++;
+ }
+ VLOG(1) << "Read " << frames_read << " frames to end of buffer";
+}
+
+bool CanLogger::ReadFrame() {
+ errno = 0;
+ struct canfd_frame frame;
+ ssize_t bytes_read = read(fd_.get(), &frame, sizeof(struct canfd_frame));
+
+ if (bytes_read < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
+ // There are no more frames sitting in the recieve buffer.
+ return false;
+ }
+
+ VLOG(2) << "Read " << bytes_read << " bytes";
+ PCHECK(bytes_read > 0);
+ PCHECK(bytes_read == static_cast<ssize_t>(CAN_MTU) ||
+ bytes_read == static_cast<ssize_t>(CANFD_MTU))
+ << "Incomplete can frame";
+
+ struct timeval tv;
+ PCHECK(ioctl(fd_.get(), SIOCGSTAMP, &tv) == 0)
+ << "Failed to get timestamp of CAN frame";
+
+ aos::Sender<CanFrame>::Builder builder = frames_sender_.MakeBuilder();
+
+ auto frame_data = builder.fbb()->CreateVector<uint8_t>(frame.data, frame.len);
+
+ CanFrame::Builder can_frame_builder = builder.MakeBuilder<CanFrame>();
+ can_frame_builder.add_can_id(frame.can_id);
+ can_frame_builder.add_data(frame_data);
+ can_frame_builder.add_monotonic_timestamp_ns(
+ static_cast<std::chrono::nanoseconds>(
+ std::chrono::seconds(tv.tv_sec) +
+ std::chrono::microseconds(tv.tv_usec))
+ .count());
+
+ builder.CheckOk(builder.Send(can_frame_builder.Finish()));
+
+ return true;
+}
+
+} // namespace can_logger
+} // namespace frc971
diff --git a/frc971/can_logger/can_logger.h b/frc971/can_logger/can_logger.h
new file mode 100644
index 0000000..cf37841
--- /dev/null
+++ b/frc971/can_logger/can_logger.h
@@ -0,0 +1,50 @@
+#ifndef FRC971_CAN_LOGGER_CAN_LOGGER_H_
+#define FRC971_CAN_LOGGER_CAN_LOGGER_H_
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <linux/sockios.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+
+#include <chrono>
+
+#include "aos/events/event_loop.h"
+#include "aos/realtime.h"
+#include "aos/scoped/scoped_fd.h"
+#include "frc971/can_logger/can_logging_generated.h"
+
+namespace frc971 {
+namespace can_logger {
+
+// This class listens to all the traffic on a SocketCAN interface and sends it
+// on the aos event loop so it can be logged with the aos logging
+// infrastructure.
+class CanLogger {
+ public:
+ static constexpr std::chrono::milliseconds kPollPeriod =
+ std::chrono::milliseconds(100);
+
+ CanLogger(aos::EventLoop *event_loop,
+ std::string_view interface_name = "can0");
+
+ CanLogger(const CanLogger &) = delete;
+ CanLogger &operator=(const CanLogger &) = delete;
+
+ private:
+ void Poll();
+
+ // Read a CAN frame from the socket and send it on the event loop
+ // Returns true if successful and false if the recieve buffer is empty.
+ bool ReadFrame();
+
+ aos::ScopedFD fd_;
+ aos::Sender<CanFrame> frames_sender_;
+};
+
+} // namespace can_logger
+} // namespace frc971
+
+#endif // FRC971_CAN_LOGGER_CAN_LOGGER_H_
diff --git a/frc971/can_logger/can_logger_main.cc b/frc971/can_logger/can_logger_main.cc
new file mode 100644
index 0000000..42c7162
--- /dev/null
+++ b/frc971/can_logger/can_logger_main.cc
@@ -0,0 +1,18 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/can_logger/can_logger.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::can_logger::CanLogger can_logger(&event_loop, "can0");
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/frc971/can_logger/can_logging.fbs b/frc971/can_logger/can_logging.fbs
new file mode 100644
index 0000000..d6ec8b9
--- /dev/null
+++ b/frc971/can_logger/can_logging.fbs
@@ -0,0 +1,13 @@
+namespace frc971.can_logger;
+
+// A message to represent a single CAN or CAN FD frame.
+table CanFrame {
+ // CAN id + flags
+ can_id:uint32 (id: 0);
+ // The body of the CAN frame up to 64 bytes long.
+ data:[ubyte] (id: 1);
+ // The hardware timestamp of when the frame came in
+ monotonic_timestamp_ns:uint64 (id: 2);
+}
+
+root_type CanFrame;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 51f92b1..9c17fc8 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -13,6 +13,28 @@
namespace control_loops {
namespace drivetrain {
+// What to use the top two buttons for on the pistol grip.
+enum class PistolTopButtonUse {
+ // Normal shifting.
+ kShift,
+ // Line following (currently just uses top button).
+ kLineFollow,
+ // Don't use the top button
+ kNone,
+};
+
+enum class PistolSecondButtonUse {
+ kTurn1,
+ kShiftLow,
+ kNone,
+};
+
+enum class PistolBottomButtonUse {
+ kControlLoopDriving,
+ kSlowDown,
+ kNone,
+};
+
enum class ShifterType : int32_t {
HALL_EFFECT_SHIFTER = 0, // Detect when inbetween gears.
SIMPLE_SHIFTER = 1, // Switch gears without speedmatch logic.
@@ -145,6 +167,10 @@
LineFollowConfig line_follow_config{};
+ PistolTopButtonUse top_button_use = PistolTopButtonUse::kShift;
+ PistolSecondButtonUse second_button_use = PistolSecondButtonUse::kShiftLow;
+ PistolBottomButtonUse bottom_button_use = PistolBottomButtonUse::kSlowDown;
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index ff027d0..d9e239b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -30,6 +30,14 @@
: true;
}
+aos::Alliance LocalizationUtils::Alliance() {
+ joystick_state_fetcher_.Fetch();
+ return (joystick_state_fetcher_.get() != nullptr)
+ ? joystick_state_fetcher_->alliance()
+ : aos::Alliance::kInvalid;
+
+}
+
std::optional<aos::monotonic_clock::duration> LocalizationUtils::ClockOffset(
std::string_view node) {
std::optional<aos::monotonic_clock::duration> monotonic_offset;
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 26242f9..cfd443a 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -29,6 +29,7 @@
// Returns true if either there is no JoystickState message available or if
// we are currently in autonomous mode.
bool MaybeInAutonomous();
+ aos::Alliance Alliance();
// Returns the offset between our node and the specified node (or nullopt if
// no offset is available). The sign of this will be such that the time on
diff --git a/frc971/input/drivetrain_input.cc b/frc971/input/drivetrain_input.cc
index 69795dd..239eddf 100644
--- a/frc971/input/drivetrain_input.cc
+++ b/frc971/input/drivetrain_input.cc
@@ -262,7 +262,8 @@
std::unique_ptr<PistolDrivetrainInputReader> PistolDrivetrainInputReader::Make(
::aos::EventLoop *event_loop, bool default_high_gear,
- TopButtonUse top_button_use) {
+ PistolTopButtonUse top_button_use, PistolSecondButtonUse second_button_use,
+ PistolBottomButtonUse bottom_button_use) {
// Pistol Grip controller
const JoystickAxis kTriggerHigh(1, 1), kTriggerLow(1, 4),
kTriggerVelocityHigh(1, 2), kTriggerVelocityLow(1, 5),
@@ -275,6 +276,7 @@
const ButtonLocation kQuickTurn(1, 3);
const ButtonLocation kTopButton(1, 1);
+
const ButtonLocation kSecondButton(1, 2);
const ButtonLocation kBottomButton(1, 4);
// Non-existant button for nops.
@@ -282,17 +284,20 @@
// TODO(james): Make a copy assignment operator for ButtonLocation so we don't
// have to shoehorn in these ternary operators.
- const ButtonLocation kTurn1 = (top_button_use == TopButtonUse::kLineFollow)
- ? kSecondButton
- : kDummyButton;
+ const ButtonLocation kTurn1 =
+ (second_button_use == PistolSecondButtonUse::kTurn1) ? kSecondButton
+ : kDummyButton;
// Turn2 does closed loop driving.
const ButtonLocation kTurn2 =
- (top_button_use == TopButtonUse::kLineFollow) ? kTopButton : kDummyButton;
+ (top_button_use == PistolTopButtonUse::kLineFollow) ? kTopButton
+ : kDummyButton;
const ButtonLocation kShiftHigh =
- (top_button_use == TopButtonUse::kShift) ? kTopButton : kDummyButton;
+ (top_button_use == PistolTopButtonUse::kShift) ? kTopButton
+ : kDummyButton;
const ButtonLocation kShiftLow =
- (top_button_use == TopButtonUse::kShift) ? kSecondButton : kDummyButton;
+ (second_button_use == PistolSecondButtonUse::kShiftLow) ? kSecondButton
+ : kDummyButton;
std::unique_ptr<PistolDrivetrainInputReader> result(
new PistolDrivetrainInputReader(
@@ -300,7 +305,12 @@
kTriggerVelocityLow, kTriggerTorqueHigh, kTriggerTorqueLow,
kTriggerHigh, kTriggerLow, kWheelVelocityHigh, kWheelVelocityLow,
kWheelTorqueHigh, kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow,
- kTurn1, kTurn2, kBottomButton));
+ kTurn1,
+ (bottom_button_use == PistolBottomButtonUse::kControlLoopDriving)
+ ? kBottomButton
+ : kTurn2,
+ (top_button_use == PistolTopButtonUse::kNone) ? kTopButton
+ : kBottomButton));
result->set_default_high_gear(default_high_gear);
return result;
@@ -335,13 +345,25 @@
drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make(
event_loop, dt_config.default_high_gear);
break;
- case InputType::kPistol:
- drivetrain_input_reader = PistolDrivetrainInputReader::Make(
- event_loop, dt_config.default_high_gear,
+ case InputType::kPistol: {
+ // For backwards compatibility
+ PistolTopButtonUse top_button_use =
dt_config.pistol_grip_shift_enables_line_follow
- ? PistolDrivetrainInputReader::TopButtonUse::kLineFollow
- : PistolDrivetrainInputReader::TopButtonUse::kShift);
+ ? PistolTopButtonUse::kLineFollow
+ : dt_config.top_button_use;
+
+ PistolSecondButtonUse second_button_use = dt_config.second_button_use;
+ PistolBottomButtonUse bottom_button_use = dt_config.bottom_button_use;
+
+ if (top_button_use == PistolTopButtonUse::kLineFollow) {
+ second_button_use = PistolSecondButtonUse::kTurn1;
+ }
+
+ drivetrain_input_reader = PistolDrivetrainInputReader::Make(
+ event_loop, dt_config.default_high_gear, top_button_use,
+ second_button_use, bottom_button_use);
break;
+ }
case InputType::kXbox:
drivetrain_input_reader = XboxDrivetrainInputReader::Make(event_loop);
break;
diff --git a/frc971/input/drivetrain_input.h b/frc971/input/drivetrain_input.h
index a33d449..0cb724d 100644
--- a/frc971/input/drivetrain_input.h
+++ b/frc971/input/drivetrain_input.h
@@ -16,6 +16,10 @@
namespace frc971 {
namespace input {
+using control_loops::drivetrain::PistolBottomButtonUse;
+using control_loops::drivetrain::PistolSecondButtonUse;
+using control_loops::drivetrain::PistolTopButtonUse;
+
// We have a couple different joystick configurations used to drive our skid
// steer robots. These configurations don't change very often, and there are a
// small, discrete, set of them. The interface to the drivetrain is the same
@@ -179,19 +183,13 @@
public:
using DrivetrainInputReader::DrivetrainInputReader;
- // What to use the top two buttons for on the pistol grip.
- enum class TopButtonUse {
- // Normal shifting.
- kShift,
- // Line following (currently just uses top button).
- kLineFollow,
- };
-
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the (cheap) pistol grip controller.
static std::unique_ptr<PistolDrivetrainInputReader> Make(
::aos::EventLoop *event_loop, bool default_high_gear,
- TopButtonUse top_button_use);
+ PistolTopButtonUse top_button_use,
+ PistolSecondButtonUse second_button_use,
+ PistolBottomButtonUse bottom_button_use);
private:
PistolDrivetrainInputReader(
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index de79744..10f48cd 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -48,6 +48,15 @@
// NOTE: not filtered by aprilrobotics.cc so that we can log
// more detections.
distortion_factor:double (id: 5);
+
+ // Ratio of pose_error from the best estimation to
+ // pose error of the second best estimation.
+ // Only filled out if this pose represents a live detection.
+ // This should be significantly less than 1,
+ // otherwise this pose may be a wrong solution.
+ // NOTE: not filtered by aprilrobotics.cc so that we can log
+ // more detections.
+ pose_error_ratio:double (id: 6);
}
// Map of all target poses on a field.
diff --git a/frc971/vision/visualize_robot.cc b/frc971/vision/visualize_robot.cc
index eb2da9a..55932ef 100644
--- a/frc971/vision/visualize_robot.cc
+++ b/frc971/vision/visualize_robot.cc
@@ -1,11 +1,12 @@
#include "frc971/vision/visualize_robot.h"
-#include "glog/logging.h"
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include "glog/logging.h"
+
namespace frc971 {
namespace vision {
@@ -42,15 +43,17 @@
return projected_point;
}
-void VisualizeRobot::DrawLine(Eigen::Vector3d start3d, Eigen::Vector3d end3d) {
+void VisualizeRobot::DrawLine(Eigen::Vector3d start3d, Eigen::Vector3d end3d,
+ cv::Scalar color) {
cv::Point start2d = ProjectPoint(start3d);
cv::Point end2d = ProjectPoint(end3d);
- cv::line(image_, start2d, end2d, cv::Scalar(0, 200, 0));
+ cv::line(image_, start2d, end2d, color);
}
void VisualizeRobot::DrawFrameAxes(Eigen::Affine3d H_world_target,
- std::string label, double axis_scale) {
+ std::string label, cv::Scalar label_color,
+ double axis_scale) {
// Map origin to display from global (world) frame to camera frame
Eigen::Affine3d H_viewpoint_target =
H_world_viewpoint_.inverse() * H_world_target;
@@ -79,13 +82,13 @@
label_coord[0] = label_coord[0] / label_coord[2] + 5;
label_coord[1] = label_coord[1] / label_coord[2];
cv::putText(image_, label, cv::Point(label_coord[0], label_coord[1]),
- cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
+ cv::FONT_HERSHEY_PLAIN, 1.0, label_color);
}
}
void VisualizeRobot::DrawRobotOutline(Eigen::Affine3d H_world_robot,
- std::string label) {
- DrawFrameAxes(H_world_robot, label);
+ std::string label, cv::Scalar color) {
+ DrawFrameAxes(H_world_robot, label, color);
const double kBotHalfWidth = 0.75 / 2.0;
const double kBotHalfLength = 1.0 / 2.0;
// Compute coordinates for front/rear and right/left corners
@@ -98,10 +101,10 @@
Eigen::Vector3d rr_corner =
H_world_robot * Eigen::Vector3d(-kBotHalfLength, kBotHalfWidth, 0);
- DrawLine(fr_corner, fl_corner);
- DrawLine(fl_corner, rl_corner);
- DrawLine(rl_corner, rr_corner);
- DrawLine(rr_corner, fr_corner);
+ DrawLine(fr_corner, fl_corner, color);
+ DrawLine(fl_corner, rl_corner, color);
+ DrawLine(rl_corner, rr_corner, color);
+ DrawLine(rr_corner, fr_corner, color);
}
} // namespace vision
diff --git a/frc971/vision/visualize_robot.h b/frc971/vision/visualize_robot.h
index f5e75af..cd8b4d0 100644
--- a/frc971/vision/visualize_robot.h
+++ b/frc971/vision/visualize_robot.h
@@ -24,6 +24,14 @@
// Set image on which to draw
void SetImage(cv::Mat image) { image_ = image; }
+ // Sets image to all black.
+ // Uses default_size if no image has been set yet, else image_.size()
+ void ClearImage(cv::Size default_size = cv::Size(1280, 720)) {
+ auto image_size = (image_.data == nullptr ? default_size : image_.size());
+ cv::Mat black_image_mat = cv::Mat::zeros(image_size, CV_8UC3);
+ SetImage(black_image_mat);
+ }
+
// Set the viewpoint of the camera relative to a global origin
void SetViewpoint(Eigen::Affine3d view_origin) {
H_world_viewpoint_ = view_origin;
@@ -49,16 +57,19 @@
cv::Point ProjectPoint(Eigen::Vector3d point3d);
// Draw a line connecting two 3D points
- void DrawLine(Eigen::Vector3d start, Eigen::Vector3d end);
+ void DrawLine(Eigen::Vector3d start, Eigen::Vector3d end,
+ cv::Scalar color = cv::Scalar(0, 200, 0));
// Draw coordinate frame for a target frame relative to the world frame
// Axes are drawn (x,y,z) -> (red, green, blue)
void DrawFrameAxes(Eigen::Affine3d H_world_target, std::string label = "",
+ cv::Scalar label_color = cv::Scalar(0, 0, 255),
double axis_scale = 0.25);
// TODO<Jim>: Also implement DrawBoardOutline? Maybe one function w/
// parameters?
- void DrawRobotOutline(Eigen::Affine3d H_world_robot, std::string label = "");
+ void DrawRobotOutline(Eigen::Affine3d H_world_robot, std::string label = "",
+ cv::Scalar color = cv::Scalar(0, 200, 0));
Eigen::Affine3d H_world_viewpoint_; // Where to view the world from
cv::Mat image_; // Image to draw on
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index aef97f7..bc6049f 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -281,12 +281,18 @@
actionOffsets.push(actionOffset);
}
}
+ const teamNumberFb = builder.createString(this.teamNumber.toString());
+ const compLevelFb = builder.createString(this.compLevel);
const actionsVector = SubmitActions.createActionsListVector(
builder,
actionOffsets
);
SubmitActions.startSubmitActions(builder);
+ SubmitActions.addTeamNumber(builder, teamNumberFb);
+ SubmitActions.addMatchNumber(builder, this.matchNumber);
+ SubmitActions.addSetNumber(builder, this.setNumber);
+ SubmitActions.addCompLevel(builder, compLevelFb);
SubmitActions.addActionsList(builder, actionsVector);
builder.finish(SubmitActions.endSubmitActions(builder));
diff --git a/y2023/BUILD b/y2023/BUILD
index 095b856..08713b7 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -37,6 +37,7 @@
"//y2023/autonomous:binaries",
":joystick_reader",
":wpilib_interface",
+ "//frc971/can_logger",
"//aos/network:message_bridge_client",
"//aos/network:message_bridge_server",
"//y2023/control_loops/drivetrain:drivetrain",
@@ -211,6 +212,7 @@
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//frc971/can_logger:can_logging_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
diff --git a/y2023/constants.cc b/y2023/constants.cc
index f11c68c..c8b75d1 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -106,28 +106,29 @@
0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 1.00731305518279;
+ 2.27068625283861;
break;
case kPracticeTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.261970010788946;
+ arm_proximal->zeroing.measured_absolute_position = 0.24572544970387;
arm_proximal->potentiometer_offset =
- 10.5178592988554 + 0.0944609125285876 - 0.00826532984625095;
+ 10.5178592988554 + 0.0944609125285876 - 0.00826532984625095 +
+ 0.167359305216504;
- arm_distal->zeroing.measured_absolute_position = 0.507166003869875;
+ arm_distal->zeroing.measured_absolute_position = 0.0915283983599425;
arm_distal->potentiometer_offset =
7.673132586937 - 0.0799284644472573 - 0.0323574039310657 +
- 0.0143810684138064 + 0.00945555248207735;
+ 0.0143810684138064 + 0.00945555248207735 + 0.452446388633863;
- roll_joint->zeroing.measured_absolute_position = 1.85482286175059;
+ roll_joint->zeroing.measured_absolute_position = 0.0722321007692069;
roll_joint->potentiometer_offset =
0.624713611895747 + 3.10458504917251 - 0.0966407797407789 +
0.0257708772364788 - 0.0395076737853459 - 6.87914956118006 -
- 0.097581301615046;
+ 0.097581301615046 + 3.3424421683095 - 3.97605190912604;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 6.04062267812154;
+ 1.04410369068834;
break;
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 2f91d1b..35a9a1e 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -4,10 +4,10 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_ext_2023-03-05.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-2_cam-23-06_ext_2023-03-25.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-3_cam-23-07_ext_2023-03-26.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json' %}
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
index c559810..d63c518 100644
--- a/y2023/constants/common.json
+++ b/y2023/constants/common.json
@@ -1,2 +1,6 @@
"target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %},
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
+ }
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index bd038b8..61c3365 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -29,11 +29,19 @@
tof:TimeOfFlight (id: 0);
}
+// Set of april tag targets, by april tag ID, to ignore when on a
+// given alliance.
+table IgnoreTargets {
+ red:[uint64] (id: 0);
+ blue:[uint64] (id: 1);
+}
+
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
scoring_map:localizer.ScoringMap (id: 2);
robot:RobotConstants (id: 3);
+ ignore_targets:IgnoreTargets (id: 4);
}
root_type Constants;
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index a1e77af..0a226f0 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -28,5 +28,9 @@
}
]
}
+ },
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
}
}
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.cc b/y2023/control_loops/drivetrain/drivetrain_base.cc
index 4efba61..a11a896 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_base.cc
@@ -76,6 +76,10 @@
.finished()
.asDiagonal()),
.max_controllable_offset = 0.5},
+ frc971::control_loops::drivetrain::PistolTopButtonUse::kNone,
+ frc971::control_loops::drivetrain::PistolSecondButtonUse::kTurn1,
+ frc971::control_loops::drivetrain::PistolBottomButtonUse::
+ kControlLoopDriving,
};
return kDrivetrainConfig;
diff --git a/y2023/control_loops/drivetrain/drivetrain_can_position.fbs b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
index 10d7c9a..34fbf12 100644
--- a/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -20,6 +20,9 @@
// In meters traveled on the drivetrain
position:float (id: 5);
+
+ // Nominal range is -1 to 1, but can be -2 to +2
+ duty_cycle: float (id: 6);
}
// CAN readings from the CAN sensor reader loop
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index cd0d5c9..19e5a34 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -31,7 +31,7 @@
control1=np.array([3.170156, -0.561227]),
control2=np.array([2.972776, -1.026820]),
end=points['GroundPickupBackConeUp'],
- control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
points['GroundPickupBackConeDownBase'] = to_theta_with_circular_index_and_roll(
@@ -44,7 +44,7 @@
control1=np.array([3.170156, -0.561227]),
control2=np.array([2.972776, -1.026820]),
end=points['GroundPickupBackConeDownBase'],
- control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
points[
@@ -58,7 +58,7 @@
control1=np.array([3.495221564200401, 0.4737763579250964]),
control2=np.array([4.110392601248856, 1.0424853539638115]),
end=points['GroundPickupFrontConeDownBase'],
- control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ control_alpha_rolls=[(.95, -np.pi / 2.0)],
))
points['ScoreFrontLowConeDownBase'] = to_theta_with_circular_index_and_roll(
@@ -240,7 +240,7 @@
control1=np.array([3.153228, -0.497009]),
control2=np.array([2.972776, -1.026820]),
end=points['GroundPickupBackCube'],
- control_alpha_rolls=[(0.4, 0.0), (.9, -np.pi / 2.0)],
+ control_alpha_rolls=[(.9, -np.pi / 2.0)],
))
points['GroundPickupFrontCube'] = to_theta_with_circular_index_and_roll(
@@ -253,7 +253,7 @@
control1=np.array([3.338852196583635, 0.34968650009090885]),
control2=np.array([4.28246270189025, 1.492916470137478]),
end=points['GroundPickupFrontCube'],
- control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
+ control_alpha_rolls=[(.9, np.pi / 2.0)],
))
points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
@@ -266,7 +266,7 @@
control1=np.array([3.6130298244820453, -0.2781204657180023]),
control2=np.array([3.804763224169111, -0.5179424890517237]),
end=points['ScoreBackMidConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
points['ScoreBackLowConeUp'] = to_theta_with_circular_index_and_roll(
@@ -279,7 +279,7 @@
control1=np.array([3.260123029490386, -0.5296803702636037]),
control2=np.array([3.1249665389044283, -0.7810758529482493]),
end=points['ScoreBackLowConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
named_segments.append(
@@ -335,12 +335,12 @@
ThetaSplineSegment(
name="NeutralToHPPickupBackConeUp",
start=points['Neutral'],
- control1=np.array([2.0, -0.239378]),
- control2=np.array([1.6, -0.626835]),
+ control1=np.array([2.7014360412658567, -0.32490272351246796]),
+ control2=np.array([0.8282769211604871, -1.8026615176254461]),
end=points['HPPickupBackConeUp'],
- control_alpha_rolls=[(0.3, 0.0), (.9, np.pi / 2.0)],
+ control_alpha_rolls=[(.9, np.pi / 2.0)],
alpha_unitizer=np.matrix(
- f"{1.0 / 9.0} 0 0; 0 {1.0 / 6.0} 0; 0 0 {1.0 / 100.0}"),
+ f"{1.0 / 15.0} 0 0; 0 {1.0 / 15.0} 0; 0 0 {1.0 / 100.0}"),
))
points['HPPickupFrontConeUp'] = np.array(
@@ -352,12 +352,12 @@
ThetaSplineSegment(
name="NeutralToHPPickupFrontConeUp",
start=points['Neutral'],
- control1=np.array([4.068204933788692, -0.05440997896697275]),
- control2=np.array([4.861911360838861, -0.03790410600482508]),
+ control1=np.array([3.7428100581067785, 0.3957215032198551]),
+ control2=np.array([4.349242198354247, 0.8724403878333801]),
end=points['HPPickupFrontConeUp'],
- control_alpha_rolls=[(0.3, 0.0), (.9, -np.pi / 2.0)],
+ control_alpha_rolls=[(.8, -np.pi / 2.0)],
alpha_unitizer=np.matrix(
- f"{1.0 / 9.0} 0 0; 0 {1.0 / 6.0} 0; 0 0 {1.0 / 100.0}"),
+ f"{1.0 / 15.0} 0 0; 0 {1.0 / 15.0} 0; 0 0 {1.0 / 70.0}"),
))
points['ScoreFrontHighConeUp'] = to_theta_with_circular_index_and_roll(
@@ -370,7 +370,7 @@
control1=np.array([2.594244, 0.417442]),
control2=np.array([1.51325, 0.679748]),
end=points['ScoreFrontHighConeUp'],
- control_alpha_rolls=[(0.20, 0.0), (.95, -np.pi / 2.0)],
+ control_alpha_rolls=[(.95, -np.pi / 2.0)],
))
points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
@@ -383,7 +383,7 @@
control1=np.array([3.0, 0.317442]),
control2=np.array([2.9, 0.479748]),
end=points['ScoreFrontMidConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
+ control_alpha_rolls=[(.95, -np.pi / 2.0)],
))
points['Starting'] = np.array((np.pi, -0.125053863467887, 0.0))
@@ -424,7 +424,7 @@
control1=np.array([3.338852196583635, 0.34968650009090885]),
control2=np.array([4.28246270189025, 1.492916470137478]),
end=points['ScoreFrontLowCube'],
- control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
+ control_alpha_rolls=[(.9, np.pi / 2.0)],
))
points['ScoreFrontMidCube'] = to_theta_with_circular_index_and_roll(
@@ -437,7 +437,7 @@
control1=np.array([3.1310824883477952, 0.23591705727105095]),
control2=np.array([3.0320025094685965, 0.43674789928668933]),
end=points["ScoreFrontMidCube"],
- control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
+ control_alpha_rolls=[(0.95, np.pi * 0.5)],
))
named_segments.append(
@@ -460,7 +460,7 @@
control1=np.array([2.537484161662287, 0.059700523547219]),
control2=np.array([2.449391812539668, 0.4141564369176016]),
end=points["ScoreFrontHighCube"],
- control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
+ control_alpha_rolls=[(0.95, np.pi * 0.5)],
))
named_segments.append(
@@ -496,7 +496,7 @@
control1=np.array([3.3485646154655404, -0.4369603013926491]),
control2=np.array([3.2653593368256995, -0.789587049476034]),
end=points["ScoreBackMidCube"],
- control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
+ control_alpha_rolls=[(0.95, -np.pi * 0.5)],
))
named_segments.append(
@@ -522,7 +522,7 @@
control1=np.array([3.6804854484103684, -0.3494541095053125]),
control2=np.array([3.9889380578509517, -0.6637934755748516]),
end=points["ScoreBackHighCube"],
- control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
+ control_alpha_rolls=[(0.95, -np.pi * 0.5)],
))
named_segments.append(
@@ -545,7 +545,7 @@
control1=np.array([3.153481004695907, 0.4827717171390571]),
control2=np.array([4.107487625131798, 0.9935705415901082]),
end=points['GroundPickupFrontConeUp'],
- control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ control_alpha_rolls=[(.95, -np.pi / 2.0)],
))
points['ScoreFrontLowConeUp'] = to_theta_with_circular_index_and_roll(
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index b4d047a..c15b49d 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -143,8 +143,8 @@
DRIVER_CAM_Z_OFFSET = 3.225 * IN_TO_M
DRIVER_CAM_POINTS = rect_points(
- (-5.126 * IN_TO_M + joint_center[0], 0.393 * IN_TO_M + joint_center[0]),
- (5.125 * IN_TO_M + joint_center[1], 17.375 * IN_TO_M + joint_center[1]),
+ (-0.252, -0.252 + 0.098),
+ (-3.0 * IN_TO_M + joint_center[1], -8.0 * IN_TO_M + joint_center[1]),
(-8.475 * IN_TO_M - DRIVER_CAM_Z_OFFSET,
-4.350 * IN_TO_M - DRIVER_CAM_Z_OFFSET))
diff --git a/y2023/control_loops/python/roll.py b/y2023/control_loops/python/roll.py
index ce0fac0..750b34f 100644
--- a/y2023/control_loops/python/roll.py
+++ b/y2023/control_loops/python/roll.py
@@ -24,9 +24,9 @@
motor=control_loop.BAG(),
G=18.0 / 48.0 * 1.0 / 36.0,
# 598.006 in^2 lb
- J=0.175,
+ J=0.175 * 3.0,
q_pos=0.40,
- q_vel=20.0,
+ q_vel=5.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=4.0,
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index e5bc830..afca18b 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -166,6 +166,7 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 444be0d..b600bbb 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -109,6 +109,9 @@
if (!beambreak_status && !preloaded_with_cone) {
state_ = EndEffectorState::INTAKING;
}
+ if (game_piece_ != vision::Class::CUBE) {
+ *roller_voltage = 1.3;
+ }
break;
case EndEffectorState::SPITTING:
// If spit requested, spit
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
index 48bca1b..5cee266 100644
--- a/y2023/control_loops/superstructure/led_indicator.cc
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -1,6 +1,7 @@
#include "y2023/control_loops/superstructure/led_indicator.h"
namespace led = ctre::phoenix::led;
+namespace chrono = std::chrono;
namespace y2023::control_loops::superstructure {
@@ -13,6 +14,8 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_position_fetcher_(
event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_goal_fetcher_(
+ event_loop_->MakeFetcher<Goal>("/superstructure")),
server_statistics_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/roborio/aos")),
@@ -34,8 +37,10 @@
config.brightnessScalar = 1.0;
candle_.ConfigAllSettings(config, 0);
- event_loop_->AddPhasedLoop([&](int) { DecideColor(); },
- std::chrono::milliseconds(20));
+ event_loop_->AddPhasedLoop([this](int) { DecideColor(); },
+ chrono::milliseconds(20));
+ event_loop_->OnRun(
+ [this]() { startup_time_ = event_loop_->monotonic_now(); });
}
// This method will be called once per scheduler run
@@ -75,6 +80,7 @@
drivetrain_output_fetcher_.Fetch();
drivetrain_status_fetcher_.Fetch();
client_statistics_fetcher_.Fetch();
+ superstructure_goal_fetcher_.Fetch();
gyro_reading_fetcher_.Fetch();
localizer_output_fetcher_.Fetch();
@@ -96,15 +102,18 @@
// Not zeroed
if (superstructure_status_fetcher_.get() &&
!superstructure_status_fetcher_->zeroed()) {
- DisplayLed(255, 0, 255);
+ DisplayLed(255, 255, 0);
return;
}
- // If the imu gyro readings are not being sent/updated recently
- if (!gyro_reading_fetcher_.get() ||
- gyro_reading_fetcher_.context().monotonic_event_time <
- event_loop_->monotonic_now() -
- frc971::controls::kLoopFrequency * 10) {
+ // If the imu gyro readings are not being sent/updated recently. Only do this
+ // after we've been on for a bit.
+ if (event_loop_->context().monotonic_event_time >
+ startup_time_ + chrono::seconds(5) &&
+ (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time +
+ frc971::controls::kLoopFrequency * 10 <
+ event_loop_->monotonic_now())) {
if (flash_counter_.Flash()) {
DisplayLed(255, 0, 0);
} else {
@@ -127,59 +136,49 @@
return;
}
- if (superstructure_status_fetcher_.get()) {
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_goal_fetcher_.get()) {
+ const bool cone = (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN);
+ const bool intaking = (superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_UP ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CUBE ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_LAST ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_DOWN);
// Check if end effector is intaking.
if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::INTAKING) {
+ EndEffectorState::LOADED &&
+ intaking) {
if (flash_counter_.Flash()) {
- DisplayLed(255, 165, 0);
- } else {
- DisplayLed(0, 0, 0);
+ if (cone) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(138, 43, 226);
+ }
+ return;
}
-
- return;
- }
- // Check if end effector is spitting.
- if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::SPITTING) {
- if (flash_counter_.Flash()) {
- DisplayLed(0, 255, 0);
- } else {
- DisplayLed(0, 0, 0);
- }
-
- return;
- }
-
- // Check the if there is a cone in the end effector.
- if (superstructure_status_fetcher_->game_piece() ==
- vision::Class::CONE_UP ||
- superstructure_status_fetcher_->game_piece() ==
- vision::Class::CONE_DOWN) {
- DisplayLed(255, 255, 0);
- return;
- }
- // Check if the cube beam break is triggered.
- if (superstructure_status_fetcher_->game_piece() == vision::Class::CUBE) {
- DisplayLed(138, 43, 226);
- return;
}
// Check if there is a target that is in sight
- if (drivetrain_status_fetcher_.get() != nullptr &&
- drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
- DisplayLed(255, 165, 0);
- return;
- }
-
if (event_loop_->monotonic_now() <
- last_accepted_time_ + std::chrono::milliseconds(500)) {
- DisplayLed(0, 0, 255);
+ last_accepted_time_ + chrono::milliseconds(100)) {
+ if (drivetrain_status_fetcher_.get() != nullptr &&
+ drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(0, 255, 0);
+ return;
+ } else {
+ DisplayLed(0, 0, 255);
+ }
return;
}
-
- return;
}
+
+ DisplayLed(0, 0, 0);
}
} // namespace y2023::control_loops::superstructure
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
index d88650d..dfdf4b1 100644
--- a/y2023/control_loops/superstructure/led_indicator.h
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -12,6 +12,7 @@
#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 "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"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
@@ -69,13 +70,14 @@
void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
- ctre::phoenix::led::CANdle candle_{0, ""};
+ ctre::phoenix::led::CANdle candle_{8, "rio"};
aos::EventLoop *event_loop_;
aos::Fetcher<frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
aos::Fetcher<Status> superstructure_status_fetcher_;
aos::Fetcher<Position> superstructure_position_fetcher_;
+ aos::Fetcher<Goal> superstructure_goal_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics>
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
@@ -89,6 +91,9 @@
aos::monotonic_clock::time_point last_accepted_time_ =
aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point startup_time_ =
+ aos::monotonic_clock::min_time;
+
FlashCounter flash_counter_{kFlashIterations};
};
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 7ac7000..2ee2d2b 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -28,5 +28,4 @@
}
-
root_type Goal;
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 83ca2b6..b09e077 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -38,6 +38,9 @@
// Raw position
position:float (id: 5);
+
+ // Nominal range is -1 to 1, but can be -2 to +2
+ duty_cycle:float (id: 6);
}
table Position {
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index 7795462..43c4481 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -1,6 +1,19 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//tools/build_rules:js.bzl", "ts_project")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+ts_project(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
flatbuffer_cc_library(
name = "status_fbs",
diff --git a/y2023/localizer/corrections_plotter.ts b/y2023/localizer/corrections_plotter.ts
index a114f3f..8a27f55 100644
--- a/y2023/localizer/corrections_plotter.ts
+++ b/y2023/localizer/corrections_plotter.ts
@@ -52,19 +52,25 @@
const localizerOutput = aosPlotter.addMessageSource(
'/localizer', 'frc971.controls.LocalizerOutput');
+ const statsPlot = aosPlotter.addPlot(element);
+ statsPlot.plot.getAxisLabels().setTitle('Statistics');
+ statsPlot.plot.getAxisLabels().setXLabel(TIME);
+ statsPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
+
+ statsPlot
+ .addMessageLine(localizerStatus, ['statistics[]', 'total_accepted'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+ statsPlot
+ .addMessageLine(localizerStatus, ['statistics[]', 'total_candidates'])
+ .setDrawLine(false)
+ .setColor(RED);
+
const rejectionPlot = aosPlotter.addPlot(element);
rejectionPlot.plot.getAxisLabels().setTitle('Rejection Reasons');
rejectionPlot.plot.getAxisLabels().setXLabel(TIME);
rejectionPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
- rejectionPlot
- .addMessageLine(localizerStatus, ['statistics[]', 'total_accepted'])
- .setDrawLine(false)
- .setColor(BLUE);
- rejectionPlot
- .addMessageLine(localizerStatus, ['statistics[]', 'total_candidates'])
- .setDrawLine(false)
- .setColor(RED);
for (let ii = 0; ii < targets.length; ++ii) {
rejectionPlot.addMessageLine(targets[ii], ['rejection_reason'])
.setDrawLine(false)
@@ -126,6 +132,21 @@
.setLabel('pi' + (ii + 1));
}
+ const thetaPlot = aosPlotter.addPlot(element);
+ thetaPlot.plot.getAxisLabels().setTitle('Yaw');
+ thetaPlot.plot.getAxisLabels().setXLabel(TIME);
+ thetaPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ thetaPlot.addMessageLine(targets[ii], ['implied_robot_theta'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+ thetaPlot.addMessageLine(localizerOutput, ['theta'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
const aprilTagPlot = aosPlotter.addPlot(element);
aprilTagPlot.plot.getAxisLabels().setTitle('April Tag IDs');
aprilTagPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -137,4 +158,16 @@
.setColor(PI_COLORS[ii])
.setLabel('pi' + (ii + 1));
}
+
+ const imageAgePlot = aosPlotter.addPlot(element);
+ imageAgePlot.plot.getAxisLabels().setTitle('Image Age');
+ imageAgePlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAgePlot.plot.getAxisLabels().setYLabel('[sec]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ imageAgePlot.addMessageLine(targets[ii], ['image_age_sec'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
}
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 4bb07e2..c663954 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -9,6 +9,9 @@
DEFINE_double(max_pose_error, 1e-6,
"Throw out target poses with a higher pose error than this");
+DEFINE_double(
+ max_pose_error_ratio, 0.4,
+ "Throw out target poses with a higher pose error ratio than this");
DEFINE_double(distortion_noise_scalar, 1.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
@@ -245,6 +248,29 @@
return builder->Finish();
}
+bool Localizer::UseAprilTag(uint64_t target_id) {
+ if (target_poses_.count(target_id) == 0) {
+ return false;
+ }
+
+ const flatbuffers::Vector<uint64_t> *ignore_tags = nullptr;
+
+ switch (utils_.Alliance()) {
+ case aos::Alliance::kRed:
+ ignore_tags =
+ CHECK_NOTNULL(constants_fetcher_.constants().ignore_targets()->red());
+ break;
+ case aos::Alliance::kBlue:
+ ignore_tags = CHECK_NOTNULL(
+ constants_fetcher_.constants().ignore_targets()->blue());
+ break;
+ case aos::Alliance::kInvalid:
+ return true;
+ }
+ return std::find(ignore_tags->begin(), ignore_tags->end(), target_id) ==
+ ignore_tags->end();
+}
+
flatbuffers::Offset<TargetEstimateDebug> Localizer::HandleTarget(
int camera_index, const aos::monotonic_clock::time_point capture_time,
const frc971::vision::TargetPoseFbs &target,
@@ -264,7 +290,7 @@
const uint64_t target_id = target.id();
builder.add_april_tag(target_id);
VLOG(2) << aos::FlatbufferToJson(&target);
- if (target_poses_.count(target_id) == 0) {
+ if (!UseAprilTag(target_id)) {
VLOG(1) << "Rejecting target due to invalid ID " << target_id;
return RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, &builder);
}
@@ -337,6 +363,11 @@
<< target.pose_error();
return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR,
&builder);
+ } else if (target.pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+ VLOG(1) << "Rejecting target due to high pose error ratio "
+ << target.pose_error_ratio();
+ return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR_RATIO,
+ &builder);
}
double theta_at_capture = state_at_capture.value()(StateIdx::kTheta);
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index accb0c1..4b0715b 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -91,6 +91,8 @@
static flatbuffers::Offset<CumulativeStatistics> StatisticsForCamera(
const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb);
+ bool UseAprilTag(uint64_t target_id);
+
aos::EventLoop *const event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
diff --git a/y2023/localizer/localizer_plotter.ts b/y2023/localizer/localizer_plotter.ts
new file mode 100644
index 0000000..51524fb
--- /dev/null
+++ b/y2023/localizer/localizer_plotter.ts
@@ -0,0 +1,217 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from '../../aos/network/www/aos_plotter';
+import {ImuMessageHandler} from '../../frc971/wpilib/imu_plot_utils';
+import * as proxy from '../../aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+
+export function plotLocalizer(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const position = aosPlotter.addMessageSource("/drivetrain",
+ "frc971.control_loops.drivetrain.Position");
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const localizer = aosPlotter.addMessageSource(
+ '/localizer', 'y2023.localizer.Status');
+ const imu = aosPlotter.addRawMessageSource(
+ '/localizer', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ // Drivetrain Status estimated relative position
+ const positionPlot = aosPlotter.addPlot(element);
+ positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
+ "of the Drivetrain");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
+ const leftPosition =
+ positionPlot.addMessageLine(status, ["estimated_left_position"]);
+ leftPosition.setColor(RED);
+ const rightPosition =
+ positionPlot.addMessageLine(status, ["estimated_right_position"]);
+ rightPosition.setColor(GREEN);
+ positionPlot
+ .addMessageLine(localizer, ['imu', 'left_encoder'])
+ .setColor(BROWN)
+ .setPointSize(0.0);
+ positionPlot
+ .addMessageLine(localizer, ['imu', 'right_encoder'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
+ positionPlot.addMessageLine(position, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(position, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+
+ // Drivetrain Velocities
+ const velocityPlot = aosPlotter.addPlot(element);
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+ const leftVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
+ leftVelocity.setColor(RED);
+ const rightVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
+ rightVelocity.setColor(GREEN);
+
+ const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
+ leftSpeed.setColor(BLUE);
+ const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
+ rightSpeed.setColor(BROWN);
+
+ const ekfLeftVelocity = velocityPlot.addMessageLine(
+ localizer, ['state', 'left_velocity']);
+ ekfLeftVelocity.setColor(RED);
+ ekfLeftVelocity.setPointSize(0.0);
+ const ekfRightVelocity = velocityPlot.addMessageLine(
+ localizer, ['state', 'right_velocity']);
+ ekfRightVelocity.setColor(GREEN);
+ ekfRightVelocity.setPointSize(0.0);
+
+ // Lateral velocity
+ const lateralPlot = aosPlotter.addPlot(element);
+ lateralPlot.plot.getAxisLabels().setTitle('Lateral Velocity');
+ lateralPlot.plot.getAxisLabels().setXLabel(TIME);
+ lateralPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
+
+ lateralPlot.addMessageLine(localizer, ['state', 'lateral_velocity']).setColor(CYAN);
+
+ // Drivetrain Voltage
+ const voltagePlot = aosPlotter.addPlot(element);
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage Plots');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Voltage (V)')
+
+ voltagePlot.addMessageLine(localizer, ['state', 'left_voltage_error'])
+ .setColor(RED)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(localizer, ['state', 'right_voltage_error'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(output, ['left_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ voltagePlot.addMessageLine(output, ['right_voltage'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Heading
+ const yawPlot = aosPlotter.addPlot(element);
+ yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
+ yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
+
+ yawPlot.addMessageLine(status, ['localizer', 'theta']).setColor(GREEN);
+
+ yawPlot.addMessageLine(localizer, ['down_estimator', 'yaw']).setColor(BLUE);
+
+ yawPlot.addMessageLine(localizer, ['state', 'theta']).setColor(RED);
+
+ // Pitch/Roll
+ const orientationPlot = aosPlotter.addPlot(element);
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ orientationPlot.addMessageLine(localizer, ['down_estimator', 'lateral_pitch'])
+ .setColor(RED)
+ .setLabel('roll');
+ orientationPlot
+ .addMessageLine(localizer, ['down_estimator', 'longitudinal_pitch'])
+ .setColor(GREEN)
+ .setLabel('pitch');
+
+ const stillPlot = aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 3]);
+ stillPlot.plot.getAxisLabels().setTitle('Still Plot');
+ stillPlot.plot.getAxisLabels().setXLabel(TIME);
+ stillPlot.plot.getAxisLabels().setYLabel('bool, g\'s');
+ stillPlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ stillPlot.addMessageLine(localizer, ['down_estimator', 'gravity_magnitude'])
+ .setColor(WHITE)
+ .setDrawLine(false);
+
+ // Absolute X Position
+ const xPositionPlot = aosPlotter.addPlot(element);
+ xPositionPlot.plot.getAxisLabels().setTitle('X Position');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
+
+ xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
+ xPositionPlot.addMessageLine(localizer, ['down_estimator', 'position_x'])
+ .setColor(BLUE);
+ xPositionPlot.addMessageLine(localizer, ['state', 'x']).setColor(CYAN);
+
+ // Absolute Y Position
+ const yPositionPlot = aosPlotter.addPlot(element);
+ yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
+
+ const localizerY = yPositionPlot.addMessageLine(status, ['y']);
+ localizerY.setColor(RED);
+ yPositionPlot.addMessageLine(localizer, ['down_estimator', 'position_y'])
+ .setColor(BLUE);
+ yPositionPlot.addMessageLine(localizer, ['state', 'y']).setColor(CYAN);
+
+ // Gyro
+ const gyroPlot = aosPlotter.addPlot(element);
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(RED);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(GREEN);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(BLUE);
+
+
+ const timingPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ timingPlot.plot.getAxisLabels().setTitle('Fault Counting');
+ timingPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'imu_to_pico_checksum_mismatch'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'pico_to_pi_checksum_mismatch'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'other_zeroing_faults'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'missed_messages'])
+ .setColor(PINK)
+ .setDrawLine(false);
+}
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 7248b33..e8b7d56 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -155,6 +155,7 @@
target_builder.add_position(position_offset);
target_builder.add_orientation(quat_offset);
target_builder.add_pose_error(pose_error_);
+ target_builder.add_pose_error_ratio(pose_error_ratio_);
auto target_offset = target_builder.Finish();
auto targets_offset = builder.fbb()->CreateVector({target_offset});
@@ -277,6 +278,7 @@
uint64_t send_target_id_ = kTargetId;
double pose_error_ = 1e-7;
+ double pose_error_ratio_ = 0.1;
double implied_yaw_error_ = 0.0;
gflags::FlagSaver flag_saver_;
@@ -507,4 +509,27 @@
status_fetcher_->statistics()->Get(0)->total_candidates());
}
+// Tests that we correctly reject a detection with a high pose error ratio.
+TEST_F(LocalizerTest, HighPoseErrorRatio) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ // Send the minimum pose error to be rejected
+ constexpr double kEps = 1e-9;
+ pose_error_ratio_ = 0.4 + kEps;
+
+ 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::HIGH_POSE_ERROR_RATIO))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
} // namespace y2023::localizer::testing
diff --git a/y2023/localizer/status.fbs b/y2023/localizer/status.fbs
index 0ea779f..854603e 100644
--- a/y2023/localizer/status.fbs
+++ b/y2023/localizer/status.fbs
@@ -24,6 +24,8 @@
HIGH_DISTANCE_TO_TARGET = 6,
// The robot was travelling too fast; we don't trust the target.
ROBOT_TOO_FAST = 7,
+ // Pose estimation error ratio was higher than any normal detection.
+ HIGH_POSE_ERROR_RATIO = 8,
}
table RejectionCount {
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index cf2244a..45ab7db 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -1,14 +1,14 @@
#include "y2023/vision/aprilrobotics.h"
-#include "y2023/vision/vision_util.h"
-
#include <opencv2/highgui.hpp>
+#include "y2023/vision/vision_util.h"
+
DEFINE_bool(
debug, false,
"If true, dump a ton of debug and crash on the first valid detection.");
-DEFINE_double(min_decision_margin, 75.0,
+DEFINE_double(min_decision_margin, 50.0,
"Minimum decision margin (confidence) for an apriltag detection");
DEFINE_int32(pixel_border, 10,
"Size of image border within which to reject detected corners");
@@ -17,6 +17,8 @@
"Maximum expected value for unscaled distortion factors. Will scale "
"distortion factors so that this value (and a higher distortion) maps to "
"1.0.");
+DEFINE_uint64(pose_estimation_iterations, 50,
+ "Number of iterations for apriltag pose estimation.");
namespace y2023 {
namespace vision {
@@ -24,9 +26,12 @@
namespace chrono = std::chrono;
AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
- std::string_view channel_name)
+ std::string_view channel_name,
+ bool flip_image)
: calibration_data_(event_loop),
image_size_(0, 0),
+ flip_image_(flip_image),
+ node_name_(event_loop->node()->name()->string_view()),
ftrace_(),
image_callback_(event_loop, channel_name,
[&](cv::Mat image_color_mat,
@@ -94,8 +99,11 @@
auto builder = target_map_sender_.MakeBuilder();
std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
- for (const auto &detection : result.detections) {
- target_poses.emplace_back(BuildTargetPose(detection, builder.fbb()));
+ for (auto &detection : result.detections) {
+ auto *fbb = builder.fbb();
+ auto pose = BuildTargetPose(detection, fbb);
+ DestroyPose(&detection.pose);
+ target_poses.emplace_back(pose);
}
const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
@@ -123,7 +131,7 @@
return frc971::vision::CreateTargetPoseFbs(
*fbb, detection.det.id, position_offset, orientation_offset,
detection.det.decision_margin, detection.pose_error,
- detection.distortion_factor);
+ detection.distortion_factor, detection.pose_error_ratio);
}
void AprilRoboticsDetector::UndistortDetection(
@@ -179,6 +187,11 @@
return corner_points;
}
+void AprilRoboticsDetector::DestroyPose(apriltag_pose_t *pose) const {
+ matd_destroy(pose->R);
+ matd_destroy(pose->t);
+}
+
AprilRoboticsDetector::DetectionResult AprilRoboticsDetector::DetectTags(
cv::Mat image, aos::monotonic_clock::time_point eof) {
cv::Mat color_image;
@@ -252,8 +265,13 @@
const aos::monotonic_clock::time_point before_pose_estimation =
aos::monotonic_clock::now();
- apriltag_pose_t pose;
- double pose_error = estimate_tag_pose(&info, &pose);
+ apriltag_pose_t pose_1;
+ apriltag_pose_t pose_2;
+ double pose_error_1;
+ double pose_error_2;
+ estimate_tag_pose_orthogonal_iteration(&info, &pose_error_1, &pose_1,
+ &pose_error_2, &pose_2,
+ FLAGS_pose_estimation_iterations);
const aos::monotonic_clock::time_point after_pose_estimation =
aos::monotonic_clock::now();
@@ -262,7 +280,8 @@
before_pose_estimation)
.count()
<< " seconds for pose estimation";
- VLOG(1) << "Pose err: " << pose_error;
+ VLOG(1) << "Pose err 1: " << pose_error_1;
+ VLOG(1) << "Pose err 2: " << pose_error_2;
// Send undistorted corner points in pink
std::vector<cv::Point2f> corner_points = MakeCornerVector(det);
@@ -273,10 +292,29 @@
double distortion_factor =
ComputeDistortionFactor(orig_corner_points, corner_points);
+ // We get two estimates for poses.
+ // Choose the one with the lower estimation error
+ bool use_pose_1 = (pose_error_1 < pose_error_2);
+ auto best_pose = (use_pose_1 ? pose_1 : pose_2);
+ auto secondary_pose = (use_pose_1 ? pose_2 : pose_1);
+ double best_pose_error = (use_pose_1 ? pose_error_1 : pose_error_2);
+ double secondary_pose_error = (use_pose_1 ? pose_error_2 : pose_error_1);
+
+ CHECK_NE(best_pose_error, std::numeric_limits<double>::infinity())
+ << "Got no valid pose estimations, this should not be possible.";
+ double pose_error_ratio = best_pose_error / secondary_pose_error;
+
+ // Destroy the secondary pose if we got one
+ if (secondary_pose_error != std::numeric_limits<double>::infinity()) {
+ DestroyPose(&secondary_pose);
+ }
+
results.emplace_back(Detection{.det = *det,
- .pose = pose,
- .pose_error = pose_error,
- .distortion_factor = distortion_factor});
+ .pose = best_pose,
+ .pose_error = best_pose_error,
+ .distortion_factor = distortion_factor,
+ .pose_error_ratio = pose_error_ratio});
+
if (FLAGS_visualize) {
// Draw raw (distorted) corner points in green
cv::line(color_image, orig_corner_points[0], orig_corner_points[1],
@@ -309,9 +347,11 @@
if (FLAGS_visualize) {
// Display the result
// Rotate by 180 degrees to make it upright
- // TDOO<Jim>: Make this a flag, since we don't want it for box of pis
- cv::rotate(color_image, color_image, 1);
- cv::imshow("AprilRoboticsDetector Image", color_image);
+ if (flip_image_) {
+ cv::rotate(color_image, color_image, 1);
+ }
+ cv::imshow(absl::StrCat("AprilRoboticsDetector Image ", node_name_),
+ color_image);
}
const auto corners_offset = builder.fbb()->CreateVector(foxglove_corners);
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 7caa848..01e3138 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -30,6 +30,7 @@
apriltag_pose_t pose;
double pose_error;
double distortion_factor;
+ double pose_error_ratio;
};
struct DetectionResult {
@@ -38,11 +39,15 @@
};
AprilRoboticsDetector(aos::EventLoop *event_loop,
- std::string_view channel_name);
+ std::string_view channel_name, bool flip_image = true);
~AprilRoboticsDetector();
void SetWorkerpoolAffinities();
+ // Deletes the heap-allocated rotation and translation pointers in the given
+ // pose
+ void DestroyPose(apriltag_pose_t *pose) const;
+
// Undistorts the april tag corners using the camera calibration
void UndistortDetection(apriltag_detection_t *det) const;
@@ -78,6 +83,8 @@
std::optional<cv::Mat> extrinsics_;
cv::Mat dist_coeffs_;
cv::Size image_size_;
+ bool flip_image_;
+ std::string_view node_name_;
aos::Ftrace ftrace_;
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-03-25.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-03-25.json
new file mode 100644
index 0000000..d8b5227
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-03-25.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.858536, 0.224804, 0.460847, 0.198133, 0.473832, -0.00434901, -0.880604, -0.221657, -0.195959, 0.974394, -0.110253, 0.593406, 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" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.json
new file mode 100644
index 0000000..64d1c95
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.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.514403, -0.183311, -0.837728, -0.0051253, -0.851817, 0.00353495, -0.523827, -0.214622, 0.0989849, 0.983049, -0.154329, 0.62819, 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" }
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index a110794..e9ef662 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -35,6 +35,15 @@
DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
DEFINE_double(max_pose_error, 1e-6,
"Throw out target poses with a higher pose error than this");
+DEFINE_double(
+ max_pose_error_ratio, 0.4,
+ "Throw out target poses with a higher pose error ratio than this");
+DEFINE_uint64(wait_key, 0,
+ "Time in ms to wait between images, if no click (0 to wait "
+ "indefinitely until click).");
+DEFINE_uint64(skip_to, 1,
+ "Start at combined image of this number (1 is the first image)");
+DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
namespace y2023 {
namespace vision {
@@ -43,6 +52,7 @@
using frc971::vision::PoseUtils;
using frc971::vision::TargetMap;
using frc971::vision::TargetMapper;
+using frc971::vision::VisualizeRobot;
namespace calibration = frc971::vision::calibration;
// Change reference frame from camera to robot
@@ -53,8 +63,14 @@
return H_robot_target;
}
-frc971::vision::VisualizeRobot vis_robot_;
const int kImageWidth = 1000;
+// Map from pi node name to color for drawing
+const std::map<std::string, cv::Scalar> kPiColors = {
+ {"pi1", cv::Scalar(255, 0, 255)},
+ {"pi2", cv::Scalar(255, 255, 0)},
+ {"pi3", cv::Scalar(0, 255, 255)},
+ {"pi4", cv::Scalar(255, 165, 0)},
+};
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
@@ -63,12 +79,25 @@
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
Eigen::Affine3d extrinsics, std::string node_name,
- frc971::vision::TargetMapper mapper) {
+ frc971::vision::TargetMapper target_loc_mapper,
+ std::set<std::string> *drawn_nodes,
+ VisualizeRobot *vis_robot, size_t *display_count) {
+ bool drew = false;
+ std::stringstream label;
+ label << node_name << " - ";
+
for (const auto *target_pose_fbs : *map.target_poses()) {
// Skip detections with high pose errors
if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
- LOG(INFO) << " Skipping tag " << target_pose_fbs->id()
- << " due to pose error of " << target_pose_fbs->pose_error();
+ VLOG(1) << " Skipping tag " << target_pose_fbs->id()
+ << " due to pose error of " << target_pose_fbs->pose_error();
+ continue;
+ }
+ // Skip detections with high pose error ratios
+ if (target_pose_fbs->pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+ VLOG(1) << " Skipping tag " << target_pose_fbs->id()
+ << " due to pose error ratio of "
+ << target_pose_fbs->pose_error_ratio();
continue;
}
@@ -97,26 +126,58 @@
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
if (FLAGS_visualize) {
+ // If we've already drawn in the current image,
+ // display it before clearing and adding the new poses
+ if (drawn_nodes->count(node_name) != 0) {
+ (*display_count)++;
+ cv::putText(vis_robot->image_,
+ "Poses #" + std::to_string(*display_count),
+ cv::Point(600, 10), cv::FONT_HERSHEY_PLAIN, 1.0,
+ cv::Scalar(255, 255, 255));
+
+ if (*display_count >= FLAGS_skip_to) {
+ cv::imshow("View", vis_robot->image_);
+ cv::waitKey(FLAGS_wait_key);
+ } else {
+ VLOG(1) << "At poses #" << std::to_string(*display_count);
+ }
+ vis_robot->ClearImage();
+ drawn_nodes->clear();
+ }
+
Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
- mapper.GetTargetPoseById(target_pose_fbs->id())->pose);
+ target_loc_mapper.GetTargetPoseById(target_pose_fbs->id())->pose);
Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
- Eigen::Affine3d H_world_camera =
- H_world_target * H_camera_target.inverse();
- LOG(INFO) << node_name << ", " << target_pose_fbs->id()
- << ", t = " << pi_distributed_time
- << ", pose_error = " << target_pose_fbs->pose_error()
- << ", robot_pos (x,y,z) + "
- << H_world_robot.translation().transpose();
- vis_robot_.DrawRobotOutline(
- H_world_robot, node_name + "-" +
- std::to_string(target_pose_fbs->id()) + " " +
- std::to_string(target_pose_fbs->pose_error() /
- FLAGS_max_pose_error));
- vis_robot_.DrawFrameAxes(H_world_camera, node_name);
- vis_robot_.DrawFrameAxes(H_world_target,
- std::to_string(target_pose_fbs->id()));
+ VLOG(2) << node_name << ", " << target_pose_fbs->id()
+ << ", t = " << pi_distributed_time
+ << ", pose_error = " << target_pose_fbs->pose_error()
+ << ", pose_error_ratio = " << target_pose_fbs->pose_error_ratio()
+ << ", robot_pos (x,y,z) + "
+ << H_world_robot.translation().transpose();
+
+ label << "id " << target_pose_fbs->id() << ": err "
+ << (target_pose_fbs->pose_error() / FLAGS_max_pose_error)
+ << " ratio " << target_pose_fbs->pose_error_ratio() << " ";
+
+ vis_robot->DrawRobotOutline(H_world_robot,
+ std::to_string(target_pose_fbs->id()),
+ kPiColors.at(node_name));
+
+ vis_robot->DrawFrameAxes(H_world_target,
+ std::to_string(target_pose_fbs->id()),
+ kPiColors.at(node_name));
+ drew = true;
}
}
+ if (drew) {
+ size_t pi_number =
+ static_cast<size_t>(node_name[node_name.size() - 1] - '0');
+ cv::putText(vis_robot->image_, label.str(),
+ cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN, 1.0,
+ kPiColors.at(node_name));
+
+ drawn_nodes->emplace(node_name);
+ }
}
// Get images from pi and pass apriltag positions to HandleAprilTags()
@@ -125,10 +186,15 @@
aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
- std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors,
+ std::set<std::string> *drawn_nodes, VisualizeRobot *vis_robot,
+ size_t *display_count) {
static constexpr std::string_view kImageChannel = "/camera";
+ // For the robots, we need to flip the image since the cameras are rolled by
+ // 180 degrees
+ bool flip_image = (FLAGS_team_number != 7971);
auto detector_ptr = std::make_unique<AprilRoboticsDetector>(
- detection_event_loop, kImageChannel);
+ detection_event_loop, kImageChannel, flip_image);
// Get the camera extrinsics
cv::Mat extrinsics_cv = detector_ptr->extrinsics().value();
Eigen::Matrix4d extrinsics_matrix;
@@ -150,14 +216,8 @@
std::string node_name = detection_event_loop->node()->name()->str();
HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
- extrinsics, node_name, target_loc_mapper);
- if (FLAGS_visualize) {
- cv::imshow("View", vis_robot_.image_);
- cv::waitKey();
- cv::Mat image_mat =
- cv::Mat::zeros(cv::Size(kImageWidth, kImageWidth), CV_8UC3);
- vis_robot_.SetImage(image_mat);
- }
+ extrinsics, node_name, target_loc_mapper, drawn_nodes,
+ vis_robot, display_count);
});
}
@@ -176,8 +236,9 @@
aos::logger::LogReader reader(
aos::logger::SortParts(unsorted_logfiles),
config.has_value() ? &config->message() : nullptr);
- // Send new april tag poses. This allows us to take a log of images, then play
- // with the april detection code and see those changes take effect in mapping
+ // Send new april tag poses. This allows us to take a log of images, then
+ // play with the april detection code and see those changes take effect in
+ // mapping
constexpr size_t kNumPis = 4;
for (size_t i = 1; i <= kNumPis; i++) {
reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
@@ -196,6 +257,9 @@
FLAGS_constants_path);
std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
+ VisualizeRobot vis_robot;
+ std::set<std::string> drawn_nodes;
+ size_t display_count = 0;
const aos::Node *pi1 =
aos::configuration::GetNode(reader.configuration(), "pi1");
@@ -204,7 +268,8 @@
std::unique_ptr<aos::EventLoop> pi1_mapping_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1_mapping", pi1);
HandlePiCaptures(pi1_detection_event_loop.get(), pi1_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors);
+ &reader, ×tamped_target_detections, &detectors,
+ &drawn_nodes, &vis_robot, &display_count);
const aos::Node *pi2 =
aos::configuration::GetNode(reader.configuration(), "pi2");
@@ -213,7 +278,8 @@
std::unique_ptr<aos::EventLoop> pi2_mapping_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi2_mapping", pi2);
HandlePiCaptures(pi2_detection_event_loop.get(), pi2_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors);
+ &reader, ×tamped_target_detections, &detectors,
+ &drawn_nodes, &vis_robot, &display_count);
const aos::Node *pi3 =
aos::configuration::GetNode(reader.configuration(), "pi3");
@@ -222,7 +288,8 @@
std::unique_ptr<aos::EventLoop> pi3_mapping_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi3_mapping", pi3);
HandlePiCaptures(pi3_detection_event_loop.get(), pi3_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors);
+ &reader, ×tamped_target_detections, &detectors,
+ &drawn_nodes, &vis_robot, &display_count);
const aos::Node *pi4 =
aos::configuration::GetNode(reader.configuration(), "pi4");
@@ -231,7 +298,8 @@
std::unique_ptr<aos::EventLoop> pi4_mapping_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi4_mapping", pi4);
HandlePiCaptures(pi4_detection_event_loop.get(), pi4_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors);
+ &reader, ×tamped_target_detections, &detectors,
+ &drawn_nodes, &vis_robot, &display_count);
std::unique_ptr<aos::EventLoop> mcap_event_loop;
std::unique_ptr<aos::McapLogger> relogger;
@@ -252,20 +320,20 @@
}
if (FLAGS_visualize) {
- cv::Mat image_mat =
- cv::Mat::zeros(cv::Size(kImageWidth, kImageWidth), CV_8UC3);
- vis_robot_.SetImage(image_mat);
+ vis_robot.ClearImage();
const double kFocalLength = 500.0;
- vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ vis_robot.SetDefaultViewpoint(kImageWidth, kFocalLength);
}
reader.event_loop_factory()->Run();
- auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_target_detections);
+ if (FLAGS_solve) {
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_target_detections);
- frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
+ frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
+ mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
+ }
// Clean up all the pointers
for (auto &detector_ptr : detectors) {
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index cd52cfd..e8d76c9 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -114,29 +114,24 @@
} // namespace
static constexpr int kCANFalconCount = 6;
-static constexpr int kCANSignalsCount = 4;
-static constexpr int kRollerSignalsCount = 4;
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
class Falcon {
public:
Falcon(int device_id, std::string canbus,
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount +
- kRollerSignalsCount> *signals)
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> *signals)
: talon_(device_id, canbus),
device_id_(device_id),
device_temp_(talon_.GetDeviceTemp()),
supply_voltage_(talon_.GetSupplyVoltage()),
supply_current_(talon_.GetSupplyCurrent()),
torque_current_(talon_.GetTorqueCurrent()),
- position_(talon_.GetPosition()) {
+ position_(talon_.GetPosition()),
+ duty_cycle_(talon_.GetDutyCycle()) {
// device temp is not timesynced so don't add it to the list of signals
device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
- CHECK_EQ(kCANSignalsCount, 4);
CHECK_NOTNULL(signals);
- CHECK_LE(signals->size() + 4u, signals->capacity());
supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
signals->push_back(&supply_voltage_);
@@ -149,6 +144,9 @@
position_.SetUpdateFrequency(kCANUpdateFreqHz);
signals->push_back(&position_);
+
+ duty_cycle_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&duty_cycle_);
}
void PrintConfigs() {
@@ -232,6 +230,7 @@
builder.add_supply_voltage(supply_voltage());
builder.add_supply_current(supply_current());
builder.add_torque_current(torque_current());
+ builder.add_duty_cycle(duty_cycle());
double invert =
(inverted_ ==
@@ -250,6 +249,7 @@
float supply_voltage() const { return supply_voltage_.GetValue().value(); }
float supply_current() const { return supply_current_.GetValue().value(); }
float torque_current() const { return torque_current_.GetValue().value(); }
+ float duty_cycle() const { return duty_cycle_.GetValue().value(); }
float position() const { return position_.GetValue().value(); }
// returns the monotonic timestamp of the latest timesynced reading in the
@@ -275,12 +275,17 @@
ctre::phoenixpro::StatusSignalValue<units::current::ampere_t> supply_current_,
torque_current_;
ctre::phoenixpro::StatusSignalValue<units::angle::turn_t> position_;
+ ctre::phoenixpro::StatusSignalValue<units::dimensionless::scalar_t>
+ duty_cycle_;
};
class CANSensorReader {
public:
- CANSensorReader(aos::EventLoop *event_loop)
+ CANSensorReader(
+ aos::EventLoop *event_loop,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry)
: event_loop_(event_loop),
+ signals_(signals_registry.begin(), signals_registry.end()),
can_position_sender_(
event_loop->MakeSender<drivetrain::CANPosition>("/drivetrain")),
roller_falcon_data_(std::nullopt) {
@@ -294,12 +299,6 @@
});
}
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
- *get_signals_registry() {
- return &signals_;
- };
-
void set_falcons(std::shared_ptr<Falcon> right_front,
std::shared_ptr<Falcon> right_back,
std::shared_ptr<Falcon> right_under,
@@ -323,16 +322,8 @@
private:
void Loop() {
- CHECK_EQ(signals_.size(), 28u);
ctre::phoenix::StatusCode status =
- ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(
- 2000_ms, {signals_[0], signals_[1], signals_[2], signals_[3],
- signals_[4], signals_[5], signals_[6], signals_[7],
- signals_[8], signals_[9], signals_[10], signals_[11],
- signals_[12], signals_[13], signals_[14], signals_[15],
- signals_[16], signals_[17], signals_[18], signals_[19],
- signals_[20], signals_[21], signals_[22], signals_[23],
- signals_[24], signals_[25], signals_[26], signals_[27]});
+ ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(2000_ms, signals_);
if (!status.IsOK()) {
AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
@@ -376,6 +367,7 @@
roller_falcon_data.supply_voltage = roller_falcon_->supply_voltage();
roller_falcon_data.device_temp = roller_falcon_->device_temp();
roller_falcon_data.position = -roller_falcon_->position();
+ roller_falcon_data.duty_cycle = roller_falcon_->duty_cycle();
roller_falcon_data_ =
std::make_optional<superstructure::CANFalconT>(roller_falcon_data);
}
@@ -383,9 +375,7 @@
aos::EventLoop *event_loop_;
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
- signals_;
+ const std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_;
aos::Sender<drivetrain::CANPosition> can_position_sender_;
std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
@@ -934,25 +924,27 @@
std::shared_ptr<frc::DigitalOutput> superstructure_reading =
make_unique<frc::DigitalOutput>(25);
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry;
+ std::shared_ptr<Falcon> right_front =
+ std::make_shared<Falcon>(1, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> right_back =
+ std::make_shared<Falcon>(2, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> right_under =
+ std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> left_front =
+ std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> left_back =
+ std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> left_under =
+ std::make_shared<Falcon>(6, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> roller =
+ std::make_shared<Falcon>(13, "Drivetrain Bus", &signals_registry);
+
// Thread 3.
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
- CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
-
- std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
- 1, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
- 2, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
- 3, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
- 4, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
- 5, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
- 6, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> roller = std::make_shared<Falcon>(
- 13, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop,
+ std::move(signals_registry));
can_sensor_reader.set_falcons(right_front, right_back, right_under,
left_front, left_back, left_under, roller);
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index cf3a14e..452a570 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -268,6 +268,14 @@
"max_size": 448
},
{
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "roborio",
+ "frequency": 6000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/drivetrain",
"type": "y2023.control_loops.drivetrain.CANPosition",
"source_node": "roborio",
@@ -566,6 +574,13 @@
"nodes": [
"roborio"
]
+ },
+ {
+ "name": "can_logger",
+ "executable_name": "can_logger",
+ "nodes": [
+ "roborio"
+ ]
}
],
"maps": [