Merge "Warn when a channel isn't logged and terminates reading a log"
diff --git a/WORKSPACE b/WORKSPACE
index e7de0e2..285b1f0 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -723,11 +723,17 @@
urls = ["https://www.frc971.org/Build-Dependencies/small_sample_logfile.fbs"],
)
-http_file(
+http_archive(
name = "drivetrain_replay",
- downloaded_file_path = "spinning_wheels_while_still.bfbs",
- sha256 = "8abe3bbf7ac7a3ab37ad8a313ec22fc244899d916f5e9037100b02e242f5fb45",
- urls = ["https://www.frc971.org/Build-Dependencies/spinning_wheels_while_still4.bfbs"],
+ build_file_content = """
+filegroup(
+ name = "drivetrain_replay",
+ srcs = glob(["**/*.bfbs"]),
+ visibility = ["//visibility:public"],
+)
+ """,
+ sha256 = "115dcd2fe005cb9cad3325707aa7f4466390c43a08555edf331c06c108bdf692",
+ url = "https://www.frc971.org/Build-Dependencies/2021-03-20_drivetrain_spin_wheels.tar.gz",
)
# OpenCV armhf (for raspberry pi)
diff --git a/aos/BUILD b/aos/BUILD
index 0159623..04293df 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -335,6 +335,7 @@
":flatbuffer_utils",
":flatbuffers",
":json_tokenizer",
+ "//aos/util:file",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
diff --git a/aos/aos_send.cc b/aos/aos_send.cc
index d0dea01..d94418a 100644
--- a/aos/aos_send.cc
+++ b/aos/aos_send.cc
@@ -39,6 +39,7 @@
cli_info.event_loop->MakeRawSender(channel);
flatbuffers::FlatBufferBuilder fbb(sender->fbb_allocator()->size(),
sender->fbb_allocator());
+ fbb.ForceDefaults(true);
fbb.Finish(aos::JsonToFlatbuffer(std::string_view(argv[1]), channel->schema(),
&fbb));
sender->Send(fbb.GetSize());
diff --git a/aos/events/logging/log_writer.cc b/aos/events/logging/log_writer.cc
index 0b08c94..f68bbf6 100644
--- a/aos/events/logging/log_writer.cc
+++ b/aos/events/logging/log_writer.cc
@@ -249,6 +249,10 @@
// Note: this ship may have already sailed, but we don't have to make it
// worse.
// TODO(austin): Test...
+ //
+ // This is safe to call here since we have set last_synchronized_time_ as the
+ // same time as in the header, and all the data before it should be logged
+ // without ordering concerns.
LogUntil(last_synchronized_time_);
timer_handler_->Setup(event_loop_->monotonic_now() + polling_period_,
@@ -260,7 +264,7 @@
CHECK(log_namer_) << ": Not logging right now";
if (end_time != aos::monotonic_clock::min_time) {
- LogUntil(end_time);
+ DoLogData(end_time);
}
timer_handler_->Disable();
diff --git a/aos/events/logging/log_writer.h b/aos/events/logging/log_writer.h
index f5b55a7..992633a 100644
--- a/aos/events/logging/log_writer.h
+++ b/aos/events/logging/log_writer.h
@@ -247,7 +247,9 @@
void WriteMissingTimestamps();
- // Fetches from each channel until all the data is logged.
+ // Fetches from each channel until all the data is logged. This is dangerous
+ // because it lets you log for more than 1 period. All calls need to verify
+ // that t isn't greater than 1 period in the future.
void LogUntil(monotonic_clock::time_point t);
void RecordFetchResult(aos::monotonic_clock::time_point start,
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 892dd2e..80d909b 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -80,6 +80,12 @@
closedir(directory);
}
+std::vector<std::string> FindLogs(std::string filename) {
+ std::vector<std::string> files;
+ FindLogs(&files, filename);
+ return files;
+}
+
std::vector<std::string> FindLogs(int argc, char **argv) {
std::vector<std::string> found_logfiles;
diff --git a/aos/events/logging/logfile_sorting.h b/aos/events/logging/logfile_sorting.h
index 8dee3da..964e592 100644
--- a/aos/events/logging/logfile_sorting.h
+++ b/aos/events/logging/logfile_sorting.h
@@ -96,6 +96,10 @@
// them to the vector.
void FindLogs(std::vector<std::string> *files, std::string filename);
+// Recursively searches the file/folder for .bfbs and .bfbs.xz files and returns
+// them in a vector.
+std::vector<std::string> FindLogs(std::string filename);
+
// Recursively searches for logfiles in argv[1] and onward.
std::vector<std::string> FindLogs(int argc, char **argv);
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 17078dc..d8ab66e 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -28,7 +28,7 @@
using aos::message_bridge::RemoteMessage;
using aos::testing::MessageCounter;
-constexpr std::string_view kSingleConfigSha1(
+constexpr std::string_view kSingleConfigSha256(
"bc8c9c2e31589eae6f0e36d766f6a437643e861d9568b7483106841cf7504dea");
std::vector<std::vector<std::string>> ToLogReaderVector(
@@ -78,7 +78,7 @@
const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name = tmpdir + "/logfile";
const ::std::string config =
- absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
const ::std::string logfile = base_name + ".part0.bfbs";
// Remove it.
unlink(config.c_str());
@@ -142,11 +142,11 @@
const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name1 = tmpdir + "/logfile1";
const ::std::string config1 =
- absl::StrCat(base_name1, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name1, kSingleConfigSha256, ".bfbs");
const ::std::string logfile1 = base_name1 + ".part0.bfbs";
const ::std::string base_name2 = tmpdir + "/logfile2";
const ::std::string config2 =
- absl::StrCat(base_name2, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name2, kSingleConfigSha256, ".bfbs");
const ::std::string logfile2 = base_name2 + ".part0.bfbs";
unlink(logfile1.c_str());
unlink(config1.c_str());
@@ -180,7 +180,7 @@
const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name = tmpdir + "/logfile";
const ::std::string config =
- absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
const ::std::string logfile = base_name + ".part0.bfbs";
// Remove it.
unlink(config.c_str());
@@ -213,11 +213,11 @@
const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name1 = tmpdir + "/logfile1";
const ::std::string config1 =
- absl::StrCat(base_name1, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name1, kSingleConfigSha256, ".bfbs");
const ::std::string logfile1 = base_name1 + ".part0.bfbs";
const ::std::string base_name2 = tmpdir + "/logfile2";
const ::std::string config2 =
- absl::StrCat(base_name2, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name2, kSingleConfigSha256, ".bfbs");
const ::std::string logfile2 = base_name2 + ".part0.bfbs";
unlink(logfile1.c_str());
unlink(config1.c_str());
@@ -283,7 +283,7 @@
const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name = tmpdir + "/logfile";
const ::std::string config =
- absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
const ::std::string logfile0 = base_name + ".part0.bfbs";
const ::std::string logfile1 = base_name + ".part1.bfbs";
// Remove it.
@@ -368,7 +368,7 @@
const ::std::string tmpdir = aos::testing::TestTmpDir();
const ::std::string base_name = tmpdir + "/logfile";
const ::std::string config =
- absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+ absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
const ::std::string logfile = base_name + ".part0.bfbs";
// Remove the log file.
unlink(config.c_str());
diff --git a/aos/events/logging/timestamp_plot.gnuplot b/aos/events/logging/timestamp_plot.gnuplot
index 7ad62da..94ef7e4 100755
--- a/aos/events/logging/timestamp_plot.gnuplot
+++ b/aos/events/logging/timestamp_plot.gnuplot
@@ -23,6 +23,10 @@
offsetfile = "/tmp/timestamp_noncausal_offsets.csv"
#set term qt 0
+if (ARG3 ne "" ) {
+ set term png
+ set output ARG3
+}
plot samplefile12 using 1:2 title 'sample 1-2', \
samplefile21 using 1:(-$2) title 'sample 2-1', \
@@ -30,4 +34,8 @@
noncausalfile21 using 1:(-$3) title 'nc 2-1' with lines, \
offsetfile using ((column(node1_index) - node1_start_time + (column(node2_index) - node2_start_time)) / 2):(column(node2_index) - column(node1_index)) title 'filter 2-1' with linespoints
+if (ARG3 ne "" ) {
+ exit
+}
+
pause -1
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 9d431b7..8474bb8 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -154,9 +154,9 @@
}
void FreeBufferIndex(int i) {
- // This extra checking has a large performance hit with msan, so just skip
- // it.
-#if !__has_feature(memory_sanitizer)
+ // This extra checking has a large performance hit with sanitizers that
+ // track memory accesses, so just skip it.
+#if !__has_feature(memory_sanitizer) && !__has_feature(address_sanitizer)
DCHECK(std::find(available_buffer_indices_.begin(),
available_buffer_indices_.end(),
i) == available_buffer_indices_.end())
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index 052fa9c..1f475be 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -9,6 +9,7 @@
#include "aos/fast_string_builder.h"
#include "aos/flatbuffer_utils.h"
#include "aos/flatbuffers.h"
+#include "aos/util/file.h"
#include "flatbuffers/flatbuffers.h"
#include "flatbuffers/reflection.h"
@@ -111,13 +112,10 @@
// Parses a file as a binary flatbuffer or dies.
template <typename T>
inline FlatbufferVector<T> FileToFlatbuffer(const std::string_view path) {
- std::ifstream instream(std::string(path), std::ios::in | std::ios::binary);
+ const std::string data_string = util::ReadFileToStringOrDie(path);
ResizeableBuffer data;
- std::istreambuf_iterator<char> it(instream);
- while (it != std::istreambuf_iterator<char>()) {
- data.push_back(*it);
- ++it;
- }
+ data.resize(data_string.size());
+ memcpy(data.data(), data_string.data(), data_string.size());
return FlatbufferVector<T>(std::move(data));
}
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index 9dc12d2..aa259df 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -79,10 +79,20 @@
EXPECT_TRUE(JsonAndBack("{ \"foo_enum_nonconsecutive\": \"Big\" }"));
}
+// Tests that Inf is handled correctly
+TEST_F(JsonToFlatbufferTest, Inf) {
+ EXPECT_TRUE(JsonAndBack("{ \"foo_float\": inf }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_float\": -inf }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_double\": inf }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_double\": -inf }"));
+}
+
// Tests that NaN is handled correctly
TEST_F(JsonToFlatbufferTest, Nan) {
EXPECT_TRUE(JsonAndBack("{ \"foo_float\": nan }"));
EXPECT_TRUE(JsonAndBack("{ \"foo_float\": -nan }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_double\": nan }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_double\": -nan }"));
}
// Tests that we can handle decimal points.
diff --git a/aos/json_tokenizer.cc b/aos/json_tokenizer.cc
index 9403daa..47fcdbe 100644
--- a/aos/json_tokenizer.cc
+++ b/aos/json_tokenizer.cc
@@ -152,6 +152,12 @@
return true;
}
+ // Inf is also acceptable.
+ if (Consume("inf")) {
+ *s = ::std::string(original.substr(0, original.size() - data_.size()));
+ return true;
+ }
+
// Then, we either get a 0, or we get a nonzero. Only nonzero can be followed
// by a second number.
if (!Consume("0")) {
@@ -463,6 +469,14 @@
return true;
}
+ if (field_value() == "inf") {
+ *value = std::numeric_limits<double>::infinity();
+ return true;
+ } else if (field_value() == "-inf") {
+ *value = -std::numeric_limits<double>::infinity();
+ return true;
+ }
+
*value = strtod(field_value().c_str(), const_cast<char **>(&pos));
if (pos != field_value().c_str() + field_value().size() || errno != 0) {
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 4b6a44b..292d24d 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -17,14 +17,17 @@
"of CSV files in /tmp/. This should only be needed when debugging "
"time synchronization.");
-DEFINE_int32(max_invalid_distance_ns, 500,
+DEFINE_int32(max_invalid_distance_ns, 0,
"The max amount of time we will let the solver go backwards.");
namespace aos {
namespace message_bridge {
namespace {
namespace chrono = std::chrono;
-}
+
+const Eigen::IOFormat kHeavyFormat(Eigen::StreamPrecision, Eigen::DontAlignCols,
+ ", ", ";\n", "[", "]", "[", "]");
+} // namespace
TimestampProblem::TimestampProblem(size_t count) {
CHECK_GT(count, 1u);
@@ -34,13 +37,15 @@
node_mapping_.resize(count, 0);
}
-// TODO(austin): Add linear inequality constraints too.
+// TODO(austin): Add linear inequality constraints too. Currently we just
+// enforce them.
//
// TODO(austin): Add a rate of change constraint from the last sample. 1
// ms/s. Figure out how to define it. Do this last. This lets us handle
// constraints going away, and constraints close in time.
//
-// TODO(austin): Use the timestamp of the remote timestamp as more data.
+// TODO(austin): When the new newton's method solver prooves it's worth, kill
+// the old SLSQP solver. It will be unreachable for a little bit.
std::vector<double> TimestampProblem::SolveDouble() {
MaybeUpdateNodeMapping();
@@ -146,6 +151,239 @@
return success;
}
+Eigen::VectorXd TimestampProblem::Gradient(
+ const Eigen::Ref<Eigen::VectorXd> time_offsets) const {
+ Eigen::VectorXd grad = Eigen::VectorXd::Zero(live_nodes_);
+ for (size_t i = 0; i < filters_.size(); ++i) {
+ for (const struct FilterPair &filter : filters_[i]) {
+ // Reminder, our cost function has the following form.
+ // ((tb - (1 + ma) ta - ba)^2
+ // We are ignoring the slope when taking the derivative and applying the
+ // chain rule to keep the gradient smooth. This means that the gradient
+ // is +- 2 * error.
+ //
+ const size_t a_solution_index = NodeToFullSolutionIndex(i);
+ const size_t b_solution_index = NodeToFullSolutionIndex(filter.b_index);
+ const double error =
+ 2.0 * filter.filter->OffsetError(base_clock_[i],
+ time_offsets(a_solution_index),
+ base_clock_[filter.b_index],
+ time_offsets(b_solution_index));
+
+ grad(a_solution_index) += -error;
+ grad(b_solution_index) += error;
+ }
+ }
+ return grad;
+}
+
+Eigen::MatrixXd TimestampProblem::Hessian(
+ const Eigen::Ref<Eigen::VectorXd> /*time_offsets*/) const {
+ Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(live_nodes_, live_nodes_);
+
+ for (size_t i = 0; i < filters_.size(); ++i) {
+ for (const struct FilterPair &filter : filters_[i]) {
+ // Reminder, our cost function has the following form.
+ // ((tb - (1 + ma) ta - ba)^2
+ // We are ignoring the slope when taking the derivative and applying the
+ // chain rule to keep the gradient smooth. This means that the Hessian is
+ // 2 for d^2 cost/dta^2 and d^2 cost/dtb^2
+ const size_t a_solution_index = NodeToFullSolutionIndex(i);
+ const size_t b_solution_index = NodeToFullSolutionIndex(filter.b_index);
+ hessian(a_solution_index, a_solution_index) += 2;
+ hessian(b_solution_index, a_solution_index) += -2;
+ hessian(a_solution_index, b_solution_index) =
+ hessian(b_solution_index, a_solution_index);
+ hessian(b_solution_index, b_solution_index) += 2;
+ }
+ }
+
+ return hessian;
+}
+
+Eigen::VectorXd TimestampProblem::Newton(
+ const Eigen::Ref<Eigen::VectorXd> time_offsets) const {
+ CHECK_GT(live_nodes_, 0u) << ": No live nodes to solve for.";
+ // TODO(austin): Each of the DCost functions does a binary search of the
+ // timestamps list. By the time we have computed the gradient and Hessian,
+ // we've done 5 binary searches for the same information.
+ const Eigen::VectorXd grad = Gradient(time_offsets);
+ const Eigen::MatrixXd hessian = Hessian(time_offsets);
+ const Eigen::MatrixXd constraint_jacobian =
+ Eigen::MatrixXd::Ones(1, live_nodes_) / static_cast<double>(live_nodes_);
+ // https://www.cs.purdue.edu/homes/jhonorio/16spring-cs52000-equality.pdf
+ //
+ // Queue long explanation for why this is the right math...
+ //
+ // Our cost function is piecewise quadratic and simple by design. It should
+ // also be convex. This means it is equivalent for us to drive the gradient
+ // to 0 to find the corresponding times on all nodes.
+ //
+ // This gets us close but doesn't let us solve for the time corresponding to a
+ // specific time on one node on all the nodes.
+ //
+ // To do this, we want a Newton solver which works for equality constraints.
+ // argmin f(x) subject to {A x = b}
+ // More specifically, we want a version of this which will start with
+ // infeasible initial solutions.
+ //
+ // The newton step for this is
+ // X(n+1) = X(n) + xnt
+ //
+ // [Hessian(cost(X(n))) A^T] [xnt] = [-grad(cost(X(n)))]
+ // [A 0] [w] [-A X(n) + b ]
+ //
+ // It turns out that w is the dual newton step. But we don't actually need to
+ // track the dual problem to solve our problem except to show that the dual
+ // problem has also converged.
+ //
+ // We could set A to [1, 0, 0, ...] and force a single clock to a specific
+ // time. But, that will result in the optimal solution being different
+ // depending on which node is picked.
+ //
+ // Instead, let's solve for the average clock instead. This will be always
+ // symmetric, and we can drive the goal for that clock to make any individual
+ // clock have the right time. That would be a solver wrapped around a solver.
+ //
+ // Turns out, we can do that by combining the iterations. If we set A to
+ // [1/n, 1/n, 1/n ...], and b to the distributed clock, that would drive our
+ // states such that the distributed clock will be what we want. If we instead
+ // leave A the same, but set (-A X(n) + b) to be ( - [1, 0, 0...] * X(n) +
+ // goal_clock), we will drive the distributed clock to be what it needs to be
+ // to set a node's clock to the right time.
+ //
+ // This ends up working surprisingly well. A toy problem with 2 line segments
+ // and 2 nodes converges in 2 iterations.
+ //
+ // TODO(austin): Maybe drive the distributed so we drive the min clock? This
+ // will solve the for loop at the same time, making things faster.
+ //
+ //
+ // To ensure reliable convergence, we want to make 1 adjustment to the above
+ // problem statement.
+ //
+ // d cost/dta =>
+ // 2 * (tb - (1 + ma) ta - ba) * (-(1 + ma))
+ //
+ // This means that as you move between line segments with different slopes,
+ // you end up with step changes in the gradient. Solvers like continuous
+ // derivatives. But, we don't really care if this is an exact solution to the
+ // cost problem. We just care that it is close to a solution to the cost
+ // problem and more importantly well behaved.
+ //
+ // The simple fix is to ignore the slope when applying the chain rule. This
+ // makes the derivative just be the distance to the line, which is a
+ // continuous function. Newtons method then converges really really easily
+ // every time.
+
+ Eigen::MatrixXd a;
+ a.resize(live_nodes_ + 1, live_nodes_ + 1);
+ a.block(0, 0, live_nodes_, live_nodes_) = hessian;
+ a.block(0, live_nodes_, live_nodes_, 1) = constraint_jacobian.transpose();
+ a.block(live_nodes_, 0, 1, live_nodes_) = constraint_jacobian;
+ a(live_nodes_, live_nodes_) = 0.0;
+
+ Eigen::VectorXd b = Eigen::VectorXd::Zero(live_nodes_ + 1);
+ b.block(0, 0, live_nodes_, 1) = -grad;
+
+ // Since we are driving the clock on the solution node to the base_clock, that
+ // is equivalent to driving the solution node's offset to 0.
+ b(live_nodes_) = -time_offsets(NodeToFullSolutionIndex(solution_node_));
+
+ return a.colPivHouseholderQr().solve(b);
+}
+
+std::vector<monotonic_clock::time_point> TimestampProblem::SolveNewton() {
+ constexpr int kMaxIterations = 200;
+ MaybeUpdateNodeMapping();
+ VLOG(1) << "Solving for node " << solution_node_ << " at "
+ << base_clock(solution_node_);
+ Eigen::VectorXd data = Eigen::VectorXd::Zero(live_nodes_);
+
+ int solution_number = 0;
+ while (true) {
+ Eigen::VectorXd step = Newton(data);
+
+ if (VLOG_IS_ON(1)) {
+ // Print out the gradient ignoring the component removed by the equality
+ // constraint. This tells us what gradient we are depending to try to
+ // finish our solution.
+ const Eigen::MatrixXd constraint_jacobian =
+ Eigen::MatrixXd::Ones(1, live_nodes_) /
+ static_cast<double>(live_nodes_);
+ Eigen::VectorXd adjusted_grad =
+ Gradient(data) + step(live_nodes_) * constraint_jacobian.transpose();
+
+ VLOG(1) << "Adjusted grad " << solution_number << " -> "
+ << std::setprecision(12) << std::fixed << std::setfill(' ')
+ << adjusted_grad.transpose().format(kHeavyFormat);
+ }
+
+ VLOG(1) << "Step " << solution_number << " -> " << std::setprecision(12)
+ << std::fixed << std::setfill(' ')
+ << step.transpose().format(kHeavyFormat);
+ // We got there if the max step is small (this is strongly correlated to the
+ // gradient since the Hessian is constant), and our solution node's time is
+ // also close.
+ if (step.block(0, 0, live_nodes_, 1).lpNorm<Eigen::Infinity>() < 1e-4 &&
+ std::abs(data(NodeToFullSolutionIndex(solution_node_))) < 1e-4) {
+ break;
+ }
+
+ data += step.block(0, 0, live_nodes_, 1);
+
+ ++solution_number;
+
+ // We are doing all our math with both an int64 base and a double offset.
+ // This lets us handle large offsets while retaining precision down to the
+ // nanosecond easily.
+ //
+ // Some problems start out with a poor initial solution. This is especially
+ // true for the first solution. Because we control the solver, as we
+ // determine that the double is getting too big, we can move that
+ // information to the int64 base clock. Threshold this to not be *too* big
+ // since it makes it hard to debug as the data keeps jumping around.
+ for (size_t j = 0; j < size(); ++j) {
+ const size_t solution_index = NodeToFullSolutionIndex(j);
+ if (j != solution_node_ && live(j) &&
+ std::abs(data(solution_index)) > 1000) {
+ int64_t dsolution =
+ static_cast<int64_t>(std::round(data(solution_index)));
+ base_clock_[j] += chrono::nanoseconds(dsolution);
+ data(solution_index) -= dsolution;
+ }
+ }
+
+ // And finally, don't let us iterate forever. If it isn't converging,
+ // report back.
+ if (solution_number > kMaxIterations) {
+ break;
+ }
+ }
+
+ VLOG(1) << "Solving for node " << solution_node_ << " of "
+ << base_clock(solution_node_) << " in " << solution_number
+ << " cycles";
+ std::vector<monotonic_clock::time_point> result(size());
+ for (size_t i = 0; i < size(); ++i) {
+ if (live(i)) {
+ result[i] =
+ base_clock(i) + std::chrono::nanoseconds(static_cast<int64_t>(
+ std::round(data(NodeToFullSolutionIndex(i)))));
+ VLOG(1) << "live " << result[i] << " "
+ << data(NodeToFullSolutionIndex(i));
+ } else {
+ result[i] = monotonic_clock::min_time;
+ VLOG(1) << "dead " << result[i];
+ }
+ }
+ if (solution_number > kMaxIterations) {
+ LOG(FATAL) << "Failed to converge.";
+ }
+
+ return result;
+}
+
double TimestampProblem::Cost(const double *time_offsets, double *grad) {
++cost_call_count_;
@@ -998,7 +1236,7 @@
}
std::tuple<NoncausalTimestampFilter *,
- std::vector<aos::monotonic_clock::time_point>>
+ std::vector<aos::monotonic_clock::time_point>, int>
MultiNodeNoncausalOffsetEstimator::NextSolution(
TimestampProblem *problem,
const std::vector<aos::monotonic_clock::time_point> &base_times) {
@@ -1047,11 +1285,12 @@
problem->set_solution_node(node_a_index);
problem->set_base_clock(problem->solution_node(), next_node_time);
- if (VLOG_IS_ON(1)) {
+ if (VLOG_IS_ON(2)) {
problem->Debug();
}
// TODO(austin): Can we cache? Solving is expensive.
- std::vector<monotonic_clock::time_point> solution = problem->Solve();
+ std::vector<monotonic_clock::time_point> solution =
+ problem->SolveNewton();
// Bypass checking if order validation is turned off. This lets us dump a
// CSV file so we can view the problem and figure out what to do. The
@@ -1122,39 +1361,6 @@
<< " -> " << (result_times[i] - solution[i]).count()
<< "ns";
}
- // Since we found a problem with the solution, solve one problem per
- // node, starting at the problem point. This will show us any
- // inconsistencies due to the problem phrasing and which node we
- // solved from.
- for (size_t a_index = 0; a_index < solution.size(); ++a_index) {
- if (!problem->live(a_index)) {
- continue;
- }
- for (size_t node_index = 0; node_index < solution.size();
- ++node_index) {
- // Offset everything based on the elapsed time since the last
- // solution on the node we are solving for. The rate that time
- // elapses should be ~1.
- problem->set_base_clock(node_index, solution[node_index]);
- }
-
- problem->set_solution_node(a_index);
- problem->Debug();
- const std::vector<double> resolve_solution_double =
- problem->SolveDouble();
- problem->PrintSolution(resolve_solution_double);
-
- const std::vector<monotonic_clock::time_point> resolve_solution =
- problem->DoubleToMonotonic(resolve_solution_double.data());
-
- LOG(INFO) << "Candidate solution for resolved node " << a_index
- << " is";
- for (size_t i = 0; i < resolve_solution.size(); ++i) {
- LOG(INFO) << " " << resolve_solution[i] << " vs original "
- << solution[i] << " -> "
- << (resolve_solution[i] - solution[i]).count();
- }
- }
if (skip_order_validation_) {
next_node_filter->Consume();
@@ -1175,7 +1381,7 @@
VLOG(1) << " " << result_times[i];
}
}
- return std::make_tuple(next_filter, std::move(result_times));
+ return std::make_tuple(next_filter, std::move(result_times), solution_index);
}
std::optional<std::tuple<distributed_clock::time_point,
@@ -1188,7 +1394,8 @@
// Ok, now solve for the minimum time on each channel.
std::vector<aos::monotonic_clock::time_point> result_times;
NoncausalTimestampFilter *next_filter = nullptr;
- std::tie(next_filter, result_times) =
+ int solution_node_index = 0;
+ std::tie(next_filter, result_times, solution_node_index) =
NextSolution(&problem, last_monotonics_);
CHECK(!all_done_);
@@ -1235,13 +1442,16 @@
if (first_solution_) {
std::vector<aos::monotonic_clock::time_point> resolved_times;
NoncausalTimestampFilter *resolved_next_filter = nullptr;
+ int resolved_solution_node_index = 0;
VLOG(1) << "Resolving with updated base times for accuracy.";
- std::tie(resolved_next_filter, resolved_times) =
+ std::tie(resolved_next_filter, resolved_times,
+ resolved_solution_node_index) =
NextSolution(&problem, result_times);
first_solution_ = false;
next_filter = resolved_next_filter;
+ solution_node_index = resolved_solution_node_index;
// Force any unknown nodes to track the distributed clock (which starts at 0
// too).
@@ -1265,23 +1475,34 @@
case TimeComparison::kAfter:
problem.Debug();
for (size_t i = 0; i < result_times.size(); ++i) {
- LOG(INFO) << " " << last_monotonics_[i] << " vs " << result_times[i];
+ LOG(INFO) << " " << last_monotonics_[i] << " vs " << result_times[i]
+ << " -> " << (last_monotonics_[i] - result_times[i]).count()
+ << "ns";
}
- LOG(FATAL) << "Found a solution before the last returned solution.";
+ LOG(FATAL)
+ << "Found a solution before the last returned solution on node "
+ << solution_node_index;
break;
case TimeComparison::kEq:
return NextTimestamp();
- case TimeComparison::kInvalid:
- if (InvalidDistance(last_monotonics_, result_times) <
+ case TimeComparison::kInvalid: {
+ const chrono::nanoseconds invalid_distance =
+ InvalidDistance(last_monotonics_, result_times);
+ if (invalid_distance <
chrono::nanoseconds(FLAGS_max_invalid_distance_ns)) {
return NextTimestamp();
}
+ LOG(INFO) << "Times can't be compared by " << invalid_distance.count()
+ << "ns";
CHECK_EQ(last_monotonics_.size(), result_times.size());
for (size_t i = 0; i < result_times.size(); ++i) {
- LOG(INFO) << " " << last_monotonics_[i] << " vs " << result_times[i];
+ LOG(INFO) << " " << last_monotonics_[i] << " vs " << result_times[i]
+ << " -> " << (last_monotonics_[i] - result_times[i]).count()
+ << "ns";
}
- LOG(FATAL) << "Found solutions which can't be ordered.";
- break;
+ LOG(FATAL) << "Please investigate. Use --max_invalid_distance_ns="
+ << invalid_distance.count() << " to ignore this.";
+ } break;
}
}
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 0c07850..338a515 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -64,6 +64,10 @@
// each node.
std::vector<monotonic_clock::time_point> Solve();
+ // Solves the optimization problem phrased using the symmetric Netwon's method
+ // solver and returns the optimal time on each node.
+ std::vector<monotonic_clock::time_point> SolveNewton();
+
// Returns the squared error for all of the offsets.
// time_offsets is the offsets from the base_clock for every node (in order)
// except the solution node. It should be one element shorter than the number
@@ -126,6 +130,17 @@
return reinterpret_cast<TimestampProblem *>(data)->Cost(time_offsets, grad);
}
+ // Returns the Hessian of the cost function at time_offsets.
+ Eigen::MatrixXd Hessian(const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+ // Returns the gradient of the cost function at time_offsets.
+ Eigen::VectorXd Gradient(
+ const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+
+ // Returns the newton step of the timestamp problem. The last term is the
+ // scalar on the equality constraint. This needs to be removed from the
+ // solution to get the actual newton step.
+ Eigen::VectorXd Newton(const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+
void MaybeUpdateNodeMapping() {
if (node_mapping_valid_) {
return;
@@ -139,21 +154,28 @@
node_mapping_[i] = std::numeric_limits<size_t>::max();
}
}
+ live_nodes_ = live_node_index;
node_mapping_valid_ = true;
}
// Converts from a node index to an index in the solution.
size_t NodeToSolutionIndex(size_t node_index) const {
- CHECK(node_mapping_valid_);
CHECK_NE(node_index, solution_node_);
// The solver is going to provide us a matrix with solution_node_ removed.
// The indices of all nodes before solution_node_ are in the same spot, and
// the indices of the nodes after solution node are shifted over.
- size_t mapped_node_index = node_mapping_[node_index];
+ size_t mapped_node_index = NodeToFullSolutionIndex(node_index);
return node_index < solution_node_ ? mapped_node_index
: (mapped_node_index - 1);
}
+ // Converts from a node index to an index in the solution without skipping the
+ // solution node.
+ size_t NodeToFullSolutionIndex(size_t node_index) const {
+ CHECK(node_mapping_valid_);
+ return node_mapping_[node_index];
+ }
+
// Number of times Cost has been called for tracking.
int cost_call_count_ = 0;
@@ -166,8 +188,12 @@
std::vector<monotonic_clock::time_point> base_clock_;
std::vector<bool> live_;
+ // True if both node_mapping_ and live_nodes_ are valid.
bool node_mapping_valid_ = false;
+ // Mapping from a node index to an index in the solution.
std::vector<size_t> node_mapping_;
+ // The number of live nodes there are.
+ size_t live_nodes_ = 0;
// Filter and the node index it is referencing.
// filter->Offset(ta) + ta => t_(b_node);
@@ -345,7 +371,7 @@
TimestampProblem MakeProblem();
std::tuple<NoncausalTimestampFilter *,
- std::vector<aos::monotonic_clock::time_point>>
+ std::vector<aos::monotonic_clock::time_point>, int>
NextSolution(TimestampProblem *problem,
const std::vector<aos::monotonic_clock::time_point> &base_times);
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 54ce35f..1d13601 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -481,6 +481,63 @@
EXPECT_TRUE(time_converter.NextTimestamp());
}
+// Tests that our Newtons method solver returns consistent answers for a simple
+// problem or two. Also confirm that the residual error to the constraints
+// looks sane, meaning it is centered.
+TEST(TimestampProblemTest, SolveNewton) {
+ FlatbufferDetachedBuffer<Node> node_a_buffer =
+ JsonToFlatbuffer<Node>("{\"name\": \"test_a\"}");
+ const Node *const node_a = &node_a_buffer.message();
+
+ FlatbufferDetachedBuffer<Node> node_b_buffer =
+ JsonToFlatbuffer<Node>("{\"name\": \"test_b\"}");
+ const Node *const node_b = &node_b_buffer.message();
+
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+ const monotonic_clock::time_point ta = e + chrono::milliseconds(500);
+
+ // Setup a time problem with an interesting shape that isn't simple and
+ // parallel.
+ NoncausalTimestampFilter a(node_a, node_b);
+ a.Sample(e, chrono::milliseconds(1002));
+ a.Sample(e + chrono::milliseconds(1000), chrono::milliseconds(1001));
+ a.Sample(e + chrono::milliseconds(3000), chrono::milliseconds(999));
+
+ NoncausalTimestampFilter b(node_b, node_a);
+ b.Sample(e + chrono::milliseconds(1000), -chrono::milliseconds(999));
+ b.Sample(e + chrono::milliseconds(2000), -chrono::milliseconds(1000));
+ b.Sample(e + chrono::milliseconds(4000), -chrono::milliseconds(1002));
+
+ TimestampProblem problem(2);
+ problem.set_base_clock(0, ta);
+ problem.set_base_clock(1, e);
+ problem.set_solution_node(0);
+ problem.add_filter(0, &a, 1);
+ problem.add_filter(1, &b, 0);
+
+ problem.Debug();
+
+ problem.set_base_clock(0, e + chrono::seconds(1));
+ problem.set_base_clock(1, e);
+
+ problem.set_solution_node(0);
+ std::vector<monotonic_clock::time_point> result1 = problem.SolveNewton();
+
+ problem.set_base_clock(1, result1[1]);
+ problem.set_solution_node(1);
+ std::vector<monotonic_clock::time_point> result2 = problem.SolveNewton();
+
+ EXPECT_EQ(result1[0], e + chrono::seconds(1));
+ EXPECT_EQ(result1[0], result2[0]);
+ EXPECT_EQ(result1[1], result2[1]);
+
+ // Confirm that the error is almost equal for both directions. The solution
+ // is an integer solution, so there will be a little bit of error left over.
+ EXPECT_NEAR(a.OffsetError(result1[0], 0.0, result1[1], 0.0) -
+ b.OffsetError(result1[1], 0.0, result1[0], 0.0),
+ 0.0, 0.5);
+}
+
} // namespace testing
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/timestamp_channel.cc b/aos/network/timestamp_channel.cc
index d022b60..bc8c1d9 100644
--- a/aos/network/timestamp_channel.cc
+++ b/aos/network/timestamp_channel.cc
@@ -42,7 +42,7 @@
RemoteMessage::GetFullyQualifiedName(), name_, node_, true);
if (shared_timestamp_channel != nullptr) {
LOG(WARNING) << "Failed to find timestamp channel {\"name\": \""
- << split_timestamp_channel << "\", \"type\": \""
+ << split_timestamp_channel_name << "\", \"type\": \""
<< RemoteMessage::GetFullyQualifiedName()
<< "\"}, falling back to old version.";
return shared_timestamp_channel;
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 498f111..dd77f40 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -49,6 +49,18 @@
*ta_base += ta_digits;
*ta -= static_cast<double>(ta_digits.count());
+ // Sign, numerical precision wins again.
+ // *ta_base=1000.300249970sec, *ta=-1.35525e-20
+ // We then promptly round this to
+ // *ta_base=1000.300249969sec, *ta=1
+ // The 1.0 then breaks the LT assumption below, so we kersplat.
+ //
+ // Detect this case directly and move the 1.0 back into ta_base.
+ if (*ta == 1.0) {
+ *ta = 0.0;
+ *ta_base += chrono::nanoseconds(1);
+ }
+
CHECK_GE(*ta, 0.0);
CHECK_LT(*ta, 1.0);
}
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index f8955c6..f10746f 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -75,6 +75,12 @@
value = self.vol_input.get_value()
self.drawing_area.points.setConstraint("VOLTAGE", value)
+ def input_combobox_choice(self, combo):
+ text = combo.get_active_text()
+ if text is not None:
+ print("Combo Clicked on: " + text)
+ set_field(text)
+
def __init__(self):
Gtk.Window.__init__(self)
@@ -187,8 +193,37 @@
container.put(self.output_json, 210, 0)
container.put(self.input_json, 320, 0)
- self.show_all()
+ #Dropdown feature
+ self.label = Gtk.Label("Change Map:")
+ self.label.set_size_request(100,40)
+ container.put(self.label,430,0)
+
+ game_store = Gtk.ListStore(str)
+ games = [
+ "2020 Field",
+ "2019 Field",
+ "2021 Galactic Search ARed",
+ "2021 Galactic Search ABlue",
+ "2021 Galactic Search BRed",
+ "2021 Galactic Search BBlue",
+ "2021 AutoNav Barrel Racing",
+ "2021 AutoNav Slalom",
+ "2021 AutoNav Bounce",
+ ]
+
+ self.game_combo = Gtk.ComboBoxText()
+ self.game_combo.set_entry_text_column(0)
+ self.game_combo.connect("changed", self.input_combobox_choice)
+
+ for game in games:
+ self.game_combo.append_text(game)
+
+ self.game_combo.set_active(0)
+ self.game_combo.set_size_request(100,40)
+ container.put(self.game_combo,440,30)
+
+ self.show_all()
window = GridWindow()
RunApp()
diff --git a/y2020/BUILD b/y2020/BUILD
index cd001a1..024b6ac 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -152,6 +152,7 @@
":config_pi2",
":config_pi3",
":config_pi4",
+ ":config_pi5",
":config_roborio",
],
flatbuffers = [
@@ -191,6 +192,7 @@
"pi2",
"pi3",
"pi4",
+ "pi5",
]
]
@@ -289,5 +291,5 @@
parameters = {"NUM": str(num)},
target_compatible_with = ["@platforms//os:linux"],
)
- for num in range(1, 5)
+ for num in range(1, 6)
]
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 1b98592..75a8475 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -117,16 +117,6 @@
],
)
-aos_config(
- name = "replay_config",
- src = "drivetrain_replay_config.json",
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//y2020:config",
- ],
-)
-
cc_test(
name = "localizer_test",
srcs = ["localizer_test.cc"],
@@ -150,8 +140,8 @@
name = "drivetrain_replay_test",
srcs = ["drivetrain_replay_test.cc"],
data = [
- ":replay_config",
- "@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
+ "//y2020:config",
+ "@drivetrain_replay",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_config.json b/y2020/control_loops/drivetrain/drivetrain_replay_config.json
deleted file mode 100644
index 987e55b..0000000
--- a/y2020/control_loops/drivetrain/drivetrain_replay_config.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
- "channels": [
- {
- "name": "/drivetrain",
- "type": "frc971.IMUValues",
- "frequency": 2000,
- "source_node": "roborio"
- }
- ],
- "imports": [
- "../../y2020.json"
- ]
-}
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 2e6b327..e6a3f6e 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -22,9 +22,10 @@
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_string(
- logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
+ logfile,
+ "external/drivetrain_replay/",
"Name of the logfile to read from.");
-DEFINE_string(config, "y2020/control_loops/drivetrain/replay_config.json",
+DEFINE_string(config, "y2020/config.json",
"Name of the config file to replay using.");
namespace y2020 {
@@ -36,7 +37,8 @@
public:
DrivetrainReplayTest()
: config_(aos::configuration::ReadConfig(FLAGS_config)),
- reader_(FLAGS_logfile, &config_.message()) {
+ reader_(aos::logger::SortParts(aos::logger::FindLogs(FLAGS_logfile)),
+ &config_.message()) {
aos::network::OverrideTeamNumber(971);
// TODO(james): Actually enforce not sending on the same buses as the
@@ -55,10 +57,6 @@
frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
GetDrivetrainConfig();
- // Make the modification required to the imu transform to work with the 2016
- // logs...
- config.imu_transform << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
- config.gyro_type = frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
localizer_ =
std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
@@ -70,25 +68,6 @@
test_event_loop_ =
reader_.event_loop_factory()->MakeEventLoop("drivetrain_test", roborio_);
- // IMU readings used to be published out one at a time, but we now expect
- // batches. Batch them up to upgrade the data.
- imu_sender_ =
- test_event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain");
- test_event_loop_->MakeWatcher(
- "/drivetrain", [this](const frc971::IMUValues &values) {
- aos::Sender<frc971::IMUValuesBatch>::Builder builder =
- imu_sender_.MakeBuilder();
- flatbuffers::Offset<frc971::IMUValues> values_offsets =
- aos::CopyFlatBuffer(&values, builder.fbb());
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
- values_offset = builder.fbb()->CreateVector(&values_offsets, 1);
- frc971::IMUValuesBatch::Builder imu_values_batch_builder =
- builder.MakeBuilder<frc971::IMUValuesBatch>();
- imu_values_batch_builder.add_readings(values_offset);
- builder.Send(imu_values_batch_builder.Finish());
- });
-
status_fetcher_ = test_event_loop_->MakeFetcher<
frc971::control_loops::drivetrain::Status>("/drivetrain");
}
@@ -102,7 +81,6 @@
std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
drivetrain_;
std::unique_ptr<aos::EventLoop> test_event_loop_;
- aos::Sender<frc971::IMUValuesBatch> imu_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
};
@@ -116,7 +94,10 @@
ASSERT_TRUE(status_fetcher_->has_x());
ASSERT_TRUE(status_fetcher_->has_y());
ASSERT_TRUE(status_fetcher_->has_theta());
- EXPECT_LT(std::abs(status_fetcher_->x()), 0.25);
+ EXPECT_NEAR(status_fetcher_->estimated_left_position(),
+ status_fetcher_->estimated_right_position(), 0.1);
+ EXPECT_LT(std::abs(status_fetcher_->x()),
+ std::abs(status_fetcher_->estimated_left_position()) / 2.0);
// Because the encoders should not be affecting the y or yaw axes, expect a
// reasonably precise result (although, since this is a real worl dtest, the
// robot probably did actually move be some non-zero amount).
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 3059bf1..32c8834 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -24,7 +24,7 @@
}
// Indices of the pis to use.
-const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+const std::array<std::string, 5> kPisToUse{"pi1", "pi2", "pi3", "pi4", "pi5"};
// Calculates the pose implied by the camera target, just based on
// distance/heading components.
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 5dbad02..c3b6464 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -65,7 +65,7 @@
right_encoder, 0, 0, 0, 0, 0, 0)
.finished(),
ekf_.P());
- };
+ }
private:
// Storage for a single turret position data point.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index d195b5d..a3e26b5 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -131,13 +131,14 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
event_loop_factory()->SetTimeConverter(&time_converter_);
- CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 5);
+ CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 6);
CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), pi1_), 1);
time_converter_.AddMonotonic({monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
+ monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch()});
set_team_id(frc971::control_loops::testing::kTeamNumber);
set_battery_voltage(12.0);
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 1cf54a2..14f6ac9 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -364,14 +364,24 @@
void set_feeder_falcon(
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
feeder_falcon_ = ::std::move(t);
- CHECK_EQ(ctre::phoenix::OKAY,
- feeder_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kFeederSupplyCurrentLimit(),
- Values::kFeederSupplyCurrentLimit(), 0}));
- CHECK_EQ(ctre::phoenix::OKAY,
- feeder_falcon_->ConfigStatorCurrentLimit(
- {true, Values::kFeederStatorCurrentLimit(),
- Values::kFeederStatorCurrentLimit(), 0}));
+ {
+ auto result = feeder_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kFeederSupplyCurrentLimit(),
+ Values::kFeederSupplyCurrentLimit(), 0});
+ if (result != ctre::phoenix::OKAY) {
+ LOG(WARNING) << "Failed to configure feeder supply current limit: "
+ << result;
+ }
+ }
+ {
+ auto result = feeder_falcon_->ConfigStatorCurrentLimit(
+ {true, Values::kFeederStatorCurrentLimit(),
+ Values::kFeederStatorCurrentLimit(), 0});
+ if (result != ctre::phoenix::OKAY) {
+ LOG(WARNING) << "Failed to configure feeder stator current limit: "
+ << result;
+ }
+ }
}
void set_washing_machine_control_panel_victor(
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 59f3fc0..70e51b0 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -17,6 +17,7 @@
"y2020_pi2.json",
"y2020_pi3.json",
"y2020_pi4.json",
+ "y2020_pi5.json",
"y2020_laptop.json"
]
}
diff --git a/y2020/y2020_laptop.json b/y2020/y2020_laptop.json
index 9ebac58..936f2cd 100644
--- a/y2020/y2020_laptop.json
+++ b/y2020/y2020_laptop.json
@@ -101,6 +101,20 @@
]
},
{
+ "name": "/pi5/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi5",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["laptop"],
+ "destination_nodes": [
+ {
+ "name": "laptop",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
"name": "/laptop/aos",
"type": "aos.timing.Report",
"source_node": "laptop",
@@ -119,7 +133,7 @@
"name": "/laptop/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "laptop",
- "frequency": 2,
+ "frequency": 10,
"num_senders": 2
},
{
@@ -166,6 +180,13 @@
"timestamp_logger_nodes": ["laptop"]
},
{
+ "name": "pi5",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["laptop"]
+ },
+ {
"name": "roborio",
"priority": 1,
"time_to_live": 5000000,
@@ -175,7 +196,7 @@
]
},
{
- "name": "/laptop/aos/remote_timestamps/roborio",
+ "name": "/laptop/aos/remote_timestamps/roborio/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -184,7 +205,7 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi1",
+ "name": "/laptop/aos/remote_timestamps/pi1/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -193,7 +214,7 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi2",
+ "name": "/laptop/aos/remote_timestamps/pi2/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -202,7 +223,7 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi3",
+ "name": "/laptop/aos/remote_timestamps/pi3/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -211,7 +232,16 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi4",
+ "name": "/laptop/aos/remote_timestamps/pi4/laptop/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "laptop",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/laptop/aos/remote_timestamps/pi5/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -330,6 +360,34 @@
"time_to_live": 5000000
}
]
+ },
+ {
+ "name": "/pi5/camera",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi5",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["laptop"],
+ "destination_nodes": [
+ {
+ "name": "laptop",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi5/camera",
+ "type": "frc971.vision.sift.ImageMatchResult",
+ "source_node": "pi5",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["laptop"],
+ "destination_nodes": [
+ {
+ "name": "laptop",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
}
],
"maps": [
@@ -367,6 +425,9 @@
},
{
"name": "pi4"
+ },
+ {
+ "name": "pi5"
}
]
}
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index 47d576a..61094be 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -34,7 +34,7 @@
"name": "/pi{{ NUM }}/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi{{ NUM }}",
- "frequency": 2,
+ "frequency": 10,
"num_senders": 2
},
{
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 2e29eee..2a204ae 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -46,7 +46,7 @@
"name": "/roborio/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "roborio",
- "frequency": 2,
+ "frequency": 10,
"num_senders": 2
},
{
@@ -57,28 +57,42 @@
"num_senders": 2
},
{
- "name": "/roborio/aos/remote_timestamps/pi1",
+ "name": "/roborio/aos/remote_timestamps/laptop/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
- "name": "/roborio/aos/remote_timestamps/pi2",
+ "name": "/roborio/aos/remote_timestamps/pi1/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
- "name": "/roborio/aos/remote_timestamps/pi3",
+ "name": "/roborio/aos/remote_timestamps/pi2/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
- "name": "/roborio/aos/remote_timestamps/pi4",
+ "name": "/roborio/aos/remote_timestamps/pi3/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
+ "logger": "NOT_LOGGED",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi4/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
+ "logger": "NOT_LOGGED",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi5/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
@@ -119,6 +133,13 @@
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": ["roborio"],
"time_to_live": 5000000
+ },
+ {
+ "name": "pi5",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["roborio"],
+ "time_to_live": 5000000
}
]
},
@@ -192,47 +213,17 @@
},
{
"name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Output",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Status",
"source_node": "roborio",
"frequency": 200,
"max_size": 2000,
- "num_senders": 2,
- "destination_nodes": [
- {
- "name": "pi1",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- },
- {
- "name": "pi2",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- },
- {
- "name": "pi3",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- },
- {
- "name": "pi4",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/drivetrain",
- "type": "frc971.control_loops.drivetrain.Output",
- "source_node": "roborio",
- "frequency": 200,
"num_senders": 2
},
{
@@ -352,6 +343,9 @@
},
{
"name": "pi4"
+ },
+ {
+ "name": "pi5"
}
]
}