Merge "Don't assume orientation of target in y2020 localizer"
diff --git a/aos/events/logging/log_reader.cc b/aos/events/logging/log_reader.cc
index a1269e1..5fe61cd 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -825,6 +825,9 @@
// all. Read the rest of the messages and drop them on the floor while
// doing some basic validation.
while (state->OldestMessageTime() != BootTimestamp::max_time()) {
+ // TODO(austin): This force queues up the rest of the log file for all
+ // the other nodes. We should do this through the timer instead to
+ // keep memory usage down.
TimestampedMessage next = state->PopOldest();
// Make sure that once we have seen the last message on a channel,
// data doesn't start back up again. If the user wants to play
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 8e536c8..81bf18c 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -6,6 +6,7 @@
#include <utility>
#include <vector>
+#include "absl/container/btree_map.h"
#include "aos/events/logging/logfile_utils.h"
#include "aos/flatbuffer_merge.h"
#include "aos/flatbuffers.h"
@@ -61,6 +62,20 @@
return header->has_configuration();
}
+bool HasNewTimestamps(const LogFileHeader *header) {
+ if (header->has_oldest_remote_monotonic_timestamps()) {
+ CHECK(header->has_oldest_local_monotonic_timestamps());
+ CHECK(header->has_oldest_remote_unreliable_monotonic_timestamps());
+ CHECK(header->has_oldest_local_unreliable_monotonic_timestamps());
+ return true;
+ } else {
+ CHECK(!header->has_oldest_local_monotonic_timestamps());
+ CHECK(!header->has_oldest_remote_unreliable_monotonic_timestamps());
+ CHECK(!header->has_oldest_local_unreliable_monotonic_timestamps());
+ return false;
+ }
+}
+
} // namespace
void FindLogs(std::vector<std::string> *files, std::string filename) {
@@ -190,6 +205,32 @@
std::map<std::string, std::vector<std::string>> boots;
};
+// For a pair of nodes, this holds the oldest times that messages were
+// transfered.
+struct BootPairTimes {
+ // Pair of local and remote timestamps for the oldest message forwarded to
+ // this node.
+ monotonic_clock::time_point oldest_remote_monotonic_timestamp;
+ monotonic_clock::time_point oldest_local_monotonic_timestamp;
+
+ // Pair of local and remote timestamps for the oldest unreliable message
+ // forwarded to this node.
+ monotonic_clock::time_point oldest_remote_unreliable_monotonic_timestamp;
+ monotonic_clock::time_point oldest_local_unreliable_monotonic_timestamp;
+};
+
+std::ostream &operator<<(std::ostream &out, const BootPairTimes &time) {
+ out << "{.oldest_remote_monotonic_timestamp="
+ << time.oldest_remote_monotonic_timestamp
+ << ", .oldest_local_monotonic_timestamp="
+ << time.oldest_local_monotonic_timestamp
+ << ", .oldest_remote_unreliable_monotonic_timestamp="
+ << time.oldest_remote_unreliable_monotonic_timestamp
+ << ", .oldest_local_unreliable_monotonic_timestamp="
+ << time.oldest_local_unreliable_monotonic_timestamp << "}";
+ return out;
+}
+
// Helper class to make it easier to sort a list of log files into
// std::vector<LogFile>
struct PartsSorter {
@@ -200,6 +241,19 @@
std::map<std::string, std::shared_ptr<const Configuration>>
config_sha256_lookup;
+ // List of all config sha256's we have found.
+ std::set<std::string> config_sha256_list;
+
+ // Map from a observed pair of boots to the associated timestamps.
+ // logger_node -> logger_node_boot_uuid -> destination_node index ->
+ // destination_boot_uuid -> times.
+ absl::btree_map<
+ std::string,
+ absl::btree_map<
+ std::string,
+ absl::btree_map<size_t, absl::btree_map<std::string, BootPairTimes>>>>
+ boot_times;
+
// Map holding the log_event_uuid -> second map. The second map holds the
// parts_uuid -> list of parts for sorting.
std::map<std::string, UnsortedLogPartsMap> parts_list;
@@ -211,9 +265,13 @@
// Populates the class's datastructures from the input file list.
void PopulateFromFiles(const std::vector<std::string> &parts);
- // Wrangle parts_list into a map of boot uuids -> boot counts.
+ // Wrangles everything into a map of boot uuids -> boot counts.
MapBoots ComputeBootCounts();
+ // Computes the boot constraints to solve for old and new logs.
+ std::map<std::string, NodeBootState> ComputeOldBootConstraints();
+ std::map<std::string, NodeBootState> ComputeNewBootConstraints();
+
// Reformats old_parts into a list of logfiles and returns it. This destroys
// state in PartsSorter.
std::vector<LogFile> FormatOldParts();
@@ -270,9 +328,9 @@
? log_header->message().source_node_boot_uuid()->string_view()
: "";
- const std::string_view configuration_sha256 =
+ std::string configuration_sha256 =
log_header->message().has_configuration_sha256()
- ? log_header->message().configuration_sha256()->string_view()
+ ? log_header->message().configuration_sha256()->str()
: "";
if (ConfigOnly(&log_header->message())) {
@@ -286,6 +344,7 @@
hash, std::shared_ptr<const Configuration>(
header, header->message().configuration()));
}
+ config_sha256_list.emplace(hash);
continue;
}
@@ -294,9 +353,34 @@
if (configuration_sha256.empty()) {
CHECK(log_header->message().has_configuration())
<< ": Failed to find header on " << part;
+ // If we don't have a configuration_sha256, we need to have a
+ // configuration directly inside the header. This ends up being a bit
+ // unwieldy to deal with, so let's instead copy the configuration, hash
+ // it, and then use the configuration_sha256 that we found to continue
+ // down the old code path.
+
+ // Do a recursive copy to normalize the flatbuffer. Different
+ // configurations can be built different ways, and can even have their
+ // vtable out of order. Don't think and just trigger a copy.
+ const FlatbufferDetachedBuffer<Configuration> config_copy =
+ RecursiveCopyFlatBuffer(log_header->message().configuration());
+ std::string config_copy_sha256 = Sha256(config_copy.span());
+
+ if (config_sha256_lookup.find(config_copy_sha256) ==
+ config_sha256_lookup.end()) {
+ auto header =
+ std::make_shared<SizePrefixedFlatbufferVector<LogFileHeader>>(
+ *log_header);
+ config_sha256_lookup.emplace(
+ config_copy_sha256, std::shared_ptr<const Configuration>(
+ header, header->message().configuration()));
+ }
+ config_sha256_list.emplace(config_copy_sha256);
+ configuration_sha256 = std::move(config_copy_sha256);
} else {
CHECK(!log_header->message().has_configuration())
<< ": Found header where one shouldn't be on " << part;
+ config_sha256_list.emplace(configuration_sha256);
}
// Looks like an old log. No UUID, index, and also single node. We have
@@ -327,6 +411,7 @@
old_parts.emplace_back();
old_parts.back().parts.monotonic_start_time = monotonic_start_time;
old_parts.back().parts.realtime_start_time = realtime_start_time;
+ old_parts.back().parts.config_sha256 = configuration_sha256;
old_parts.back().unsorted_parts.emplace_back(
std::make_pair(first_message_time, part));
old_parts.back().name = name;
@@ -334,6 +419,7 @@
result->unsorted_parts.emplace_back(
std::make_pair(first_message_time, part));
CHECK_EQ(result->name, name);
+ CHECK_EQ(result->parts.config_sha256, configuration_sha256);
}
continue;
}
@@ -377,7 +463,7 @@
}
VLOG(1) << "Parts: " << parts_uuid << ", source boot uuid "
- << source_boot_uuid;
+ << source_boot_uuid << " " << parts_index;
auto it = log_it->second.unsorted_parts.find(
std::pair(parts_uuid, std::string(source_boot_uuid)));
if (it == log_it->second.unsorted_parts.end()) {
@@ -397,6 +483,135 @@
CHECK_EQ(it->second.config_sha256, configuration_sha256);
}
+ // We've got a newer log with boot_uuids, and oldest timestamps. Fill in
+ // this->boot_times with the info we have found.
+ if (HasNewTimestamps(&log_header->message())) {
+ auto node_boot_times_it = boot_times.find(logger_node);
+ if (node_boot_times_it == boot_times.end()) {
+ node_boot_times_it =
+ boot_times
+ .emplace(
+ logger_node,
+ absl::btree_map<
+ std::string,
+ absl::btree_map<
+ size_t,
+ absl::btree_map<std::string, BootPairTimes>>>())
+ .first;
+ }
+ auto source_boot_times_it =
+ node_boot_times_it->second.find(std::string(logger_boot_uuid));
+
+ if (source_boot_times_it == node_boot_times_it->second.end()) {
+ source_boot_times_it =
+ node_boot_times_it->second
+ .emplace(
+ logger_boot_uuid,
+ absl::btree_map<
+ size_t, absl::btree_map<std::string, BootPairTimes>>())
+ .first;
+ }
+
+ // Older logs don't have our fancy new boot_uuids and oldest timestamps.
+ // So only fill those out when we find them.
+ CHECK(log_header->message().has_boot_uuids());
+ const size_t boot_uuids_size = log_header->message().boot_uuids()->size();
+ CHECK_EQ(
+ boot_uuids_size,
+ log_header->message().oldest_local_monotonic_timestamps()->size());
+ CHECK_EQ(
+ boot_uuids_size,
+ log_header->message().oldest_remote_monotonic_timestamps()->size());
+ CHECK_EQ(boot_uuids_size,
+ log_header->message()
+ .oldest_local_unreliable_monotonic_timestamps()
+ ->size());
+ CHECK_EQ(boot_uuids_size,
+ log_header->message()
+ .oldest_remote_unreliable_monotonic_timestamps()
+ ->size());
+ CHECK(!logger_boot_uuid.empty());
+ for (size_t node_index = 0; node_index < boot_uuids_size; ++node_index) {
+ const std::string_view boot_uuid =
+ log_header->message().boot_uuids()->Get(node_index)->string_view();
+
+ const monotonic_clock::time_point oldest_local_monotonic_timestamp(
+ chrono::nanoseconds(
+ log_header->message().oldest_local_monotonic_timestamps()->Get(
+ node_index)));
+ const monotonic_clock::time_point oldest_remote_monotonic_timestamp(
+ chrono::nanoseconds(
+ log_header->message().oldest_remote_monotonic_timestamps()->Get(
+ node_index)));
+ const monotonic_clock::time_point
+ oldest_local_unreliable_monotonic_timestamp(chrono::nanoseconds(
+ log_header->message()
+ .oldest_local_unreliable_monotonic_timestamps()
+ ->Get(node_index)));
+ const monotonic_clock::time_point
+ oldest_remote_unreliable_monotonic_timestamp(chrono::nanoseconds(
+ log_header->message()
+ .oldest_remote_unreliable_monotonic_timestamps()
+ ->Get(node_index)));
+ if (boot_uuid.empty() || boot_uuid == logger_boot_uuid) {
+ CHECK_EQ(oldest_local_monotonic_timestamp, monotonic_clock::max_time);
+ CHECK_EQ(oldest_remote_monotonic_timestamp,
+ monotonic_clock::max_time);
+ CHECK_EQ(oldest_local_unreliable_monotonic_timestamp,
+ monotonic_clock::max_time);
+ CHECK_EQ(oldest_remote_unreliable_monotonic_timestamp,
+ monotonic_clock::max_time);
+ continue;
+ }
+
+ // Now, we have a valid pairing.
+ auto destination_boot_times_it =
+ source_boot_times_it->second.find(node_index);
+ if (destination_boot_times_it == source_boot_times_it->second.end()) {
+ destination_boot_times_it =
+ source_boot_times_it->second
+ .emplace(node_index,
+ absl::btree_map<std::string, BootPairTimes>())
+ .first;
+ }
+
+ auto boot_times_it =
+ destination_boot_times_it->second.find(std::string(boot_uuid));
+
+ if (boot_times_it == destination_boot_times_it->second.end()) {
+ // We have a new boot UUID pairing. Copy over the data we have.
+ destination_boot_times_it->second.emplace(
+ boot_uuid,
+ BootPairTimes{.oldest_remote_monotonic_timestamp =
+ oldest_remote_monotonic_timestamp,
+ .oldest_local_monotonic_timestamp =
+ oldest_local_monotonic_timestamp,
+ .oldest_remote_unreliable_monotonic_timestamp =
+ oldest_remote_unreliable_monotonic_timestamp,
+ .oldest_local_unreliable_monotonic_timestamp =
+ oldest_local_unreliable_monotonic_timestamp});
+ } else {
+ // If we found an existing entry, update the min to be the min of all
+ // records. This lets us combine info from multiple part files.
+ if (oldest_remote_monotonic_timestamp <
+ boot_times_it->second.oldest_remote_monotonic_timestamp) {
+ boot_times_it->second.oldest_remote_monotonic_timestamp =
+ oldest_remote_monotonic_timestamp;
+ boot_times_it->second.oldest_local_monotonic_timestamp =
+ oldest_local_monotonic_timestamp;
+ }
+ if (oldest_remote_unreliable_monotonic_timestamp <
+ boot_times_it->second
+ .oldest_remote_unreliable_monotonic_timestamp) {
+ boot_times_it->second.oldest_remote_unreliable_monotonic_timestamp =
+ oldest_remote_unreliable_monotonic_timestamp;
+ boot_times_it->second.oldest_local_unreliable_monotonic_timestamp =
+ oldest_local_unreliable_monotonic_timestamp;
+ }
+ }
+ }
+ }
+
// First part might be min_time. If it is, try to put a better time on it.
if (it->second.monotonic_start_time == monotonic_clock::min_time) {
it->second.monotonic_start_time = monotonic_start_time;
@@ -421,8 +636,6 @@
std::vector<LogFile> PartsSorter::FormatOldParts() {
// Now reformat old_parts to be in the right datastructure to report.
- std::map<std::string, std::shared_ptr<const Configuration>>
- copied_config_sha256;
CHECK(!old_parts.empty());
std::vector<LogFile> result;
@@ -452,16 +665,11 @@
std::string config_copy_sha256 = Sha256(config_copy.span());
- auto it = copied_config_sha256.find(config_copy_sha256);
- if (it != copied_config_sha256.end()) {
- log_file.config = it->second;
- } else {
- std::shared_ptr<const Configuration> config(
- header, header->message().configuration());
+ auto it = config_sha256_lookup.find(p.parts.config_sha256);
+ CHECK(it != config_sha256_lookup.end());
- copied_config_sha256.emplace(std::move(config_copy_sha256), config);
- log_file.config = config;
- }
+ log_file.config = it->second;
+ log_file.config_sha256 = p.parts.config_sha256;
for (std::pair<monotonic_clock::time_point, std::string> &f :
p.unsorted_parts) {
@@ -479,11 +687,140 @@
return result;
}
-MapBoots PartsSorter::ComputeBootCounts() {
+std::map<std::string, NodeBootState> PartsSorter::ComputeNewBootConstraints() {
std::map<std::string, NodeBootState> boot_constraints;
- // TODO(austin): This is the "old" way. Once we have better ordering info in
- // headers, we should use it.
+ CHECK_EQ(config_sha256_list.size(), 1u) << ": Found more than one config";
+ auto config_it = config_sha256_lookup.find(*config_sha256_list.begin());
+ CHECK(config_it != config_sha256_lookup.end())
+ << ": Failed to find a config with a sha256 of "
+ << *config_sha256_list.begin();
+ const Configuration *config = config_it->second.get();
+
+ for (const auto &node_boot_times : boot_times) {
+ const std::string &logger_node_name = node_boot_times.first;
+
+ // We know nothing about the order of the logger node's boot, but we
+ // know it happened. If there is only 1 single boot, the constraint
+ // code will happily mark it as boot 0. Otherwise we'll get the
+ // appropriate boot count if it can be computed or an error.
+ //
+ // Add it to the boots list to kick this off.
+ auto logger_node_boot_constraints_it =
+ boot_constraints.find(logger_node_name);
+ if (logger_node_boot_constraints_it == boot_constraints.end()) {
+ logger_node_boot_constraints_it =
+ boot_constraints
+ .insert(std::make_pair(logger_node_name, NodeBootState()))
+ .first;
+ }
+
+ for (const auto &source_boot_time : node_boot_times.second) {
+ const std::string &logger_boot_uuid = source_boot_time.first;
+ logger_node_boot_constraints_it->second.boots.insert(logger_boot_uuid);
+
+ for (const auto &source_nodes : source_boot_time.second) {
+ const std::string source_node_name =
+ config->nodes()->Get(source_nodes.first)->name()->str();
+
+ // Now, we have a bunch of remote boots for the same local boot and
+ // remote node. We want to sort them by observed local time. This will
+ // tell us which ones happened first.
+ std::vector<std::pair<std::string, BootPairTimes>> source_boot_times;
+ for (const auto &boot_time : source_nodes.second) {
+ source_boot_times.emplace_back(boot_time);
+ }
+ std::sort(
+ source_boot_times.begin(), source_boot_times.end(),
+ [](const std::pair<std::string, BootPairTimes> &a,
+ const std::pair<std::string, BootPairTimes> &b) {
+ // There are cases where we will only have a reliable timestamp.
+ // In that case, we need to use oldest_local_monotonic_timestamp.
+ // But, that may result in collisions if the same message gets
+ // forwarded to both boots, so it will have the same timestamp.
+ // Solve that by breaking the tie with the unreliable messages.
+ if (a.second.oldest_local_monotonic_timestamp ==
+ b.second.oldest_local_monotonic_timestamp) {
+ CHECK_NE(a.second.oldest_local_unreliable_monotonic_timestamp,
+ b.second.oldest_local_unreliable_monotonic_timestamp);
+ return a.second.oldest_local_unreliable_monotonic_timestamp <
+ b.second.oldest_local_unreliable_monotonic_timestamp;
+ } else {
+ return a.second.oldest_local_monotonic_timestamp <
+ b.second.oldest_local_monotonic_timestamp;
+ }
+ });
+
+ // Now take our sorted list and build up constraints so we can solve.
+ for (size_t boot_id = 0; boot_id < source_boot_times.size();
+ ++boot_id) {
+ const std::pair<std::string, BootPairTimes> &boot_time =
+ source_boot_times[boot_id];
+ const std::string &source_boot_uuid = boot_time.first;
+
+ auto source_node_boot_constraints_it =
+ boot_constraints.find(source_node_name);
+ if (source_node_boot_constraints_it == boot_constraints.end()) {
+ source_node_boot_constraints_it =
+ boot_constraints
+ .insert(std::make_pair(source_node_name, NodeBootState()))
+ .first;
+ }
+
+ // Track that this boot happened.
+ source_node_boot_constraints_it->second.boots.insert(
+ source_boot_uuid);
+
+ if (boot_id > 0) {
+ // And now add the constraints. The vector is in order, so all we
+ // need to do is to iterate through it and put pairwise constraints
+ // in there.
+ std::map<std::string, std::vector<std::pair<std::string, bool>>>
+ &per_node_boot_constraints =
+ source_node_boot_constraints_it->second.constraints;
+
+ const std::pair<std::string, BootPairTimes> &prior_boot_time =
+ source_boot_times[boot_id - 1];
+ const std::string &prior_boot_uuid = prior_boot_time.first;
+
+ auto first_per_boot_constraints =
+ per_node_boot_constraints.find(prior_boot_uuid);
+ if (first_per_boot_constraints == per_node_boot_constraints.end()) {
+ first_per_boot_constraints =
+ per_node_boot_constraints
+ .insert(std::make_pair(
+ prior_boot_uuid,
+ std::vector<std::pair<std::string, bool>>()))
+ .first;
+ }
+
+ auto second_per_boot_constraints =
+ per_node_boot_constraints.find(source_boot_uuid);
+ if (second_per_boot_constraints ==
+ per_node_boot_constraints.end()) {
+ second_per_boot_constraints =
+ per_node_boot_constraints
+ .insert(std::make_pair(
+ source_boot_uuid,
+ std::vector<std::pair<std::string, bool>>()))
+ .first;
+ }
+
+ first_per_boot_constraints->second.emplace_back(
+ std::make_pair(source_boot_uuid, true));
+ second_per_boot_constraints->second.emplace_back(
+ std::make_pair(prior_boot_uuid, false));
+ }
+ }
+ }
+ }
+ }
+
+ return boot_constraints;
+}
+
+std::map<std::string, NodeBootState> PartsSorter::ComputeOldBootConstraints() {
+ std::map<std::string, NodeBootState> boot_constraints;
for (std::pair<const std::string, UnsortedLogPartsMap> &logs : parts_list) {
{
// We know nothing about the order of the logger node's boot, but we know
@@ -596,7 +933,10 @@
for (size_t i = 1; i < boot_parts_index_ranges.second.size(); ++i) {
CHECK_LT(boot_parts_index_ranges.second[i - 1].second.second,
boot_parts_index_ranges.second[i].second.first)
- << ": Overlapping parts_index, please investigate";
+ << ": Overlapping parts_index, please investigate "
+ << boot_parts_index_ranges.first << " "
+ << boot_parts_index_ranges.second[i - 1].first << " "
+ << boot_parts_index_ranges.second[i].first;
auto first_per_boot_constraints = per_node_boot_constraints.find(
boot_parts_index_ranges.second[i - 1].first);
if (first_per_boot_constraints == per_node_boot_constraints.end()) {
@@ -626,6 +966,13 @@
}
}
}
+ return boot_constraints;
+}
+
+MapBoots PartsSorter::ComputeBootCounts() {
+ const std::map<std::string, NodeBootState> boot_constraints =
+ boot_times.empty() ? ComputeOldBootConstraints()
+ : ComputeNewBootConstraints();
// Print out our discovered constraints on request.
if (VLOG_IS_ON(2)) {
@@ -657,7 +1004,7 @@
// This doesn't need to catch all the cases, it just needs to report when it
// fails.
MapBoots boots;
- for (std::pair<const std::string, NodeBootState> &node_state :
+ for (const std::pair<const std::string, NodeBootState> &node_state :
boot_constraints) {
CHECK_GT(node_state.second.boots.size(), 0u)
<< ": Need a boot from each node.";
@@ -765,8 +1112,6 @@
MapBoots map_boot_counts = ComputeBootCounts();
boot_counts->boot_count_map = std::move(map_boot_counts.boot_count_map);
- std::map<std::string, std::shared_ptr<const Configuration>>
- copied_config_sha256;
// Now, sort them and produce the final vector form.
std::vector<LogFile> result;
result.reserve(parts_list.size());
@@ -821,59 +1166,22 @@
new_parts.parts.emplace_back(std::move(p.first));
}
- if (!parts.second.config_sha256.empty()) {
- // The easy case. We've got a sha256 to point to, so go look it up.
- // Abort if it doesn't exist.
- auto it = config_sha256_lookup.find(parts.second.config_sha256);
- CHECK(it != config_sha256_lookup.end())
- << ": Failed to find a matching config with a SHA256 of "
- << parts.second.config_sha256;
- new_parts.config_sha256 = std::move(parts.second.config_sha256);
- new_parts.config = it->second;
- if (!seen_part) {
- new_file.config_sha256 = new_parts.config_sha256;
- new_file.config = new_parts.config;
- config_sha256 = new_file.config_sha256;
- } else {
- CHECK_EQ(config_sha256, new_file.config_sha256)
- << ": Mismatched configs in " << new_file;
- }
+ CHECK(!parts.second.config_sha256.empty());
+ // The easy case. We've got a sha256 to point to, so go look it up.
+ // Abort if it doesn't exist.
+ auto it = config_sha256_lookup.find(parts.second.config_sha256);
+ CHECK(it != config_sha256_lookup.end())
+ << ": Failed to find a matching config with a SHA256 of "
+ << parts.second.config_sha256;
+ new_parts.config_sha256 = std::move(parts.second.config_sha256);
+ new_parts.config = it->second;
+ if (!seen_part) {
+ new_file.config_sha256 = new_parts.config_sha256;
+ new_file.config = new_parts.config;
+ config_sha256 = new_file.config_sha256;
} else {
- CHECK(config_sha256.empty())
- << ": Part " << new_parts
- << " is missing a sha256 but other parts have one.";
- if (!seen_part) {
- // We want to use a single Configuration flatbuffer for all the parts
- // to make downstream easier. Since this is an old log, it doesn't
- // have a SHA256 in the header to rely on, so we need a way to detect
- // duplicates.
- //
- // SHA256 is decently fast, so use that as a representative hash of
- // the header.
- auto header =
- std::make_shared<SizePrefixedFlatbufferVector<LogFileHeader>>(
- std::move(*ReadHeader(new_parts.parts[0])));
-
- // Do a recursive copy to normalize the flatbuffer. Different
- // configurations can be built different ways, and can even have their
- // vtable out of order. Don't think and just trigger a copy.
- FlatbufferDetachedBuffer<Configuration> config_copy =
- RecursiveCopyFlatBuffer(header->message().configuration());
-
- std::string config_copy_sha256 = Sha256(config_copy.span());
-
- auto it = copied_config_sha256.find(config_copy_sha256);
- if (it != copied_config_sha256.end()) {
- new_file.config = it->second;
- } else {
- std::shared_ptr<const Configuration> config(
- header, header->message().configuration());
-
- copied_config_sha256.emplace(std::move(config_copy_sha256), config);
- new_file.config = config;
- }
- }
- new_parts.config = new_file.config;
+ CHECK_EQ(config_sha256, new_file.config_sha256)
+ << ": Mismatched configs in " << new_file;
}
seen_part = true;
@@ -883,11 +1191,10 @@
}
{
- CHECK_EQ(config_sha256_lookup.size() + copied_config_sha256.size(), 1u)
+ CHECK_EQ(config_sha256_lookup.size(), 1u)
<< ": We only support log files with 1 config in them.";
std::shared_ptr<const aos::Configuration> config =
- config_sha256_lookup.empty() ? copied_config_sha256.begin()->second
- : config_sha256_lookup.begin()->second;
+ config_sha256_lookup.begin()->second;
boot_counts->boots.resize(configuration::NodesCount(config.get()));
for (std::pair<const std::string, std::vector<std::string>> &boots :
diff --git a/third_party/rawrtc/usrsctp/usrsctplib/netinet/sctp_usrreq.c b/third_party/rawrtc/usrsctp/usrsctplib/netinet/sctp_usrreq.c
index 7d14a92..9584ae7 100755
--- a/third_party/rawrtc/usrsctp/usrsctplib/netinet/sctp_usrreq.c
+++ b/third_party/rawrtc/usrsctp/usrsctplib/netinet/sctp_usrreq.c
@@ -196,16 +196,18 @@
sctp_stop_main_timer();
#endif
#if defined(__Userspace__)
+#if defined(THREAD_SUPPORT)
#if defined(INET) || defined(INET6)
recv_thread_destroy();
-#endif
+#endif // INIT || INET6
+// TODO(jkuszmaul): The corresponding #if in recv_thread_init() actually uses Darwin || DragonFly || FreeBSD--not !Windows.
#if !defined(__Userspace_os_Windows)
#if defined(INET) || defined(INET6)
if (SCTP_BASE_VAR(userspace_route) != -1) {
pthread_join(SCTP_BASE_VAR(recvthreadroute), NULL);
}
-#endif
-#endif
+#endif // INIT || INET6
+#endif // !__Userspace_os_Windows
#ifdef INET
if (SCTP_BASE_VAR(userspace_rawsctp) != -1) {
#if defined(__Userspace_os_Windows)
@@ -213,7 +215,7 @@
CloseHandle(SCTP_BASE_VAR(recvthreadraw));
#else
pthread_join(SCTP_BASE_VAR(recvthreadraw), NULL);
-#endif
+#endif // __Userspace_os_Windows
}
if (SCTP_BASE_VAR(userspace_udpsctp) != -1) {
#if defined(__Userspace_os_Windows)
@@ -221,9 +223,9 @@
CloseHandle(SCTP_BASE_VAR(recvthreadudp));
#else
pthread_join(SCTP_BASE_VAR(recvthreadudp), NULL);
-#endif
+#endif // __Userspace_os_Windows
}
-#endif
+#endif // INET
#ifdef INET6
if (SCTP_BASE_VAR(userspace_rawsctp6) != -1) {
#if defined(__Userspace_os_Windows)
@@ -231,7 +233,7 @@
CloseHandle(SCTP_BASE_VAR(recvthreadraw6));
#else
pthread_join(SCTP_BASE_VAR(recvthreadraw6), NULL);
-#endif
+#endif // __Userspace_os_Windows
}
if (SCTP_BASE_VAR(userspace_udpsctp6) != -1) {
#if defined(__Userspace_os_Windows)
@@ -239,17 +241,18 @@
CloseHandle(SCTP_BASE_VAR(recvthreadudp6));
#else
pthread_join(SCTP_BASE_VAR(recvthreadudp6), NULL);
-#endif
+#endif // __Userspace_os_Windows
}
-#endif
+#endif // INET6
SCTP_BASE_VAR(timer_thread_should_exit) = 1;
#if defined(__Userspace_os_Windows)
WaitForSingleObject(SCTP_BASE_VAR(timer_thread), INFINITE);
CloseHandle(SCTP_BASE_VAR(timer_thread));
#else
pthread_join(SCTP_BASE_VAR(timer_thread), NULL);
-#endif
-#endif
+#endif // __Userspace_os_Windows
+#endif // THREAD_SUPPORT
+#endif // __Userspace__
sctp_pcb_finish();
#if defined(__Windows__)
sctp_finish_sysctls();
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index ed2b4b9..d6f3716 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -19,6 +19,8 @@
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_bool(skip_sift, false,
"If true don't run any feature extraction. Just forward images.");
+DEFINE_bool(ransac_pose, false,
+ "If true, do pose estimate with RANSAC; else, use ITERATIVE mode.");
namespace frc971 {
namespace vision {
@@ -436,9 +438,27 @@
cv::Mat R_field_camera_vec, R_field_camera, T_field_camera;
// Compute the pose of the camera (global origin relative to camera)
- cv::solvePnPRansac(per_image_good_match.training_points_3d,
- per_image_good_match.query_points, CameraIntrinsics(),
- CameraDistCoeffs(), R_camera_field_vec, T_camera_field);
+ if (FLAGS_ransac_pose) {
+ // RANSAC computation is designed to be more robust to outliers.
+ // But, we found it bounces around a lot, even with identical points
+ cv::solvePnPRansac(per_image_good_match.training_points_3d,
+ per_image_good_match.query_points, CameraIntrinsics(),
+ CameraDistCoeffs(), R_camera_field_vec,
+ T_camera_field);
+ } else {
+ // ITERATIVE mode is potentially less robust to outliers, but we
+ // found it to be more stable
+ //
+ // TODO<Jim>: We should explore feeding an initial estimate into
+ // the pose calculations. I found this to help guide the solution
+ // to something hearby, but it also can lock it into an incorrect
+ // solution
+ cv::solvePnP(per_image_good_match.training_points_3d,
+ per_image_good_match.query_points, CameraIntrinsics(),
+ CameraDistCoeffs(), R_camera_field_vec, T_camera_field,
+ false, CV_ITERATIVE);
+ }
+
CHECK_EQ(cv::Size(1, 3), T_camera_field.size());
// Convert to float32's (from float64) to be compatible with the rest
diff --git a/y2021_bot3/constants.h b/y2021_bot3/constants.h
index 02c08bc..fd6ab72 100644
--- a/y2021_bot3/constants.h
+++ b/y2021_bot3/constants.h
@@ -27,6 +27,8 @@
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
+ static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
+ static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
index c016cad..718edca 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -3,6 +3,13 @@
namespace y2021_bot3.control_loops.superstructure;
table Goal {
+ // Intake speed for the intake, positive number pulls balls into the robot
+ intake_speed:double (id:0);
+
+ // Outtake speed for the outtake roller, positive number means it is outtaking balls
+ outtake_speed:double (id:1);
+
+ // TODO: Add climber
}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
index 88bdf2f..47b9476 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -1,6 +1,13 @@
namespace y2021_bot3.control_loops.superstructure;
table Output {
+ // Voltage for intake motor, positive number is intaking balls
+ intake_volts:double (id:0);
+
+ // Voltage for outtake motor, positive number is outtaking balls
+ outtake_volts:double (id:1);
+
+ // TODO: Add climber
}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
index e615c62..4f94e76 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2021_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -10,6 +10,13 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
+ // Intake speed for the intake, positive number means it is intaking balls
+ intake_speed:double (id:2);
+
+ // Outtake speed for the outtake roller, positive number outtakes balls
+ outtake_speed:double (id:3);
+
+ // TODO: Add climber
}
root_type Status;
diff --git a/y2021_bot3/wpilib_interface.cc b/y2021_bot3/wpilib_interface.cc
index 3b6b691..1ad2f52 100644
--- a/y2021_bot3/wpilib_interface.cc
+++ b/y2021_bot3/wpilib_interface.cc
@@ -31,6 +31,7 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
@@ -192,10 +193,42 @@
: ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
event_loop, "/superstructure") {}
+ void set_intake_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ intake_falcon_ = ::std::move(t);
+ ConfigureFalcon(intake_falcon_.get());
+ }
+
+ void set_outtake_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ outtake_falcon_ = ::std::move(t);
+ ConfigureFalcon(outtake_falcon_.get());
+ }
+
private:
- void Write(const superstructure::Output & /*output*/) override {}
+ void ConfigureFalcon(::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+ falcon->ConfigSupplyCurrentLimit({true, Values::kRollerSupplyCurrentLimit(),
+ Values::kRollerSupplyCurrentLimit(), 0});
+ falcon->ConfigStatorCurrentLimit({true, Values::kRollerStatorCurrentLimit(),
+ Values::kRollerStatorCurrentLimit(), 0});
+ }
+
+ void WriteToFalcon(const double voltage,
+ ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+ falcon->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ }
+
+ void Write(const superstructure::Output &output) override {
+ WriteToFalcon(output.intake_volts(), intake_falcon_.get());
+ WriteToFalcon(output.outtake_volts(), outtake_falcon_.get());
+ }
void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> intake_falcon_,
+ outtake_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -237,6 +270,10 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
SuperstructureWriter superstructure_writer(&output_event_loop);
+ superstructure_writer.set_intake_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ superstructure_writer.set_outtake_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
AddLoop(&output_event_loop);